diff --git a/CMakeLists.txt b/CMakeLists.txt index 433a2e4868..ffb81902d8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -675,6 +675,10 @@ VP_OPTION(WITH_QBDEVICE "" "" "Build qbdevice-api as built-in lib VP_OPTION(WITH_TAKKTILE2 "" "" "Build Right Hand takktile2 driver as built-in library" "" ON IF (VISP_CXX_STANDARD GREATER VISP_CXX_STANDARD_98) AND (NOT WIN32) AND (NOT WINRT) AND (NOT IOS) AND (NOT ANDROID)) VP_OPTION(WITH_CATCH2 "" "" "Use catch2" "" ON IF (VISP_CXX_STANDARD GREATER VISP_CXX_STANDARD_98)) VP_OPTION(WITH_POLOLU "" "" "Build rapa pololu as built-in library" "" ON IF (NOT WINRT) AND (NOT IOS) AND (NOT ANDROID)) +VP_OPTION(WITH_PUGIXML "" "" "Use pugixml built-in third-party" "" ON) +VP_OPTION(WITH_SIMDLIB "" "" "Use simdlib built-in third-party" "" ON) +VP_OPTION(WITH_STBIMAGE "" "" "Use std_image built-in third-party" "" ON) +VP_OPTION(WITH_TINYEXR "" "" "Use tinyexr built-in third-party" "" ON) # ---------------------------------------------------------------------------- # Check for specific functions. Should be after cxx standard detection in VISPDetectCXXStandard.cmake and potential modification depending on pcl, realsense2, libfranka @@ -998,6 +1002,10 @@ VP_SET(VISP_HAVE_QBDEVICE TRUE IF (BUILD_MODULE_visp_robot AND WITH_QBDEVICE) VP_SET(VISP_HAVE_TAKKTILE2 TRUE IF (BUILD_MODULE_visp_robot AND WITH_TAKKTILE2)) VP_SET(VISP_HAVE_POLOLU TRUE IF (BUILD_MODULE_visp_robot AND WITH_POLOLU)) VP_SET(VISP_HAVE_CATCH2 TRUE IF (BUILD_MODULE_visp_core AND WITH_CATCH2)) +VP_SET(VISP_HAVE_SIMDLIB TRUE IF (BUILD_MODULE_visp_core AND WITH_SIMDLIB)) +VP_SET(VISP_HAVE_STBIMAGE TRUE IF (BUILD_MODULE_visp_core AND WITH_STBIMAGE)) +VP_SET(VISP_HAVE_TINYEXR TRUE IF (BUILD_MODULE_visp_core AND WITH_TINYEXR)) +VP_SET(VISP_HAVE_PUGIXML TRUE IF (BUILD_MODULE_visp_core AND WITH_PUGIXML)) VP_SET(VISP_HAVE_QUALISYS TRUE IF (BUILD_MODULE_visp_sensor AND USE_QUALISYS)) VP_SET(VISP_HAVE_VICON TRUE IF (BUILD_MODULE_visp_sensor AND USE_VICON)) @@ -1325,6 +1333,7 @@ if(VISP_VCSVERSION) status(" Version control:" ${VISP_VCSVERSION}) endif() +# ========================== contrib modules ========================== if(VISP_CONTRIB_MODULES_PATH) set(__dump_extra_header OFF) foreach(p ${VISP_CONTRIB_MODULES_PATH}) @@ -1579,8 +1588,9 @@ status(" Use JPEG:" USE_JPEG THEN "yes (ver ${JPEG_LIB_ status(" Use PNG:" USE_PNG THEN "yes (ver ${PNG_VERSION_STRING})" ELSE "no") status(" \\- Use ZLIB:" USE_ZLIB THEN "yes (ver ${ZLIB_VERSION_STRING})" ELSE "no") status(" Use OpenCV:" USE_OPENCV THEN "yes (ver ${OpenCV_VERSION})" ELSE "no") -status(" Use stb_image (built-in):" "yes (ver ${STBIMAGE_VERSION})") -status(" Use TinyEXR (built-in):" "yes (ver ${TINYEXR_VERSION})") +status(" Use stb_image (built-in):" WITH_STBIMAGE THEN "yes (ver ${STBIMAGE_VERSION})" ELSE "no") +status(" Use TinyEXR (built-in):" WITH_TINYEXR THEN "yes (ver ${TINYEXR_VERSION})" ELSE "no") +status(" Use simdlib (built-in):" WITH_SIMDLIB THEN "yes" ELSE "no") status("") status(" Real robots: ") status(" Use Afma4:" USE_AFMA4 THEN "yes" ELSE "no") @@ -1647,7 +1657,7 @@ status(" \\- Use AprilTag big family:" WITH_APRILTAG_BIG_FAMILY THEN "yes" E status("") status(" Misc: ") status(" Use Clipper (built-in):" WITH_CLIPPER THEN "yes (ver ${CLIPPER_VERSION})" ELSE "no") -status(" Use pugixml (built-in):" "yes (ver ${PUGIXML_VERSION})") +status(" Use pugixml (built-in):" WITH_PUGIXML THEN "yes (ver ${PUGIXML_VERSION})" ELSE "no") status(" Use libxml2:" USE_XML2 THEN "yes (ver ${XML2_VERSION_STRING})" ELSE "no") status(" Use json (nlohmann):" USE_NLOHMANN_JSON THEN "yes (ver ${nlohmann_json_VERSION})" ELSE "no") status("") @@ -1655,7 +1665,7 @@ status(" Optimization: ") status(" Use OpenMP:" USE_OPENMP THEN "yes" ELSE "no") status(" Use st::thread:" USE_THREADS THEN "yes" ELSE "no") status(" Use pthread (built-in):" WITH_PTHREAD THEN "yes (ver ${PTHREADS_VERSION})" ELSE "no") -status(" Use Simd (built-in):" "yes (ver ${SIMD_VERSION})") +status(" Use simdlib (built-in):" WITH_SIMDLIB THEN "yes" ELSE "no") status("") status(" DNN: ") status(" Use CUDA Toolkit:" USE_TENSORRT AND CUDA_FOUND THEN "yes (ver ${CUDA_VERSION})" ELSE "no") diff --git a/apps/calibration/visp-acquire-franka-calib-data.cpp b/apps/calibration/visp-acquire-franka-calib-data.cpp index 3498622c4b..2de13c8a44 100644 --- a/apps/calibration/visp-acquire-franka-calib-data.cpp +++ b/apps/calibration/visp-acquire-franka-calib-data.cpp @@ -43,7 +43,7 @@ #include #if defined(VISP_HAVE_REALSENSE2) && \ - (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && defined(VISP_HAVE_FRANKA) && \ + (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && defined(VISP_HAVE_FRANKA) && defined(VISP_HAVE_PUGIXML) && \ defined(VISP_HAVE_MODULE_GUI) && defined(VISP_HAVE_MODULE_ROBOT) && defined(VISP_HAVE_MODULE_SENSOR) // optional void usage(const char **argv, int error, const std::string &robot_ip) @@ -103,8 +103,9 @@ int main(int argc, const char **argv) std::cout << "Image size: " << width << " x " << height << std::endl; // Save intrinsics vpCameraParameters cam; - vpXmlParserCamera xml_camera; + cam = g.getCameraParameters(RS2_STREAM_COLOR, vpCameraParameters::perspectiveProjWithDistortion); + vpXmlParserCamera xml_camera; xml_camera.save(cam, "franka_camera.xml", "Camera", width, height); #if defined(VISP_HAVE_X11) @@ -172,6 +173,9 @@ int main() #if !defined(VISP_HAVE_FRANKA) std::cout << "Install libfranka." << std::endl; #endif +#if !defined(VISP_HAVE_PUGIXML) + std::cout << "Enable pugyxml built-in usage." << std::endl; +#endif std::cout << "After installation of the missing 3rd parties, configure ViSP with cmake " << "and build ViSP again." << std::endl; diff --git a/apps/calibration/visp-acquire-universal-robots-calib-data.cpp b/apps/calibration/visp-acquire-universal-robots-calib-data.cpp index 7ecadd446d..aea3d41630 100644 --- a/apps/calibration/visp-acquire-universal-robots-calib-data.cpp +++ b/apps/calibration/visp-acquire-universal-robots-calib-data.cpp @@ -43,7 +43,7 @@ #include #if defined(VISP_HAVE_REALSENSE2) && \ - (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && defined(VISP_HAVE_UR_RTDE) && \ + (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && defined(VISP_HAVE_UR_RTDE) && defined(VISP_HAVE_PUGIXML) && \ defined(VISP_HAVE_MODULE_GUI) && defined(VISP_HAVE_MODULE_ROBOT) && defined(VISP_HAVE_MODULE_SENSOR) // optional void usage(const char **argv, int error, const std::string &robot_ip) @@ -175,6 +175,9 @@ int main() std::cout << "ViSP is not build with libur_rtde 3rd party used to control a robot from Universal Robots..." << std::endl; #endif +#if !defined(VISP_HAVE_PUGIXML) + std::cout << "Enable pugyxml built-in usage." << std::endl; +#endif std::cout << "After installation of the missing 3rd parties, configure ViSP with cmake" << " and build ViSP again." << std::endl; diff --git a/apps/calibration/visp-compute-chessboard-poses.cpp b/apps/calibration/visp-compute-chessboard-poses.cpp index 9c095bf1b3..b0b56b2af3 100644 --- a/apps/calibration/visp-compute-chessboard-poses.cpp +++ b/apps/calibration/visp-compute-chessboard-poses.cpp @@ -36,7 +36,7 @@ #include -#if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_CALIB3D) +#if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_CALIB3D) && defined(VISP_HAVE_PUGIXML) #include #include @@ -345,7 +345,12 @@ int main(int argc, const char **argv) #else int main() { - std::cerr << "OpenCV 2.3.0 or higher is requested to run the calibration." << std::endl; +#if !(defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_CALIB3D)) + std::cerr << "OpenCV calib3d module is requested to run the calibration." << std::endl; +#endif +#if !defined(VISP_HAVE_PUGIXML) + std::cout << "pugixml built-in 3rdparty is requested to run the calibration." << std::endl; +#endif return EXIT_SUCCESS; } #endif diff --git a/cmake/VISP3rdParty.cmake b/cmake/VISP3rdParty.cmake index 65426c7b36..d821000499 100644 --- a/cmake/VISP3rdParty.cmake +++ b/cmake/VISP3rdParty.cmake @@ -57,30 +57,34 @@ if(WITH_TAKKTILE2) set(TAKKTILE2_VERSION ${TAKKTILE2_MAJOR_VERSION}.${TAKKTILE2_MINOR_VERSION}.${TAKKTILE2_PATCH_VERSION}) endif() -# pugixml is always enabled to provide default XML I/O capabilities -set(PUGIXML_LIBRARY visp_pugixml) -add_subdirectory("${VISP_SOURCE_DIR}/3rdparty/pugixml-1.9") -set(PUGIXML_INCLUDE_DIRS "${${PUGIXML_LIBRARY}_SOURCE_DIR}" "${${PUGIXML_LIBRARY}_BINARY_DIR}") -set(PUGIXML_LIBRARIES ${PUGIXML_LIBRARY}) -set(PUGIXML_VERSION ${PUGIXML_MAJOR_VERSION}.${PUGIXML_MINOR_VERSION}.${PUGIXML_PATCH_VERSION}) +if(WITH_PUGIXML) + set(PUGIXML_LIBRARY visp_pugixml) + add_subdirectory("${VISP_SOURCE_DIR}/3rdparty/pugixml-1.9") + set(PUGIXML_INCLUDE_DIRS "${${PUGIXML_LIBRARY}_SOURCE_DIR}" "${${PUGIXML_LIBRARY}_BINARY_DIR}") + set(PUGIXML_LIBRARIES ${PUGIXML_LIBRARY}) + set(PUGIXML_VERSION ${PUGIXML_MAJOR_VERSION}.${PUGIXML_MINOR_VERSION}.${PUGIXML_PATCH_VERSION}) +endif() -# simdlib is always enabled since it contains fallback code to plain C++ code -set(SIMD_LIBRARY visp_simdlib) -add_subdirectory("${VISP_SOURCE_DIR}/3rdparty/simdlib") -set(SIMDLIB_INCLUDE_DIRS "${VISP_SOURCE_DIR}/3rdparty/simdlib") -set(SIMDLIB_LIBRARIES ${SIMD_LIBRARY}) +if(WITH_SIMDLIB) + set(SIMD_LIBRARY visp_simdlib) + add_subdirectory("${VISP_SOURCE_DIR}/3rdparty/simdlib") + set(SIMDLIB_INCLUDE_DIRS "${VISP_SOURCE_DIR}/3rdparty/simdlib") + set(SIMDLIB_LIBRARIES ${SIMD_LIBRARY}) +endif() -# stb is always enabled -set(STBIMAGE_LIBRARY visp_stbimage) -add_subdirectory("${VISP_SOURCE_DIR}/3rdparty/stb_image") -set(STBIMAGE_INCLUDE_DIRS "${VISP_SOURCE_DIR}/3rdparty/stb_image") -set(STBIMAGE_VERSION ${STBIMAGE_MAJOR_VERSION}.${STBIMAGE_MINOR_VERSION}.${STBIMAGE_PATCH_VERSION}) +if(WITH_STBIMAGE) + set(STBIMAGE_LIBRARY visp_stbimage) + add_subdirectory("${VISP_SOURCE_DIR}/3rdparty/stb_image") + set(STBIMAGE_INCLUDE_DIRS "${VISP_SOURCE_DIR}/3rdparty/stb_image") + set(STBIMAGE_VERSION ${STBIMAGE_MAJOR_VERSION}.${STBIMAGE_MINOR_VERSION}.${STBIMAGE_PATCH_VERSION}) +endif() -# tinyexr is always enabled -set(TINYEXR_LIBRARY visp_tinyexr) -add_subdirectory("${VISP_SOURCE_DIR}/3rdparty/tinyexr") -set(TINYEXR_INCLUDE_DIRS "${VISP_SOURCE_DIR}/3rdparty/tinyexr") -set(TINYEXR_VERSION ${TINYEXR_MAJOR_VERSION}.${TINYEXR_MINOR_VERSION}.${TINYEXR_PATCH_VERSION}) +if(WITH_TINYEXR) + set(TINYEXR_LIBRARY visp_tinyexr) + add_subdirectory("${VISP_SOURCE_DIR}/3rdparty/tinyexr") + set(TINYEXR_INCLUDE_DIRS "${VISP_SOURCE_DIR}/3rdparty/tinyexr") + set(TINYEXR_VERSION ${TINYEXR_MAJOR_VERSION}.${TINYEXR_MINOR_VERSION}.${TINYEXR_PATCH_VERSION}) +endif() if(WITH_CATCH2) set(CATCH2_LIBRARY visp_catch2) diff --git a/cmake/VISPGenerateConfigScript.cmake b/cmake/VISPGenerateConfigScript.cmake index 395301e665..17d4809160 100644 --- a/cmake/VISPGenerateConfigScript.cmake +++ b/cmake/VISPGenerateConfigScript.cmake @@ -181,6 +181,10 @@ if(NOT DEFINED CMAKE_HELPER_SCRIPT) VISP_HAVE_OPENMP WITH_CATCH2 + WITH_PUGIXML + WITH_SIMDLIB + WITH_STBIMAGE + WITH_TINYEXR FILE_VISP_SCRIPT_CONFIG FILE_VISP_SCRIPT_CONFIG_INSTALL diff --git a/cmake/templates/VISPConfig.cmake.in b/cmake/templates/VISPConfig.cmake.in index fd4b9b6bdb..58607a222e 100644 --- a/cmake/templates/VISPConfig.cmake.in +++ b/cmake/templates/VISPConfig.cmake.in @@ -243,20 +243,25 @@ set(VISP_HAVE_PARPORT "@VISP_HAVE_PARPORT@") set(VISP_HAVE_PCL "@VISP_HAVE_PCL@") set(VISP_HAVE_PIONEER "@VISP_HAVE_PIONEER@") set(VISP_HAVE_PNG "@VISP_HAVE_PNG@") +set(VISP_HAVE_POLOLU "@VISP_HAVE_POLOLU@") set(VISP_HAVE_PTHREAD "@VISP_HAVE_PTHREAD@") set(VISP_HAVE_PTU46 "@VISP_HAVE_PTU46@") +set(VISP_HAVE_PUGIXML "@VISP_HAVE_PUGIXML@") set(VISP_HAVE_PYLON "@VISP_HAVE_PYLON@") set(VISP_HAVE_QBDEVICE "@VISP_HAVE_QBDEVICE@") set(VISP_HAVE_QT "@VISP_HAVE_QT@") set(VISP_HAVE_QUALISYS "@VISP_HAVE_QUALISYS@") set(VISP_HAVE_REALSENSE "@VISP_HAVE_REALSENSE@") set(VISP_HAVE_REALSENSE2 "@VISP_HAVE_REALSENSE2@") +set(VISP_HAVE_SIMDLIB "@VISP_HAVE_SIMDLIB@") set(VISP_HAVE_SOQT "@VISP_HAVE_SOQT@") set(VISP_HAVE_SOWIN "@VISP_HAVE_SOWIN@") set(VISP_HAVE_SOXT "@VISP_HAVE_SOXT@") +set(VISP_HAVE_STBIMAGE "@VISP_HAVE_STBIMAGE@") set(VISP_HAVE_TAKKTILE2 "@VISP_HAVE_TAKKTILE2@") set(VISP_HAVE_TENSORRT "@VISP_HAVE_TENSORRT@") set(VISP_HAVE_THREADS "@VISP_HAVE_THREADS@") +set(VISP_HAVE_TINYEXR "@VISP_HAVE_TINYEXR@") set(VISP_HAVE_UEYE "@VISP_HAVE_UEYE@") set(VISP_HAVE_UR_RTDE "@VISP_HAVE_UR_RTDE@") set(VISP_HAVE_V4L2 "@VISP_HAVE_V4L2@") diff --git a/cmake/templates/vpConfig.h.in b/cmake/templates/vpConfig.h.in index 5713458ae2..78f4df6082 100644 --- a/cmake/templates/vpConfig.h.in +++ b/cmake/templates/vpConfig.h.in @@ -141,7 +141,7 @@ #cmakedefine VISP_HAVE_X11 // Always define pugixml for compatibility. -#define VISP_HAVE_PUGIXML +#cmakedefine VISP_HAVE_PUGIXML // Defined if XML2 library available. #cmakedefine VISP_HAVE_XML2 @@ -192,6 +192,15 @@ // Defined if Catch2 library available #cmakedefine VISP_HAVE_CATCH2 +// Defined if simdlib library available +#cmakedefine VISP_HAVE_SIMDLIB + +// Defined if stb_image library available +#cmakedefine VISP_HAVE_STBIMAGE + +// Defined if tinyexr library available +#cmakedefine VISP_HAVE_TINYEXR + // Defined if Eigen3 library available #cmakedefine VISP_HAVE_EIGEN3 diff --git a/doc/config-doxygen.in b/doc/config-doxygen.in index ab7ac43a11..3dbcca0aaa 100644 --- a/doc/config-doxygen.in +++ b/doc/config-doxygen.in @@ -2415,6 +2415,7 @@ PREDEFINED = @DOXYGEN_SHOULD_SKIP_THIS@ \ VISP_HAVE_POLOLU \ VISP_HAVE_PTHREAD \ VISP_HAVE_PTU46 \ + VISP_HAVE_PUGIXML \ VISP_HAVE_PYLON \ VISP_HAVE_QBDEVICE \ VISP_HAVE_QT \ @@ -2422,12 +2423,15 @@ PREDEFINED = @DOXYGEN_SHOULD_SKIP_THIS@ \ VISP_HAVE_REALSENSE \ VISP_HAVE_REALSENSE_VERSION=0x020000 \ VISP_HAVE_REALSENSE2 \ + VISP_HAVE_SIMDLIB \ VISP_HAVE_SOWIN \ VISP_HAVE_SOQT \ VISP_HAVE_SOXT \ + VISP_HAVE_STBIMAGE \ VISP_HAVE_TAKKTILE2 \ VISP_HAVE_TENSORRT \ VISP_HAVE_THREADS \ + VISP_HAVE_TINYEXR \ VISP_HAVE_UEYE \ VISP_HAVE_UR_RTDE \ VISP_HAVE_V4L2 \ diff --git a/example/calibration/calibrate-camera.cpp b/example/calibration/calibrate-camera.cpp index 84f59da1f7..560f65a4f1 100644 --- a/example/calibration/calibrate-camera.cpp +++ b/example/calibration/calibrate-camera.cpp @@ -36,7 +36,8 @@ #include -#if (VISP_HAVE_OPENCV_VERSION >= 0x030000) && defined(HAVE_OPENCV_CALIB3D) && defined(HAVE_OPENCV_HIGHGUI) && defined(HAVE_OPENCV_IMGPROC) +#if (VISP_HAVE_OPENCV_VERSION >= 0x030000) && defined(HAVE_OPENCV_CALIB3D) && defined(HAVE_OPENCV_HIGHGUI) && \ + defined(HAVE_OPENCV_IMGPROC) && defined(VISP_HAVE_PUGIXML) #include @@ -67,34 +68,34 @@ using namespace calib_helper; void usage(const char *argv[], int error) { std::cout << "Synopsis" << std::endl - << " " << argv[0] << " .cfg [--init-from-xml ]" - << " [--camera-name ] [--aspect-ratio ] [--output ] [--help] [-h]" << std::endl - << std::endl; + << " " << argv[0] << " .cfg [--init-from-xml ]" + << " [--camera-name ] [--aspect-ratio ] [--output ] [--help] [-h]" << std::endl + << std::endl; std::cout << "Description" << std::endl - << " .cfg Configuration file. See example in" << std::endl - << " \"default-chessboard.cfg\" or in \"default-circles.cfg\"." << std::endl - << " Default: \"default.cfg\"." << std::endl - << std::endl - << " --init-from-xml XML file that contains camera parameters" << std::endl - << " used to initialize the calibration process." << std::endl - << std::endl - << " --camera-name Camera name in the XML file set using \"--init-from-xml\" option." << std::endl - << " Default: \"Camera\"." << std::endl - << std::endl - << " --aspect-ratio Pixel aspect ratio. " << std::endl - << " To estimate px = py, use \"--aspect-ratio 1\" option. Set to -1" << std::endl - << " to unset any constraint for px and py parameters. " << std::endl - << " Default: -1." << std::endl - << std::endl - << " --output XML file containing estimated camera parameters." << std::endl - << " Default: \"camera.xml\"." << std::endl - << std::endl - << " --help, -h Print this helper message." << std::endl - << std::endl; + << " .cfg Configuration file. See example in" << std::endl + << " \"default-chessboard.cfg\" or in \"default-circles.cfg\"." << std::endl + << " Default: \"default.cfg\"." << std::endl + << std::endl + << " --init-from-xml XML file that contains camera parameters" << std::endl + << " used to initialize the calibration process." << std::endl + << std::endl + << " --camera-name Camera name in the XML file set using \"--init-from-xml\" option." << std::endl + << " Default: \"Camera\"." << std::endl + << std::endl + << " --aspect-ratio Pixel aspect ratio. " << std::endl + << " To estimate px = py, use \"--aspect-ratio 1\" option. Set to -1" << std::endl + << " to unset any constraint for px and py parameters. " << std::endl + << " Default: -1." << std::endl + << std::endl + << " --output XML file containing estimated camera parameters." << std::endl + << " Default: \"camera.xml\"." << std::endl + << std::endl + << " --help, -h Print this helper message." << std::endl + << std::endl; if (error) { std::cout << "Error" << std::endl - << " " - << "Unsupported parameter " << argv[error] << std::endl; + << " " + << "Unsupported parameter " << argv[error] << std::endl; } } @@ -116,19 +117,24 @@ int main(int argc, const char *argv[]) if (std::string(argv[i]) == "--init-from-xml" && i + 1 < argc) { opt_init_camera_xml_file = std::string(argv[i + 1]); i++; - } else if (std::string(argv[i]) == "--camera-name" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--camera-name" && i + 1 < argc) { opt_camera_name = std::string(argv[i + 1]); i++; - } else if (std::string(argv[i]) == "--output" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--output" && i + 1 < argc) { opt_output_file_name = std::string(argv[i + 1]); i++; - } else if (std::string(argv[i]) == "--aspect-ratio" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--aspect-ratio" && i + 1 < argc) { opt_aspect_ratio = std::atof(argv[i + 1]); i++; - } else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") { + } + else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") { usage(argv, 0); return EXIT_SUCCESS; - } else { + } + else { usage(argv, i); return EXIT_FAILURE; } @@ -157,7 +163,7 @@ int main(int argc, const char *argv[]) if (vpIoTools::checkFilename(opt_output_file_name)) { std::cout << "\nOutput file name " << opt_output_file_name << " already exists." << std::endl; std::cout << "Remove this file or change output file name using [--output ] command line option." - << std::endl; + << std::endl; return EXIT_SUCCESS; } @@ -167,10 +173,11 @@ int main(int argc, const char *argv[]) reader.setFileName(s.input); try { reader.open(I); - } catch (const vpException &e) { + } + catch (const vpException &e) { std::cout << "Catch an exception: " << e.getStringMessage() << std::endl; std::cout << "Check if input images name \"" << s.input << "\" set in " << opt_inputSettingsFile - << " config file is correct..." << std::endl; + << " config file is correct..." << std::endl; return EXIT_FAILURE; } @@ -200,11 +207,12 @@ int main(int argc, const char *argv[]) if (parser.parse(cam_init, opt_init_camera_xml_file, opt_camera_name, vpCameraParameters::perspectiveProjWithoutDistortion) != vpXmlParserCamera::SEQUENCE_OK) { std::cout << "Unable to find camera with name \"" << opt_camera_name - << "\" in file: " << opt_init_camera_xml_file << std::endl; + << "\" in file: " << opt_init_camera_xml_file << std::endl; std::cout << "Modify [--camera-name ] option value" << std::endl; return EXIT_FAILURE; } - } else { + } + else { std::cout << "Initialize camera parameters with default values " << std::endl; // Initialize camera parameters double px = cam_init.get_px(); @@ -292,7 +300,7 @@ int main(int argc, const char *argv[]) } if (!calib_status) { std::cout << "frame: " << frame_name << ", unable to calibrate from single image, image rejected" - << std::endl; + << std::endl; found = false; } } @@ -309,7 +317,8 @@ int main(int argc, const char *argv[]) "A click to process the next image", vpColor::green); vpDisplay::flush(I); vpDisplay::getClick(I); - } else { + } + else { vpDisplay::flush(I); vpTime::wait(s.tempo * 1000); } @@ -448,15 +457,16 @@ int main(int argc, const char *argv[]) if (xml.save(cam, opt_output_file_name.c_str(), opt_camera_name, I.getWidth(), I.getHeight()) == vpXmlParserCamera::SEQUENCE_OK) std::cout << "Camera parameters without distortion successfully saved in \"" << opt_output_file_name << "\"" - << std::endl; + << std::endl; else { std::cout << "Failed to save the camera parameters without distortion in \"" << opt_output_file_name << "\"" - << std::endl; + << std::endl; std::cout << "A file with the same name exists. Remove it to be able " - "to save the parameters..." - << std::endl; + "to save the parameters..." + << std::endl; } - } else { + } + else { std::cout << "Calibration without distortion failed." << std::endl; return EXIT_FAILURE; } @@ -520,8 +530,8 @@ int main(int argc, const char *argv[]) for (size_t idx = 0; idx < calib_info.size(); idx++) { std::cout << "\nThis tool computes the line fitting error (mean distance error) on image points extracted from " - "the raw distorted image." - << std::endl; + "the raw distorted image." + << std::endl; I = calib_info[idx].m_img; vpImageTools::undistort(I, cam, I_undist); @@ -546,8 +556,8 @@ int main(int argc, const char *argv[]) double line_fitting_error = vpMath::lineFitting(current_line, a, b, c); double line_fitting_error_undist = vpMath::lineFitting(current_line_undist, a, b, c); std::cout << calib_info[idx].m_frame_name << " line " << i + 1 - << " fitting error on distorted points: " << line_fitting_error - << " ; on undistorted points: " << line_fitting_error_undist << std::endl; + << " fitting error on distorted points: " << line_fitting_error + << " ; on undistorted points: " << line_fitting_error_undist << std::endl; vpImagePoint ip1 = current_line.front(); vpImagePoint ip2 = current_line.back(); @@ -555,8 +565,8 @@ int main(int argc, const char *argv[]) } std::cout << "\nThis tool computes the line fitting error (mean distance error) on image points extracted from " - "the undistorted image" - << " (vpImageTools::undistort())." << std::endl; + "the undistorted image" + << " (vpImageTools::undistort())." << std::endl; cv::Mat cvI; std::vector pointBuf; vpImageConvert::convert(I_undist, cvI); @@ -579,13 +589,14 @@ int main(int argc, const char *argv[]) double a = 0, b = 0, c = 0; double line_fitting_error = vpMath::lineFitting(current_line, a, b, c); std::cout << calib_info[idx].m_frame_name << " undistorted image, line " << i + 1 - << " fitting error: " << line_fitting_error << std::endl; + << " fitting error: " << line_fitting_error << std::endl; vpImagePoint ip1 = current_line.front() + vpImagePoint(0, I.getWidth()); vpImagePoint ip2 = current_line.back() + vpImagePoint(0, I.getWidth()); vpDisplay::displayLine(I_dist_undist, ip1, ip2, vpColor::red); } - } else { + } + else { std::string msg("Unable to detect grid on undistorted image"); std::cout << msg << std::endl; std::cout << "Check that the grid is not too close to the image edges" << std::endl; @@ -627,29 +638,31 @@ int main(int argc, const char *argv[]) if (xml.save(cam, opt_output_file_name.c_str(), opt_camera_name, I.getWidth(), I.getHeight(), ss_additional_info.str()) == vpXmlParserCamera::SEQUENCE_OK) std::cout << "Camera parameters without distortion successfully saved in \"" << opt_output_file_name << "\"" - << std::endl; + << std::endl; else { std::cout << "Failed to save the camera parameters without distortion in \"" << opt_output_file_name << "\"" - << std::endl; + << std::endl; std::cout << "A file with the same name exists. Remove it to be able " - "to save the parameters..." - << std::endl; + "to save the parameters..." + << std::endl; } std::cout << std::endl; std::cout << "Estimated pose using vpPoseVector format: [tx ty tz tux tuy tuz] with translation in meter and " - "rotation in rad" - << std::endl; + "rotation in rad" + << std::endl; for (unsigned int i = 0; i < calibrator.size(); i++) std::cout << "Estimated pose on input data extracted from " << calib_info[i].m_frame_name << ": " - << vpPoseVector(calibrator[i].cMo_dist).t() << std::endl; - } else { + << vpPoseVector(calibrator[i].cMo_dist).t() << std::endl; + } + else { std::cout << "Calibration with distortion failed." << std::endl; return EXIT_FAILURE; } std::cout << "\nCamera calibration succeeded. Results are savec in " << "\"" << opt_output_file_name << "\"" << std::endl; return EXIT_SUCCESS; - } catch (const vpException &e) { + } + catch (const vpException &e) { std::cout << "Catch an exception: " << e << std::endl; return EXIT_FAILURE; } @@ -657,9 +670,14 @@ int main(int argc, const char *argv[]) #else int main() { - std::cout << "OpenCV 2.3.0 or higher is requested to run the calibration." << std::endl; +#if !((VISP_HAVE_OPENCV_VERSION >= 0x030000) && defined(HAVE_OPENCV_CALIB3D) && defined(HAVE_OPENCV_HIGHGUI) && defined(HAVE_OPENCV_IMGPROC)) + std::cout << "OpenCV calib3d, highgui and imgproc modules are requested to run the calibration." << std::endl; std::cout << "Tip:" << std::endl; std::cout << "- Install OpenCV, configure again ViSP using cmake and build again this example" << std::endl; +#endif +#if !defined(VISP_HAVE_PUGIXML) + std::cout << "pugixml built-in 3rdparty is requested to run the calibration." << std::endl; +#endif return EXIT_SUCCESS; } #endif diff --git a/example/device/framegrabber/saveRealSenseData.cpp b/example/device/framegrabber/saveRealSenseData.cpp index 989510085b..06e56f8e45 100644 --- a/example/device/framegrabber/saveRealSenseData.cpp +++ b/example/device/framegrabber/saveRealSenseData.cpp @@ -40,7 +40,7 @@ #include #if (defined(VISP_HAVE_REALSENSE) || defined(VISP_HAVE_REALSENSE2)) && \ - (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) + (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && defined(VISP_HAVE_PUGIXML) #include #include @@ -728,6 +728,10 @@ int main(int argc, char *argv[]) int main() { std::cerr << "Need libRealSense or libRealSense2 and C++11 and displayX or displayGDI!" << std::endl; + +#if !defined(VISP_HAVE_PUGIXML) + std::cout << "pugixml built-in 3rdparty is requested." << std::endl; +#endif return EXIT_SUCCESS; } #endif diff --git a/example/servo-bebop2/servoBebop2.cpp b/example/servo-bebop2/servoBebop2.cpp index 98bd516911..90fe22ab2f 100644 --- a/example/servo-bebop2/servoBebop2.cpp +++ b/example/servo-bebop2/servoBebop2.cpp @@ -73,6 +73,12 @@ int main() std::cout << "\nThis example requires ffmpeg library. You should install it.\n" << std::endl; return EXIT_SUCCESS; } +#elif !defined(VISP_HAVE_PUGIXML) +int main() +{ + std::cout << "\nThis example requires pugixml built-in 3rdparty library. You should enable it.\n" << std::endl; + return EXIT_SUCCESS; +} #else diff --git a/example/servo-flir-ptu/servoFlirPtuIBVS.cpp b/example/servo-flir-ptu/servoFlirPtuIBVS.cpp index 54f83f6f4b..329d749377 100644 --- a/example/servo-flir-ptu/servoFlirPtuIBVS.cpp +++ b/example/servo-flir-ptu/servoFlirPtuIBVS.cpp @@ -71,8 +71,8 @@ #include #include -#if defined(VISP_HAVE_FLIR_PTU_SDK) && defined(VISP_HAVE_FLYCAPTURE) && \ - (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) +#if defined(VISP_HAVE_FLIR_PTU_SDK) && defined(VISP_HAVE_FLYCAPTURE) && \ + (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && defined(VISP_HAVE_PUGIXML) void display_point_trajectory(const vpImage &I, const vpImagePoint &ip, std::vector &traj_ip) @@ -82,7 +82,8 @@ void display_point_trajectory(const vpImage &I, const vpImagePoin if (vpImagePoint::distance(ip, traj_ip.back()) > 2.) { traj_ip.push_back(ip); } - } else { + } + else { traj_ip.push_back(ip); } for (size_t j = 1; j < traj_ip.size(); j++) { @@ -117,113 +118,127 @@ int main(int argc, char **argv) for (int i = 1; i < argc; i++) { if ((std::string(argv[i]) == "--portname" || std::string(argv[i]) == "-p") && (i + 1 < argc)) { opt_portname = std::string(argv[i + 1]); - } else if ((std::string(argv[i]) == "--baudrate" || std::string(argv[i]) == "-b") && (i + 1 < argc)) { + } + else if ((std::string(argv[i]) == "--baudrate" || std::string(argv[i]) == "-b") && (i + 1 < argc)) { opt_baudrate = std::atoi(argv[i + 1]); - } else if ((std::string(argv[i]) == "--network" || std::string(argv[i]) == "-n")) { + } + else if ((std::string(argv[i]) == "--network" || std::string(argv[i]) == "-n")) { opt_network = true; - } else if (std::string(argv[i]) == "--extrinsic" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--extrinsic" && i + 1 < argc) { opt_extrinsic = std::string(argv[i + 1]); - } else if (std::string(argv[i]) == "--intrinsic" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--intrinsic" && i + 1 < argc) { opt_intrinsic = std::string(argv[i + 1]); - } else if (std::string(argv[i]) == "--camera-name" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--camera-name" && i + 1 < argc) { opt_camera_name = std::string(argv[i + 1]); - } else if (std::string(argv[i]) == "--verbose" || std::string(argv[i]) == "-v") { + } + else if (std::string(argv[i]) == "--verbose" || std::string(argv[i]) == "-v") { opt_verbose = true; - } else if (std::string(argv[i]) == "--plot" || std::string(argv[i]) == "-p") { + } + else if (std::string(argv[i]) == "--plot" || std::string(argv[i]) == "-p") { opt_plot = true; - } else if (std::string(argv[i]) == "--display-image-trajectory" || std::string(argv[i]) == "-traj") { + } + else if (std::string(argv[i]) == "--display-image-trajectory" || std::string(argv[i]) == "-traj") { opt_display_trajectory = true; - } else if (std::string(argv[i]) == "--adaptive-gain" || std::string(argv[i]) == "-a") { + } + else if (std::string(argv[i]) == "--adaptive-gain" || std::string(argv[i]) == "-a") { opt_adaptive_gain = true; - } else if (std::string(argv[i]) == "--constant-gain" || std::string(argv[i]) == "-g") { + } + else if (std::string(argv[i]) == "--constant-gain" || std::string(argv[i]) == "-g") { opt_constant_gain = std::stod(argv[i + 1]); - } else if (std::string(argv[i]) == "--task-sequencing") { + } + else if (std::string(argv[i]) == "--task-sequencing") { opt_task_sequencing = true; - } else if (std::string(argv[i]) == "--quad-decimate" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--quad-decimate" && i + 1 < argc) { opt_quad_decimate = std::stoi(argv[i + 1]); } if (std::string(argv[i]) == "--tag-size" && i + 1 < argc) { opt_tag_size = std::stod(argv[i + 1]); - } else if (std::string(argv[i]) == "--no-convergence-threshold") { + } + else if (std::string(argv[i]) == "--no-convergence-threshold") { convergence_threshold = 0.; - } else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") { + } + else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") { std::cout << "SYNOPSIS" << std::endl - << " " << argv[0] << " [--portname ] [--baudrate ] [--network] " - << "[--extrinsic ] [--intrinsic ] [--camera-name ] " - << "[--quad-decimate ] [--tag-size ] " - << "[--adaptive-gain] [--constant-gain] [--display-image-trajectory] [--plot] [--task-sequencing] " - << "[--no-convergence-threshold] [--verbose] [--help] [-h]" << std::endl - << std::endl; + << " " << argv[0] << " [--portname ] [--baudrate ] [--network] " + << "[--extrinsic ] [--intrinsic ] [--camera-name ] " + << "[--quad-decimate ] [--tag-size ] " + << "[--adaptive-gain] [--constant-gain] [--display-image-trajectory] [--plot] [--task-sequencing] " + << "[--no-convergence-threshold] [--verbose] [--help] [-h]" << std::endl + << std::endl; std::cout << "DESCRIPTION" << std::endl - << " --portname, -p " << std::endl - << " Set serial or tcp port name." << std::endl - << std::endl - << " --baudrate, -b " << std::endl - << " Set serial communication baud rate. Default: " << opt_baudrate << "." << std::endl - << std::endl - << " --network, -n" << std::endl - << " Get PTU network information (Hostname, IP, Gateway) and exit. " << std::endl - << std::endl - << " --reset, -r" << std::endl - << " Reset PTU axis and exit. " << std::endl - << std::endl - << " --extrinsic " << std::endl - << " YAML file containing extrinsic camera parameters as a vpHomogeneousMatrix." << std::endl - << " It corresponds to the homogeneous transformation eMc, between end-effector" << std::endl - << " and camera frame." << std::endl - << std::endl - << " --intrinsic " << std::endl - << " Intrinsic camera parameters obtained after camera calibration." << std::endl - << std::endl - << " --camera-name " << std::endl - << " Name of the camera to consider in the xml file provided for intrinsic camera parameters." - << std::endl - << std::endl - << " --quad-decimate " << std::endl - << " Decimation factor used to detect AprilTag. Default " << opt_quad_decimate << "." << std::endl - << std::endl - << " --tag-size " << std::endl - << " Width in meter or the black part of the AprilTag used as target. Default " << opt_tag_size - << "." << std::endl - << std::endl - << " --adaptive-gain, -a" << std::endl - << " Enable adaptive gain instead of constant gain to speed up convergence. " << std::endl - << std::endl - << " --constant-gain, -g" << std::endl - << " Constant gain value. Default value: " << opt_constant_gain << std::endl - << std::endl - << " --display-image-trajectory, -traj" << std::endl - << " Display the trajectory of the target cog in the image. " << std::endl - << std::endl - << " --plot, -p" << std::endl - << " Enable curve plotter. " << std::endl - << std::endl - << " --task-sequencing" << std::endl - << " Enable task sequencing that allows to smoothly control the velocity of the robot. " << std::endl - << std::endl - << " --no-convergence-threshold" << std::endl - << " Disable ending servoing when it reaches the desired position." << std::endl - << std::endl - << " --verbose, -v" << std::endl - << " Additional printings. " << std::endl - << std::endl - << " --help, -h" << std::endl - << " Print this helper message. " << std::endl - << std::endl; + << " --portname, -p " << std::endl + << " Set serial or tcp port name." << std::endl + << std::endl + << " --baudrate, -b " << std::endl + << " Set serial communication baud rate. Default: " << opt_baudrate << "." << std::endl + << std::endl + << " --network, -n" << std::endl + << " Get PTU network information (Hostname, IP, Gateway) and exit. " << std::endl + << std::endl + << " --reset, -r" << std::endl + << " Reset PTU axis and exit. " << std::endl + << std::endl + << " --extrinsic " << std::endl + << " YAML file containing extrinsic camera parameters as a vpHomogeneousMatrix." << std::endl + << " It corresponds to the homogeneous transformation eMc, between end-effector" << std::endl + << " and camera frame." << std::endl + << std::endl + << " --intrinsic " << std::endl + << " Intrinsic camera parameters obtained after camera calibration." << std::endl + << std::endl + << " --camera-name " << std::endl + << " Name of the camera to consider in the xml file provided for intrinsic camera parameters." + << std::endl + << std::endl + << " --quad-decimate " << std::endl + << " Decimation factor used to detect AprilTag. Default " << opt_quad_decimate << "." << std::endl + << std::endl + << " --tag-size " << std::endl + << " Width in meter or the black part of the AprilTag used as target. Default " << opt_tag_size + << "." << std::endl + << std::endl + << " --adaptive-gain, -a" << std::endl + << " Enable adaptive gain instead of constant gain to speed up convergence. " << std::endl + << std::endl + << " --constant-gain, -g" << std::endl + << " Constant gain value. Default value: " << opt_constant_gain << std::endl + << std::endl + << " --display-image-trajectory, -traj" << std::endl + << " Display the trajectory of the target cog in the image. " << std::endl + << std::endl + << " --plot, -p" << std::endl + << " Enable curve plotter. " << std::endl + << std::endl + << " --task-sequencing" << std::endl + << " Enable task sequencing that allows to smoothly control the velocity of the robot. " << std::endl + << std::endl + << " --no-convergence-threshold" << std::endl + << " Disable ending servoing when it reaches the desired position." << std::endl + << std::endl + << " --verbose, -v" << std::endl + << " Additional printings. " << std::endl + << std::endl + << " --help, -h" << std::endl + << " Print this helper message. " << std::endl + << std::endl; std::cout << "EXAMPLE" << std::endl - << " - How to get network IP" << std::endl + << " - How to get network IP" << std::endl #ifdef _WIN32 - << " $ " << argv[0] << " --portname COM1 --network" << std::endl - << " Try to connect FLIR PTU to port: COM1 with baudrate: 9600" << std::endl + << " $ " << argv[0] << " --portname COM1 --network" << std::endl + << " Try to connect FLIR PTU to port: COM1 with baudrate: 9600" << std::endl #else - << " $ " << argv[0] << " -p /dev/ttyUSB0 --network" << std::endl - << " Try to connect FLIR PTU to port: /dev/ttyUSB0 with baudrate: 9600" << std::endl + << " $ " << argv[0] << " -p /dev/ttyUSB0 --network" << std::endl + << " Try to connect FLIR PTU to port: /dev/ttyUSB0 with baudrate: 9600" << std::endl #endif - << " PTU HostName: PTU-5" << std::endl - << " PTU IP : 169.254.110.254" << std::endl - << " PTU Gateway : 0.0.0.0" << std::endl - << " - How to run this binary using network communication" << std::endl - << " $ " << argv[0] << " --portname tcp:169.254.110.254 --tag-size 0.1 --gain 0.1" << std::endl; + << " PTU HostName: PTU-5" << std::endl + << " PTU IP : 169.254.110.254" << std::endl + << " PTU Gateway : 0.0.0.0" << std::endl + << " - How to run this binary using network communication" << std::endl + << " $ " << argv[0] << " --portname tcp:169.254.110.254 --tag-size 0.1 --gain 0.1" << std::endl; return EXIT_SUCCESS; } @@ -259,24 +274,25 @@ int main(int argc, char **argv) vpPoseVector ePc; ePc.loadYAML(opt_extrinsic, ePc); eMc.buildFrom(ePc); - } else { + } + else { std::cout << "***************************************************************" << std::endl; std::cout << "Warning, use hard coded values for extrinsic camera parameters." << std::endl; std::cout << "Create a yaml file that contains the extrinsic:" << std::endl - << std::endl - << "$ cat eMc.yaml" << std::endl - << "rows: 4" << std::endl - << "cols: 4" << std::endl - << "data:" << std::endl - << " - [0, 0, 1, -0.1]" << std::endl - << " - [-1, 0, 0, -0.123]" << std::endl - << " - [0, -1, 0, 0.035]" << std::endl - << " - [0, 0, 0, 1]" << std::endl - << std::endl - << "and load this file with [--extrinsic // Check if std:c++17 or higher -#if defined(VISP_HAVE_MAVSDK) && ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) && defined(VISP_HAVE_REALSENSE2) +#if defined(VISP_HAVE_MAVSDK) && ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) && \ + defined(VISP_HAVE_REALSENSE2) && defined(VISP_HAVE_PUGIXML) #include #include @@ -574,12 +575,13 @@ int main(int argc, char **argv) int main() { #ifndef VISP_HAVE_MAVSDK - std::cout << "\nThis example requires mavsdk library. You should install it, configure and rebuid ViSP.\n" - << std::endl; + std::cout << "\nThis example requires mavsdk library. You should install it, configure and rebuid ViSP.\n" << std::endl; #endif #ifndef VISP_HAVE_REALSENSE2 - std::cout << "\nThis example requires librealsense2 library. You should install it, configure and rebuid ViSP.\n" - << std::endl; + std::cout << "\nThis example requires librealsense2 library. You should install it, configure and rebuid ViSP.\n" << std::endl; +#endif +#if !defined(VISP_HAVE_PUGIXML) + std::cout << "\nThis example requires pugixml built-in 3rdparty." << std::endl; #endif #if !((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) std::cout diff --git a/example/servo-pololu-ptu/CMakeLists.txt b/example/servo-pololu-ptu/CMakeLists.txt index 10c599877d..ec785de8d1 100644 --- a/example/servo-pololu-ptu/CMakeLists.txt +++ b/example/servo-pololu-ptu/CMakeLists.txt @@ -49,3 +49,14 @@ foreach(cpp ${example_cpp}) visp_add_dependency(${cpp} "examples") endif() endforeach() + +if(VISP_HAVE_REALSENSE2) + # Add specific build flag to turn off warnings coming from librealsense 3rd party + list(APPEND CXX_FLAGS_MUTE_WARNINGS "-Wno-deprecated-copy-with-user-provided-copy") + list(APPEND CXX_FLAGS_MUTE_WARNINGS "-Wno-deprecated-declarations") + list(APPEND CXX_FLAGS_MUTE_WARNINGS "-Wno-ignored-qualifiers") + list(APPEND CXX_FLAGS_MUTE_WARNINGS "-Wno-sign-compare") + list(APPEND CXX_FLAGS_MUTE_WARNINGS "-Wno-unqualified-std-cast-call") + + visp_set_source_file_compile_flag(servoPololuPtuPoint2DJointVelocity.cpp ${CXX_FLAGS_MUTE_WARNINGS}) +endif() diff --git a/example/tracking/mbtEdgeKltTracking.cpp b/example/tracking/mbtEdgeKltTracking.cpp index 1cee25a362..8c86255546 100644 --- a/example/tracking/mbtEdgeKltTracking.cpp +++ b/example/tracking/mbtEdgeKltTracking.cpp @@ -269,8 +269,8 @@ int main(int argc, const char **argv) // Read the command line options if (!getOptions(argc, argv, opt_ipath, opt_configFile, opt_modelFile, opt_initFile, opt_lastFrame, displayFeatures, - opt_click_allowed, opt_display, cao3DModel, trackCylinder, useOgre, showOgreConfigDialog, - useScanline, computeCovariance, projectionError)) { + opt_click_allowed, opt_display, cao3DModel, trackCylinder, useOgre, showOgreConfigDialog, + useScanline, computeCovariance, projectionError)) { return EXIT_FAILURE; } @@ -393,10 +393,11 @@ int main(int argc, const char **argv) vpCameraParameters cam; // Initialise the tracker: camera parameters, moving edge and KLT settings + +#if defined(VISP_HAVE_PUGIXML) // From the xml file tracker.loadConfigFile(configFile); - -#if 0 +#else // Corresponding parameters manually set to have an example code // By setting the parameters: cam.initPersProjWithoutDistortion(547, 542, 338, 234); @@ -506,9 +507,9 @@ int main(int argc, const char **argv) if (opt_display) vpDisplay::display(I); tracker.resetTracker(); +#if defined(VISP_HAVE_PUGIXML) tracker.loadConfigFile(configFile); - -#if 0 +#else // Corresponding parameters manually set to have an example code // By setting the parameters: cam.initPersProjWithoutDistortion(547, 542, 338, 234); diff --git a/example/tracking/mbtEdgeTracking.cpp b/example/tracking/mbtEdgeTracking.cpp index 5381e916c9..f00940d239 100644 --- a/example/tracking/mbtEdgeTracking.cpp +++ b/example/tracking/mbtEdgeTracking.cpp @@ -269,8 +269,8 @@ int main(int argc, const char **argv) // Read the command line options if (!getOptions(argc, argv, opt_ipath, opt_configFile, opt_modelFile, opt_initFile, opt_lastFrame, displayFeatures, - opt_click_allowed, opt_display, cao3DModel, trackCylinder, useOgre, showOgreConfigDialog, - useScanline, computeCovariance, projectionError)) { + opt_click_allowed, opt_display, cao3DModel, trackCylinder, useOgre, showOgreConfigDialog, + useScanline, computeCovariance, projectionError)) { return EXIT_FAILURE; } @@ -393,9 +393,10 @@ int main(int argc, const char **argv) // Initialise the tracker: camera parameters, moving edge and KLT settings vpCameraParameters cam; +#if defined(VISP_HAVE_PUGIXML) // From the xml file tracker.loadConfigFile(configFile); -#if 0 +#else // Corresponding parameters manually set to have an example code // By setting the parameters: cam.initPersProjWithoutDistortion(547, 542, 338, 234); @@ -492,8 +493,10 @@ int main(int argc, const char **argv) if (opt_display) vpDisplay::display(I); tracker.resetTracker(); + +#if defined(VISP_HAVE_PUGIXML) tracker.loadConfigFile(configFile); -#if 0 +#else // Corresponding parameters manually set to have an example code // By setting the parameters: cam.initPersProjWithoutDistortion(547, 542, 338, 234); diff --git a/example/tracking/mbtGenericTracking.cpp b/example/tracking/mbtGenericTracking.cpp index 5249b137da..fc69649f08 100644 --- a/example/tracking/mbtGenericTracking.cpp +++ b/example/tracking/mbtGenericTracking.cpp @@ -62,8 +62,6 @@ #define GETOPTARGS "x:m:i:n:de:chtfColwvpT:" -#define USE_XML 0 - void usage(const char *name, const char *badparam) { #if VISP_HAVE_DATASET_VERSION >= 0x030600 @@ -276,8 +274,8 @@ int main(int argc, const char **argv) // Read the command line options if (!getOptions(argc, argv, opt_ipath, opt_configFile, opt_modelFile, opt_initFile, opt_lastFrame, displayFeatures, - opt_click_allowed, opt_display, cao3DModel, trackCylinder, useOgre, showOgreConfigDialog, - useScanline, computeCovariance, projectionError, trackerType)) { + opt_click_allowed, opt_display, cao3DModel, trackCylinder, useOgre, showOgreConfigDialog, + useScanline, computeCovariance, projectionError, trackerType)) { return EXIT_FAILURE; } @@ -299,7 +297,7 @@ int main(int argc, const char **argv) else ipath = vpIoTools::createFilePath(env_ipath, "mbt/cube/image%04d." + ext); -#if USE_XML +#if defined(VISP_HAVE_PUGIXML) std::string configFile; if (!opt_configFile.empty()) configFile = opt_configFile; @@ -411,7 +409,7 @@ int main(int argc, const char **argv) vpCameraParameters cam1, cam2; // Initialise the tracker: camera parameters, moving edge and KLT settings -#if USE_XML +#if defined(VISP_HAVE_PUGIXML) // From the xml file dynamic_cast(tracker)->loadConfigFile(configFile, configFile); #else @@ -541,7 +539,7 @@ int main(int argc, const char **argv) } tracker->resetTracker(); -#if USE_XML +#if defined(VISP_HAVE_PUGIXML) dynamic_cast(tracker)->loadConfigFile(configFile, configFile); #else // By setting the parameters: diff --git a/example/tracking/mbtGenericTracking2.cpp b/example/tracking/mbtGenericTracking2.cpp index 357dc9cf56..184e2d4ef9 100644 --- a/example/tracking/mbtGenericTracking2.cpp +++ b/example/tracking/mbtGenericTracking2.cpp @@ -62,8 +62,6 @@ #define GETOPTARGS "x:m:i:n:de:chtfColwvpT:" -#define USE_XML 0 - void usage(const char *name, const char *badparam) { #if VISP_HAVE_DATASET_VERSION >= 0x030600 @@ -276,8 +274,8 @@ int main(int argc, const char **argv) // Read the command line options if (!getOptions(argc, argv, opt_ipath, opt_configFile, opt_modelFile, opt_initFile, opt_lastFrame, displayFeatures, - opt_click_allowed, opt_display, cao3DModel, trackCylinder, useOgre, showOgreConfigDialog, - useScanline, computeCovariance, projectionError, trackerType)) { + opt_click_allowed, opt_display, cao3DModel, trackCylinder, useOgre, showOgreConfigDialog, + useScanline, computeCovariance, projectionError, trackerType)) { return EXIT_FAILURE; } @@ -299,7 +297,7 @@ int main(int argc, const char **argv) else ipath = vpIoTools::createFilePath(env_ipath, "mbt/cube/image%04d." + ext); -#if USE_XML +#if defined(VISP_HAVE_PUGIXML) std::string configFile; if (!opt_configFile.empty()) configFile = opt_configFile; @@ -425,7 +423,7 @@ int main(int argc, const char **argv) std::map mapOfCameraParams; // Initialise the tracker: camera parameters, moving edge and KLT settings -#if USE_XML +#if defined(VISP_HAVE_PUGIXML) // From the xml file std::map mapOfConfigFiles; mapOfConfigFiles["Camera1"] = configFile; @@ -592,7 +590,7 @@ int main(int argc, const char **argv) } tracker->resetTracker(); -#if USE_XML +#if defined(VISP_HAVE_PUGIXML) dynamic_cast(tracker)->loadConfigFile(mapOfConfigFiles); #else // By setting the parameters: diff --git a/example/tracking/mbtGenericTrackingDepth.cpp b/example/tracking/mbtGenericTrackingDepth.cpp index bad1cf3f8c..87d4f67d69 100644 --- a/example/tracking/mbtGenericTrackingDepth.cpp +++ b/example/tracking/mbtGenericTrackingDepth.cpp @@ -62,7 +62,6 @@ #define GETOPTARGS "x:X:m:M:i:n:dchfolwvpt:T:e:" -#define USE_XML 1 #define USE_SMALL_DATASET 1 // small depth dataset in ViSP-images namespace @@ -356,17 +355,17 @@ bool read_data(unsigned int cpt, const std::string &input_directory, vpImage(tracker)->loadConfigFile(configFile, configFile_depth); #else diff --git a/example/tracking/mbtGenericTrackingDepthOnly.cpp b/example/tracking/mbtGenericTrackingDepthOnly.cpp index 36228e84ca..b05d7e54c6 100644 --- a/example/tracking/mbtGenericTrackingDepthOnly.cpp +++ b/example/tracking/mbtGenericTrackingDepthOnly.cpp @@ -62,7 +62,6 @@ #define GETOPTARGS "X:M:i:n:dchfolwvpT:e:u:" -#define USE_XML 1 #define USE_SMALL_DATASET 1 // small depth dataset in ViSP-images namespace @@ -222,7 +221,8 @@ bool getOptions(int argc, const char **argv, std::string &ipath, std::string &co return true; } -struct vpRealsenseIntrinsics_t { +struct vpRealsenseIntrinsics_t +{ float ppx; /**< Horizontal coordinate of the principal point of the image, as a pixel offset from the left edge */ float ppy; /**< Vertical coordinate of the principal point of the image, as @@ -325,7 +325,7 @@ bool read_data(unsigned int cpt, const std::string &input_directory, vpImage(tracker)->loadConfigFile(configFile_depth); #else @@ -448,9 +448,9 @@ int main(int argc, const char **argv) usage(argv[0], nullptr); std::cerr << std::endl << "ERROR:" << std::endl; std::cerr << " Use -i option or set VISP_INPUT_IMAGE_PATH " << std::endl - << " environment variable to specify the location of the " << std::endl - << " image path where test images are located." << std::endl - << std::endl; + << " environment variable to specify the location of the " << std::endl + << " image path where test images are located." << std::endl + << std::endl; return EXIT_FAILURE; } @@ -469,26 +469,26 @@ int main(int argc, const char **argv) configFile_depth = opt_configFile_depth; else configFile_depth = - vpIoTools::createFilePath(!opt_ipath.empty() ? opt_ipath : env_ipath, "mbt-depth/castel/chateau_depth.xml"); + vpIoTools::createFilePath(!opt_ipath.empty() ? opt_ipath : env_ipath, "mbt-depth/castel/chateau_depth.xml"); std::string modelFile_depth; if (!opt_modelFile_depth.empty()) modelFile_depth = opt_modelFile_depth; else modelFile_depth = - vpIoTools::createFilePath(!opt_ipath.empty() ? opt_ipath : env_ipath, "mbt-depth/castel/chateau.cao"); + vpIoTools::createFilePath(!opt_ipath.empty() ? opt_ipath : env_ipath, "mbt-depth/castel/chateau.cao"); std::string vrml_ext = ".wrl"; bool use_vrml = - (modelFile_depth.compare(modelFile_depth.length() - vrml_ext.length(), vrml_ext.length(), vrml_ext) == 0); + (modelFile_depth.compare(modelFile_depth.length() - vrml_ext.length(), vrml_ext.length(), vrml_ext) == 0); if (use_vrml) { #if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION == 2 || COIN_MAJOR_VERSION == 3 || COIN_MAJOR_VERSION == 4) std::cout << "use_vrml: " << use_vrml << std::endl; #else std::cerr << "Error: vrml model file is only supported if ViSP is " - "build with Coin3D 3rd party" - << std::endl; + "build with Coin3D 3rd party" + << std::endl; return EXIT_FAILURE; #endif } @@ -572,7 +572,7 @@ int main(int argc, const char **argv) cam_color.initPersProjWithoutDistortion(615.1674804688, 615.1675415039, 312.1889953613, 243.4373779297); vpHomogeneousMatrix depth_M_color; std::string depth_M_color_filename = - vpIoTools::createFilePath(!opt_ipath.empty() ? opt_ipath : env_ipath, "mbt-depth/castel/depth_M_color.txt"); + vpIoTools::createFilePath(!opt_ipath.empty() ? opt_ipath : env_ipath, "mbt-depth/castel/depth_M_color.txt"); { std::ifstream depth_M_color_file(depth_M_color_filename.c_str()); depth_M_color.load(depth_M_color_file); @@ -596,7 +596,8 @@ int main(int argc, const char **argv) dynamic_cast(tracker)->getPose(cMo); // display the 3D model at the given pose dynamic_cast(tracker)->display(I_depth, cMo, cam, vpColor::red); - } else { + } + else { vpHomogeneousMatrix cMoi(0.04431452054, 0.09294637757, 0.3357760654, -2.677922443, 0.121297639, -0.6028463357); dynamic_cast(tracker)->initFromPose(I_depth, cMoi); } @@ -757,12 +758,12 @@ int main(int argc, const char **argv) } frame_index++; - } + } std::cout << "\nFinal poses, cMo:\n" << cMo << std::endl; std::cout << "\nComputation time, Mean: " << vpMath::getMean(time_vec) - << " ms ; Median: " << vpMath::getMedian(time_vec) << " ms ; Std: " << vpMath::getStdev(time_vec) << " ms" - << std::endl; + << " ms ; Median: " << vpMath::getMedian(time_vec) << " ms ; Std: " << vpMath::getStdev(time_vec) << " ms" + << std::endl; if (opt_click_allowed && !quit) { vpDisplay::getClick(I_depth); @@ -772,11 +773,12 @@ int main(int argc, const char **argv) tracker = nullptr; return EXIT_SUCCESS; - } catch (const vpException &e) { + } + catch (const vpException &e) { std::cout << "Catch an exception: " << e << std::endl; return EXIT_FAILURE; } -} + } #elif !(defined(VISP_HAVE_MODULE_MBT) && defined(VISP_HAVE_DISPLAY)) int main() diff --git a/example/tracking/mbtKltTracking.cpp b/example/tracking/mbtKltTracking.cpp index 8c6ba3f3d7..d9f1cf4070 100644 --- a/example/tracking/mbtKltTracking.cpp +++ b/example/tracking/mbtKltTracking.cpp @@ -261,9 +261,9 @@ int main(int argc, const char **argv) usage(argv[0], nullptr); std::cerr << std::endl << "ERROR:" << std::endl; std::cerr << " Use -i option or set VISP_INPUT_IMAGE_PATH " << std::endl - << " environment variable to specify the location of the " << std::endl - << " image path where test images are located." << std::endl - << std::endl; + << " environment variable to specify the location of the " << std::endl + << " image path where test images are located." << std::endl + << std::endl; return EXIT_FAILURE; } @@ -283,14 +283,16 @@ int main(int argc, const char **argv) if (!opt_modelFile.empty()) { modelFile = opt_modelFile; - } else { + } + else { std::string modelFileCao = "mbt/cube.cao"; std::string modelFileWrl = "mbt/cube.wrl"; if (!opt_ipath.empty()) { if (cao3DModel) { modelFile = vpIoTools::createFilePath(opt_ipath, modelFileCao); - } else { + } + else { #ifdef VISP_HAVE_COIN3D modelFile = vpIoTools::createFilePath(opt_ipath, modelFileWrl); #else @@ -298,10 +300,12 @@ int main(int argc, const char **argv) modelFile = vpIoTools::createFilePath(opt_ipath, modelFileCao); #endif } - } else { + } + else { if (cao3DModel) { modelFile = vpIoTools::createFilePath(env_ipath, modelFileCao); - } else { + } + else { #ifdef VISP_HAVE_COIN3D modelFile = vpIoTools::createFilePath(env_ipath, modelFileWrl); #else @@ -325,7 +329,8 @@ int main(int argc, const char **argv) reader.setFileName(ipath); try { reader.open(I); - } catch (...) { + } + catch (...) { std::cout << "Cannot open sequence: " << ipath << std::endl; return EXIT_FAILURE; } @@ -362,9 +367,11 @@ int main(int argc, const char **argv) // Load tracker config file (camera parameters and moving edge settings) vpCameraParameters cam; + +#if defined(VISP_HAVE_PUGIXML) // From the xml file tracker.loadConfigFile(configFile); -#if 0 +#else // Corresponding parameters manually set to have an example code // By setting the parameters: cam.initPersProjWithoutDistortion(547, 542, 338, 234); @@ -433,7 +440,8 @@ int main(int argc, const char **argv) tracker.getPose(cMo); // display the 3D model at the given pose tracker.display(I, cMo, cam, vpColor::red); - } else { + } + else { vpHomogeneousMatrix cMoi(0.02044769891, 0.1101505452, 0.5078963719, 2.063603907, 1.110231561, -0.4392789872); tracker.initFromPose(I, cMoi); } @@ -458,9 +466,11 @@ int main(int argc, const char **argv) if (opt_display) vpDisplay::display(I); tracker.resetTracker(); + +#if defined(VISP_HAVE_PUGIXML) tracker.loadConfigFile(configFile); -#if 0 - // Corresponding parameters manually set to have an example code +#else + // Corresponding parameters manually set to have an example code // By setting the parameters: cam.initPersProjWithoutDistortion(547, 542, 338, 234); @@ -550,7 +560,8 @@ int main(int argc, const char **argv) reader.close(); return EXIT_SUCCESS; - } catch (const vpException &e) { + } + catch (const vpException &e) { std::cout << "Catch an exception: " << e << std::endl; return EXIT_FAILURE; } @@ -561,8 +572,8 @@ int main(int argc, const char **argv) int main() { std::cout << "visp_mbt, visp_gui modules and OpenCV are required to run " - "this example." - << std::endl; + "this example." + << std::endl; return EXIT_SUCCESS; } diff --git a/modules/core/CMakeLists.txt b/modules/core/CMakeLists.txt index d464b4d544..2d766d6700 100644 --- a/modules/core/CMakeLists.txt +++ b/modules/core/CMakeLists.txt @@ -38,6 +38,12 @@ # Add optional 3rd parties set(opt_incs "") set(opt_libs "") +set(opt_libs_private "") + +if(WITH_LAPACK) + # lapack is private + set(opt_libs_private ${LAPACK_LIBRARIES}) +endif() # Add library ws2_32.a or ws2_32.lib for vpNetwork class if(WS2_32_FOUND) @@ -193,7 +199,7 @@ if(USE_YARP) add_definitions(${YARP_DEFINES}) endif(USE_YARP) -# Math: eigen3, gsl, lapack, OpenCV +# Math: eigen3, gsl, mkl, openblas, atlas, netlib, OpenCV if(USE_EIGEN3) if(EIGEN3_INCLUDE_DIRS) list(APPEND opt_incs ${EIGEN3_INCLUDE_DIRS}) @@ -201,22 +207,27 @@ if(USE_EIGEN3) list(APPEND opt_incs ${EIGEN3_INCLUDE_DIR}) endif() endif() + if(USE_GSL) list(APPEND opt_incs ${GSL_INCLUDE_DIRS}) list(APPEND opt_libs ${GSL_LIBRARIES}) endif() + if(USE_MKL) list(APPEND opt_incs ${MKL_INCLUDE_DIRS}) list(APPEND opt_libs ${MKL_LIBRARIES}) endif() + if(USE_OPENBLAS) list(APPEND opt_incs ${OpenBLAS_INCLUDE_DIR}) list(APPEND opt_libs ${OpenBLAS_LIBRARIES}) endif() + if(USE_ATLAS) list(APPEND opt_incs ${Atlas_INCLUDE_DIR}) list(APPEND opt_libs ${Atlas_LIBRARIES}) endif() + if(USE_NETLIB) list(APPEND opt_libs ${NETLIB_LIBRARIES}) endif() @@ -226,15 +237,18 @@ if(USE_XML2) list(APPEND opt_incs ${XML2_INCLUDE_DIRS}) list(APPEND opt_libs ${XML2_LIBRARIES}) endif() + if(USE_THREADS) if(CMAKE_THREAD_LIBS_INIT) list(APPEND opt_libs "${CMAKE_THREAD_LIBS_INIT}") endif() endif() + if(USE_ZLIB) list(APPEND opt_incs ${ZLIB_INCLUDE_DIRS}) list(APPEND opt_libs ${ZLIB_LIBRARIES}) endif() + if(USE_OPENMP) list(APPEND opt_incs ${OpenMP_CXX_INCLUDE_DIRS}) # Because there is an explicit link to libpthread location that breaks visp conda package usage on linux @@ -251,23 +265,28 @@ if(USE_OPENMP) endif() endforeach() endif() + if(USE_NLOHMANN_JSON) get_target_property(_inc_dirs "nlohmann_json::nlohmann_json" INTERFACE_INCLUDE_DIRECTORIES) list(APPEND opt_incs ${_inc_dirs}) endif() -# pugixml is always enabled to provide default XML I/O capabilities -# pugixml is private -include_directories(${PUGIXML_INCLUDE_DIRS}) +if(WITH_PUGIXML) + # pugixml is private and provides default XML I/O capabilities + include_directories(${PUGIXML_INCLUDE_DIRS}) + list(APPEND opt_libs_private ${PUGIXML_LIBRARIES}) +endif() if(WITH_CATCH2) # catch2 is private include_directories(${CATCH2_INCLUDE_DIRS}) endif() -# simdlib is always enabled since it contains fallback code to plain C++ code -# Simd lib is private -include_directories(${SIMDLIB_INCLUDE_DIRS}) +if(WITH_SIMDLIB) + # Simd lib is private + include_directories(${SIMDLIB_INCLUDE_DIRS}) + list(APPEND opt_libs_private ${SIMDLIB_LIBRARIES}) +endif() if(MSVC) # Disable Visual C++ C4996 warning @@ -369,7 +388,7 @@ if(USE_XML2) endif() endif() -vp_add_module(core PRIVATE_OPTIONAL ${LAPACK_LIBRARIES} ${PUGIXML_LIBRARIES} ${SIMDLIB_LIBRARIES} WRAP java) +vp_add_module(core PRIVATE_OPTIONAL ${opt_libs_private} WRAP java) #----------------------------------------------------------------------------- # Enable large file support diff --git a/modules/core/include/visp3/core/vpCPUFeatures.h b/modules/core/include/visp3/core/vpCPUFeatures.h index 1fe8d37c2a..53878cf6b4 100644 --- a/modules/core/include/visp3/core/vpCPUFeatures.h +++ b/modules/core/include/visp3/core/vpCPUFeatures.h @@ -68,10 +68,13 @@ VISP_EXPORT bool checkSSE41(); VISP_EXPORT bool checkSSE42(); VISP_EXPORT bool checkAVX(); VISP_EXPORT bool checkAVX2(); + +#if defined(VISP_HAVE_SIMDLIB) VISP_EXPORT bool checkNeon(); VISP_EXPORT size_t getCPUCacheL1(); VISP_EXPORT size_t getCPUCacheL2(); VISP_EXPORT size_t getCPUCacheL3(); +#endif VISP_EXPORT void printCPUInfo(); } // namespace vpCPUFeatures diff --git a/modules/core/include/visp3/core/vpGaussianFilter.h b/modules/core/include/visp3/core/vpGaussianFilter.h index 5795ea4530..a35f8e8f9b 100644 --- a/modules/core/include/visp3/core/vpGaussianFilter.h +++ b/modules/core/include/visp3/core/vpGaussianFilter.h @@ -41,6 +41,8 @@ #include +#if defined(VISP_HAVE_SIMDLIB) + /*! \class vpGaussianFilter @@ -69,3 +71,4 @@ class VISP_EXPORT vpGaussianFilter }; #endif +#endif diff --git a/modules/core/include/visp3/core/vpImageTools.h b/modules/core/include/visp3/core/vpImageTools.h index 068795c12b..864d9f6e4e 100644 --- a/modules/core/include/visp3/core/vpImageTools.h +++ b/modules/core/include/visp3/core/vpImageTools.h @@ -58,7 +58,7 @@ #include #include -#if defined _OPENMP +#if defined(_OPENMP) #include #endif @@ -73,7 +73,8 @@ class VISP_EXPORT vpImageTools { public: - enum vpImageInterpolationType { + enum vpImageInterpolationType + { INTERPOLATION_NEAREST, /*!< Nearest neighbor interpolation. */ INTERPOLATION_LINEAR, /*!< Bi-linear interpolation (optimized by SIMD lib if enabled). */ INTERPOLATION_CUBIC, /*!< Bi-cubic interpolation. */ @@ -210,10 +211,12 @@ class VISP_EXPORT vpImageTools static void resizeNearest(const vpImage &I, vpImage &Ires, unsigned int i, unsigned int j, float u, float v); +#if defined(VISP_HAVE_SIMDLIB) static void resizeSimdlib(const vpImage &Isrc, unsigned int resizeWidth, unsigned int resizeHeight, vpImage &Idst, int method); static void resizeSimdlib(const vpImage &Isrc, unsigned int resizeWidth, unsigned int resizeHeight, vpImage &Idst, int method); +#endif template static void warpNN(const vpImage &src, const vpMatrix &T, vpImage &dst, bool affine, bool centerCorner, @@ -320,13 +323,15 @@ void vpImageTools::crop(const vpImage &I, double roi_top, double roi_left, void *dst = (void *)crop[i]; memcpy(dst, src, r_width * sizeof(Type)); } - } else if (h_scale == 1) { + } + else if (h_scale == 1) { for (unsigned int i = 0; i < r_height; i++) { void *src = (void *)(I[(i + i_min_u) * v_scale] + j_min_u); void *dst = (void *)crop[i]; memcpy(dst, src, r_width * sizeof(Type)); } - } else { + } + else { for (unsigned int i = 0; i < r_height; i++) { for (unsigned int j = 0; j < r_width; j++) { crop[i][j] = I[(i + i_min_u) * v_scale][(j + j_min_u) * h_scale]; @@ -423,13 +428,15 @@ void vpImageTools::crop(const unsigned char *bitmap, unsigned int width, unsigne void *dst = (void *)crop[i]; memcpy(dst, src, r_width * sizeof(Type)); } - } else if (h_scale == 1) { + } + else if (h_scale == 1) { for (unsigned int i = 0; i < r_height; i++) { void *src = (void *)(bitmap + ((i + i_min_u) * width * v_scale + j_min_u) * sizeof(Type)); void *dst = (void *)crop[i]; memcpy(dst, src, r_width * sizeof(Type)); } - } else { + } + else { for (unsigned int i = 0; i < r_height; i++) { unsigned int i_src = (i + i_min_u) * width * v_scale + j_min_u * h_scale; for (unsigned int j = 0; j < r_width; j++) { @@ -495,7 +502,8 @@ inline void vpImageTools::binarise(vpImage &I, unsigned char thre } I.performLut(lut); - } else { + } + else { unsigned char *p = I.bitmap; unsigned char *pend = I.bitmap + I.getWidth() * I.getHeight(); for (; p < pend; p++) { @@ -525,7 +533,7 @@ template class vpUndistortInternalType unsigned int threadid; public: - vpUndistortInternalType() : src(nullptr), dst(nullptr), width(0), height(0), cam(), nthreads(0), threadid(0) {} + vpUndistortInternalType() : src(nullptr), dst(nullptr), width(0), height(0), cam(), nthreads(0), threadid(0) { } vpUndistortInternalType(const vpUndistortInternalType &u) { *this = u; } vpUndistortInternalType &operator=(const vpUndistortInternalType &u) @@ -590,8 +598,8 @@ template void *vpUndistortInternalType::vpUndistort_threaded( u_round = -1; if (v_round < 0.f) v_round = -1; - double du_double = (u_double) - (double)u_round; - double dv_double = (v_double) - (double)v_round; + double du_double = (u_double)-(double)u_round; + double dv_double = (v_double)-(double)v_round; Type v01; Type v23; if ((0 <= u_round) && (0 <= v_round) && (u_round < ((width)-1)) && (v_round < ((height)-1))) { @@ -601,7 +609,8 @@ template void *vpUndistortInternalType::vpUndistort_threaded( _mp += width; v23 = (Type)(_mp[0] + ((_mp[1] - _mp[0]) * du_double)); *dst = (Type)(v01 + ((v23 - v01) * dv_double)); - } else { + } + else { *dst = 0; } dst++; @@ -745,8 +754,8 @@ void vpImageTools::undistort(const vpImage &I, const vpCameraParameters &c u_round = -1; if (v_round < 0.f) v_round = -1; - double du_double = (u_double) - (double)u_round; - double dv_double = (v_double) - (double)v_round; + double du_double = (u_double)-(double)u_round; + double dv_double = (v_double)-(double)v_round; Type v01; Type v23; if ((0 <= u_round) && (0 <= v_round) && (u_round < (((int)width) - 1)) && (v_round < (((int)height) - 1))) { @@ -757,7 +766,8 @@ void vpImageTools::undistort(const vpImage &I, const vpCameraParameters &c v23 = (Type)(_mp[0] + ((_mp[1] - _mp[0]) * du_double)); *dst = (Type)(v01 + ((v23 - v01) * dv_double)); // printf("R %d G %d B %d\n", dst->R, dst->G, dst->B); - } else { + } + else { *dst = 0; } dst++; @@ -770,7 +780,7 @@ void vpImageTools::undistort(const vpImage &I, const vpCameraParameters &c int width = I.getWidth(); int height = I.getHeight(); - undistI.resize(height,width); + undistI.resize(height, width); double u0 = cam.get_u0(); double v0 = cam.get_v0(); @@ -784,13 +794,13 @@ void vpImageTools::undistort(const vpImage &I, const vpCameraParameters &c return; } - for(int v = 0 ; v < height; v++){ - for(int u = 0; u < height; u++){ + for (int v = 0; v < height; v++) { + for (int u = 0; u < height; u++) { double r2 = vpMath::sqr(((double)u - u0)/px) + - vpMath::sqr(((double)v-v0)/py); + vpMath::sqr(((double)v-v0)/py); double u_double = ((double)u - u0)*(1.0+kd*r2) + u0; double v_double = ((double)v - v0)*(1.0+kd*r2) + v0; - undistI[v][u] = I.getPixelBI((float)u_double,(float)v_double); + undistI[v][u] = I.getPixelBI((float)u_double, (float)v_double); } } #endif @@ -1081,7 +1091,7 @@ void vpImageTools::resize(const vpImage &I, vpImage &Ires, unsigned template void vpImageTools::resize(const vpImage &I, vpImage &Ires, const vpImageInterpolationType &method, unsigned int -#if defined _OPENMP +#if defined(_OPENMP) nThreads #endif ) @@ -1100,7 +1110,7 @@ void vpImageTools::resize(const vpImage &I, vpImage &Ires, const vpI const float scaleX = I.getWidth() / static_cast(Ires.getWidth()); const float half = 0.5f; -#if defined _OPENMP +#if defined(_OPENMP) if (nThreads > 0) { omp_set_num_threads(static_cast(nThreads)); } @@ -1118,20 +1128,23 @@ void vpImageTools::resize(const vpImage &I, vpImage &Ires, const vpI if (method == INTERPOLATION_NEAREST) { resizeNearest(I, Ires, static_cast(i), j, u, v); - } else if (method == INTERPOLATION_LINEAR) { + } + else if (method == INTERPOLATION_LINEAR) { resizeBilinear(I, Ires, static_cast(i), j, u0, v0, xFrac, yFrac); - } else if (method == INTERPOLATION_CUBIC) { + } + else if (method == INTERPOLATION_CUBIC) { resizeBicubic(I, Ires, static_cast(i), j, u, v, xFrac, yFrac); } } } } +#if defined(VISP_HAVE_SIMDLIB) template <> inline void vpImageTools::resize(const vpImage &I, vpImage &Ires, const vpImageInterpolationType &method, unsigned int -#if defined _OPENMP +#if defined(_OPENMP) nThreads #endif ) @@ -1143,14 +1156,16 @@ inline void vpImageTools::resize(const vpImage &I, vpImage(Ires.getHeight()); const float scaleX = I.getWidth() / static_cast(Ires.getWidth()); const float half = 0.5f; -#if defined _OPENMP +#if defined(_OPENMP) if (nThreads > 0) { omp_set_num_threads(static_cast(nThreads)); } @@ -1166,7 +1181,8 @@ inline void vpImageTools::resize(const vpImage &I, vpImage(i), j, u, v); - } else if (method == INTERPOLATION_CUBIC) { + } + else if (method == INTERPOLATION_CUBIC) { resizeBicubic(I, Ires, static_cast(i), j, u, v, xFrac, yFrac); } } @@ -1178,7 +1194,7 @@ template <> inline void vpImageTools::resize(const vpImage &I, vpImage &Ires, const vpImageInterpolationType &method, unsigned int -#if defined _OPENMP +#if defined(_OPENMP) nThreads #endif ) @@ -1190,14 +1206,16 @@ inline void vpImageTools::resize(const vpImage &I, vpImage &Ires if (method == INTERPOLATION_AREA) { resizeSimdlib(I, Ires.getWidth(), Ires.getHeight(), Ires, INTERPOLATION_AREA); - } else if (method == INTERPOLATION_LINEAR) { + } + else if (method == INTERPOLATION_LINEAR) { resizeSimdlib(I, Ires.getWidth(), Ires.getHeight(), Ires, INTERPOLATION_LINEAR); - } else { + } + else { const float scaleY = I.getHeight() / static_cast(Ires.getHeight()); const float scaleX = I.getWidth() / static_cast(Ires.getWidth()); const float half = 0.5f; -#if defined _OPENMP +#if defined(_OPENMP) if (nThreads > 0) { omp_set_num_threads(static_cast(nThreads)); } @@ -1213,13 +1231,15 @@ inline void vpImageTools::resize(const vpImage &I, vpImage &Ires if (method == INTERPOLATION_NEAREST) { resizeNearest(I, Ires, static_cast(i), j, u, v); - } else if (method == INTERPOLATION_CUBIC) { + } + else if (method == INTERPOLATION_CUBIC) { resizeBicubic(I, Ires, static_cast(i), j, u, v, xFrac, yFrac); } } } } } +#endif /*! Apply a warping (affine or perspective) transformation to an image. @@ -1268,21 +1288,23 @@ void vpImageTools::warpImage(const vpImage &src, const vpMatrix &T, vpImag double b2 = -M[1][0] * M[0][2] - M[1][1] * M[1][2]; M[0][2] = b1; M[1][2] = b2; - } else { + } + else { M = T.inverseByLU(); } if (fixedPointArithmetic && !pixelCenter) { fixedPointArithmetic = checkFixedPoint(0, 0, M, affine) && checkFixedPoint(dst.getWidth() - 1, 0, M, affine) && - checkFixedPoint(0, dst.getHeight() - 1, M, affine) && - checkFixedPoint(dst.getWidth() - 1, dst.getHeight() - 1, M, affine); + checkFixedPoint(0, dst.getHeight() - 1, M, affine) && + checkFixedPoint(dst.getWidth() - 1, dst.getHeight() - 1, M, affine); } if (interp_NN) { // nearest neighbor interpolation warpNN(src, M, dst, affine, pixelCenter, fixedPointArithmetic); - } else { - // bilinear interpolation + } + else { + // bilinear interpolation warpLinear(src, M, dst, affine, pixelCenter, fixedPointArithmetic); } } @@ -1331,7 +1353,8 @@ void vpImageTools::warpNN(const vpImage &src, const vpMatrix &T, vpImage &src, const vpMatrix &T, vpImage &src, const vpMatrix &T, vpIma const Type val10 = src[y_ + 1][x_]; const Type val11 = src[y_ + 1][x_ + 1]; const int64_t interp_i64 = - static_cast(s_1 * t_1 * val00 + s * t_1 * val01 + s_1 * t * val10 + s * t * val11); + static_cast(s_1 * t_1 * val00 + s * t_1 * val01 + s_1 * t * val10 + s * t * val11); const float interp = (interp_i64 >> (nbits * 2)) + (interp_i64 & 0xFFFFFFFF) * precision_2; dst[i][j] = vpMath::saturate(interp); - } else if (y_ < static_cast(src.getHeight()) - 1) { + } + else if (y_ < static_cast(src.getHeight()) - 1) { const Type val00 = src[y_][x_]; const Type val10 = src[y_ + 1][x_]; const int64_t interp_i64 = static_cast(t_1 * val00 + t * val10); const float interp = (interp_i64 >> nbits) + (interp_i64 & 0xFFFF) * precision_1; dst[i][j] = vpMath::saturate(interp); - } else if (x_ < static_cast(src.getWidth()) - 1) { + } + else if (x_ < static_cast(src.getWidth()) - 1) { const Type val00 = src[y_][x_]; const Type val01 = src[y_][x_ + 1]; const int64_t interp_i64 = static_cast(s_1 * val00 + s * val01); const float interp = (interp_i64 >> nbits) + (interp_i64 & 0xFFFF) * precision_1; dst[i][j] = vpMath::saturate(interp); - } else { + } + else { dst[i][j] = src[y_][x_]; } } @@ -1467,7 +1494,8 @@ void vpImageTools::warpLinear(const vpImage &src, const vpMatrix &T, vpIma a2_i64 += a1_i64; a5_i64 += a4_i64; } - } else { + } + else { for (unsigned int i = 0; i < dst.getHeight(); i++) { int64_t xi = a2_i64; int64_t yi = a5_i64; @@ -1495,17 +1523,20 @@ void vpImageTools::warpLinear(const vpImage &src, const vpMatrix &T, vpIma const float col1 = lerp(val10, val11, s); const float interp = lerp(col0, col1, t); dst[i][j] = vpMath::saturate(interp); - } else if (y_ < static_cast(src.getHeight()) - 1) { + } + else if (y_ < static_cast(src.getHeight()) - 1) { const Type val00 = src[y_][x_]; const Type val10 = src[y_ + 1][x_]; const float interp = lerp(val00, val10, t); dst[i][j] = vpMath::saturate(interp); - } else if (x_ < static_cast(src.getWidth()) - 1) { + } + else if (x_ < static_cast(src.getWidth()) - 1) { const Type val00 = src[y_][x_]; const Type val01 = src[y_][x_ + 1]; const float interp = lerp(val00, val01, s); dst[i][j] = vpMath::saturate(interp); - } else { + } + else { dst[i][j] = src[y_][x_]; } } @@ -1520,7 +1551,8 @@ void vpImageTools::warpLinear(const vpImage &src, const vpMatrix &T, vpIma a8_i64 += a7_i64; } } - } else { + } + else { double a0 = T[0][0]; double a1 = T[0][1]; double a2 = T[0][2]; @@ -1563,17 +1595,20 @@ void vpImageTools::warpLinear(const vpImage &src, const vpMatrix &T, vpIma const double col1 = lerp(val10, val11, s); const double interp = lerp(col0, col1, t); dst[i][j] = vpMath::saturate(interp); - } else if (y_lower < static_cast(src.getHeight()) - 1) { + } + else if (y_lower < static_cast(src.getHeight()) - 1) { const Type val00 = src[y_lower][x_lower]; const Type val10 = src[y_lower + 1][x_lower]; const double interp = lerp(val00, val10, t); dst[i][j] = vpMath::saturate(interp); - } else if (x_lower < static_cast(src.getWidth()) - 1) { + } + else if (x_lower < static_cast(src.getWidth()) - 1) { const Type val00 = src[y_lower][x_lower]; const Type val01 = src[y_lower][x_lower + 1]; const double interp = lerp(val00, val01, s); dst[i][j] = vpMath::saturate(interp); - } else { + } + else { dst[i][j] = src[y_lower][x_lower]; } } @@ -1629,20 +1664,21 @@ inline void vpImageTools::warpLinear(const vpImage &src, const vpMatrix const vpRGBa val10 = src[y_ + 1][x_]; const vpRGBa val11 = src[y_ + 1][x_ + 1]; const int64_t interpR_i64 = - static_cast(s_1 * t_1 * val00.R + s * t_1 * val01.R + s_1 * t * val10.R + s * t * val11.R); + static_cast(s_1 * t_1 * val00.R + s * t_1 * val01.R + s_1 * t * val10.R + s * t * val11.R); const float interpR = (interpR_i64 >> (nbits * 2)) + (interpR_i64 & 0xFFFFFFFF) * precision_2; const int64_t interpG_i64 = - static_cast(s_1 * t_1 * val00.G + s * t_1 * val01.G + s_1 * t * val10.G + s * t * val11.G); + static_cast(s_1 * t_1 * val00.G + s * t_1 * val01.G + s_1 * t * val10.G + s * t * val11.G); const float interpG = (interpG_i64 >> (nbits * 2)) + (interpG_i64 & 0xFFFFFFFF) * precision_2; const int64_t interpB_i64 = - static_cast(s_1 * t_1 * val00.B + s * t_1 * val01.B + s_1 * t * val10.B + s * t * val11.B); + static_cast(s_1 * t_1 * val00.B + s * t_1 * val01.B + s_1 * t * val10.B + s * t * val11.B); const float interpB = (interpB_i64 >> (nbits * 2)) + (interpB_i64 & 0xFFFFFFFF) * precision_2; dst[i][j] = vpRGBa(vpMath::saturate(interpR), vpMath::saturate(interpG), vpMath::saturate(interpB), 255); - } else if (y_ < static_cast(src.getHeight()) - 1) { + } + else if (y_ < static_cast(src.getHeight()) - 1) { const vpRGBa val00 = src[y_][x_]; const vpRGBa val10 = src[y_ + 1][x_]; const int64_t interpR_i64 = static_cast(t_1 * val00.R + t * val10.R); @@ -1656,7 +1692,8 @@ inline void vpImageTools::warpLinear(const vpImage &src, const vpMatrix dst[i][j] = vpRGBa(vpMath::saturate(interpR), vpMath::saturate(interpG), vpMath::saturate(interpB), 255); - } else if (x_ < static_cast(src.getWidth()) - 1) { + } + else if (x_ < static_cast(src.getWidth()) - 1) { const vpRGBa val00 = src[y_][x_]; const vpRGBa val01 = src[y_][x_ + 1]; const int64_t interpR_i64 = static_cast(s_1 * val00.R + s * val01.R); @@ -1670,7 +1707,8 @@ inline void vpImageTools::warpLinear(const vpImage &src, const vpMatrix dst[i][j] = vpRGBa(vpMath::saturate(interpR), vpMath::saturate(interpG), vpMath::saturate(interpB), 255); - } else { + } + else { dst[i][j] = src[y_][x_]; } } @@ -1682,7 +1720,8 @@ inline void vpImageTools::warpLinear(const vpImage &src, const vpMatrix a2_i64 += a1_i64; a5_i64 += a4_i64; } - } else { + } + else { for (unsigned int i = 0; i < dst.getHeight(); i++) { int64_t xi = a2_i64; int64_t yi = a5_i64; @@ -1720,7 +1759,8 @@ inline void vpImageTools::warpLinear(const vpImage &src, const vpMatrix dst[i][j] = vpRGBa(vpMath::saturate(interpR), vpMath::saturate(interpG), vpMath::saturate(interpB), 255); - } else if (y_ < static_cast(src.getHeight()) - 1) { + } + else if (y_ < static_cast(src.getHeight()) - 1) { const vpRGBa val00 = src[y_][x_]; const vpRGBa val10 = src[y_ + 1][x_]; const float interpR = lerp(val00.R, val10.R, t); @@ -1729,7 +1769,8 @@ inline void vpImageTools::warpLinear(const vpImage &src, const vpMatrix dst[i][j] = vpRGBa(vpMath::saturate(interpR), vpMath::saturate(interpG), vpMath::saturate(interpB), 255); - } else if (x_ < static_cast(src.getWidth()) - 1) { + } + else if (x_ < static_cast(src.getWidth()) - 1) { const vpRGBa val00 = src[y_][x_]; const vpRGBa val01 = src[y_][x_ + 1]; const float interpR = lerp(val00.R, val01.R, s); @@ -1738,7 +1779,8 @@ inline void vpImageTools::warpLinear(const vpImage &src, const vpMatrix dst[i][j] = vpRGBa(vpMath::saturate(interpR), vpMath::saturate(interpG), vpMath::saturate(interpB), 255); - } else { + } + else { dst[i][j] = src[y_][x_]; } } @@ -1753,7 +1795,8 @@ inline void vpImageTools::warpLinear(const vpImage &src, const vpMatrix a8_i64 += a7_i64; } } - } else { + } + else { double a0 = T[0][0]; double a1 = T[0][1]; double a2 = T[0][2]; @@ -1803,7 +1846,8 @@ inline void vpImageTools::warpLinear(const vpImage &src, const vpMatrix dst[i][j] = vpRGBa(vpMath::saturate(interpR), vpMath::saturate(interpG), vpMath::saturate(interpB), 255); - } else if (y_lower < static_cast(src.getHeight()) - 1) { + } + else if (y_lower < static_cast(src.getHeight()) - 1) { const vpRGBa val00 = src[y_lower][x_lower]; const vpRGBa val10 = src[y_lower + 1][x_lower]; const double interpR = lerp(val00.R, val10.R, t); @@ -1812,7 +1856,8 @@ inline void vpImageTools::warpLinear(const vpImage &src, const vpMatrix dst[i][j] = vpRGBa(vpMath::saturate(interpR), vpMath::saturate(interpG), vpMath::saturate(interpB), 255); - } else if (x_lower < static_cast(src.getWidth()) - 1) { + } + else if (x_lower < static_cast(src.getWidth()) - 1) { const vpRGBa val00 = src[y_lower][x_lower]; const vpRGBa val01 = src[y_lower][x_lower + 1]; const double interpR = lerp(val00.R, val01.R, s); @@ -1821,7 +1866,8 @@ inline void vpImageTools::warpLinear(const vpImage &src, const vpMatrix dst[i][j] = vpRGBa(vpMath::saturate(interpR), vpMath::saturate(interpG), vpMath::saturate(interpB), 255); - } else { + } + else { dst[i][j] = src[y_lower][x_lower]; } } diff --git a/modules/core/include/visp3/core/vpXmlParserCamera.h b/modules/core/include/visp3/core/vpXmlParserCamera.h index 838a5f378d..565f739b9e 100644 --- a/modules/core/include/visp3/core/vpXmlParserCamera.h +++ b/modules/core/include/visp3/core/vpXmlParserCamera.h @@ -43,6 +43,7 @@ #include +#if defined(VISP_HAVE_PUGIXML) #include /*! @@ -198,3 +199,4 @@ class VISP_EXPORT vpXmlParserCamera Impl *m_impl; }; #endif +#endif diff --git a/modules/core/include/visp3/core/vpXmlParserHomogeneousMatrix.h b/modules/core/include/visp3/core/vpXmlParserHomogeneousMatrix.h index 567c8fb631..d8243259ee 100644 --- a/modules/core/include/visp3/core/vpXmlParserHomogeneousMatrix.h +++ b/modules/core/include/visp3/core/vpXmlParserHomogeneousMatrix.h @@ -44,6 +44,7 @@ #include +#if defined(VISP_HAVE_PUGIXML) #include /*! @@ -171,3 +172,4 @@ class VISP_EXPORT vpXmlParserHomogeneousMatrix Impl *m_impl; }; #endif +#endif diff --git a/modules/core/include/visp3/core/vpXmlParserRectOriented.h b/modules/core/include/visp3/core/vpXmlParserRectOriented.h index ce1073efe4..52dcff8e69 100644 --- a/modules/core/include/visp3/core/vpXmlParserRectOriented.h +++ b/modules/core/include/visp3/core/vpXmlParserRectOriented.h @@ -42,6 +42,7 @@ #include +#if defined(VISP_HAVE_PUGIXML) #include /*! @@ -107,4 +108,5 @@ class VISP_EXPORT vpXmlParserRectOriented class Impl; Impl *m_impl; }; +#endif #endif // vpXmlParserRectOriented_h diff --git a/modules/core/src/camera/vpXmlParserCamera.cpp b/modules/core/src/camera/vpXmlParserCamera.cpp index 642b012881..034bb0ec17 100644 --- a/modules/core/src/camera/vpXmlParserCamera.cpp +++ b/modules/core/src/camera/vpXmlParserCamera.cpp @@ -41,6 +41,7 @@ #include +#if defined(VISP_HAVE_PUGIXML) #include #include @@ -1226,3 +1227,9 @@ void vpXmlParserCamera::setSubsampling_width(unsigned int subsampling) { m_impl- void vpXmlParserCamera::setSubsampling_height(unsigned int subsampling) { m_impl->setSubsampling_height(subsampling); } void vpXmlParserCamera::setWidth(unsigned int width) { m_impl->setWidth(width); } + +#elif !defined(VISP_BUILD_SHARED_LIBS) +// Work around to avoid warning: libvisp_core.a(vpXmlParserCamera.cpp.o) has no symbols +void dummy_vpXmlParserCamera() { }; + +#endif diff --git a/modules/core/src/image/private/vpBayerConversion.h b/modules/core/src/image/private/vpBayerConversion.h index 773bdbe7d3..af9dd9e410 100644 --- a/modules/core/src/image/private/vpBayerConversion.h +++ b/modules/core/src/image/private/vpBayerConversion.h @@ -145,7 +145,8 @@ void demosaicBGGRToRGBaBilinearTpl(const T *bggr, T *rgba, unsigned int width, u rgba[j * 4 + 0] = static_cast(0.5f * bggr[width + j - 1] + 0.5f * bggr[width + j + 1]); rgba[j * 4 + 1] = static_cast(0.5f * bggr[j - 1] + 0.5f * bggr[j + 1]); rgba[j * 4 + 2] = bggr[j]; - } else { + } + else { rgba[j * 4 + 0] = bggr[width + j]; rgba[j * 4 + 1] = bggr[j]; rgba[j * 4 + 2] = static_cast(0.5f * bggr[j - 1] + 0.5f * bggr[j + 1]); @@ -158,7 +159,8 @@ void demosaicBGGRToRGBaBilinearTpl(const T *bggr, T *rgba, unsigned int width, u rgba[i * width * 4 + 0] = static_cast(0.5f * bggr[(i - 1) * width + 1] + 0.5f * bggr[(i + 1) * width + 1]); rgba[i * width * 4 + 1] = bggr[i * width + 1]; rgba[i * width * 4 + 2] = bggr[i * width]; - } else { + } + else { rgba[i * width * 4 + 0] = bggr[i * width + 1]; rgba[i * width * 4 + 1] = bggr[i * width]; rgba[i * width * 4 + 2] = static_cast(0.5f * bggr[(i - 1) * width] + 0.5f * bggr[(i + 1) * width]); @@ -169,14 +171,15 @@ void demosaicBGGRToRGBaBilinearTpl(const T *bggr, T *rgba, unsigned int width, u for (unsigned int i = 1; i < height - 1; i++) { if (i % 2 == 0) { rgba[(i * width + width - 1) * 4 + 0] = - static_cast(0.5f * bggr[i * width - 1] + 0.5f * bggr[(i + 2) * width - 1]); + static_cast(0.5f * bggr[i * width - 1] + 0.5f * bggr[(i + 2) * width - 1]); rgba[(i * width + width - 1) * 4 + 1] = bggr[(i + 1) * width - 1]; rgba[(i * width + width - 1) * 4 + 2] = bggr[(i + 1) * width - 2]; - } else { + } + else { rgba[(i * width + width - 1) * 4 + 0] = bggr[(i + 1) * width - 1]; rgba[(i * width + width - 1) * 4 + 1] = bggr[(i + 1) * width - 2]; rgba[(i * width + width - 1) * 4 + 2] = - static_cast(0.5f * bggr[i * width - 2] + 0.5f * bggr[(i + 2) * width - 2]); + static_cast(0.5f * bggr[i * width - 2] + 0.5f * bggr[(i + 2) * width - 2]); } } @@ -184,19 +187,20 @@ void demosaicBGGRToRGBaBilinearTpl(const T *bggr, T *rgba, unsigned int width, u for (unsigned int j = 1; j < width - 1; j++) { if (j % 2 == 0) { rgba[((height - 1) * width + j) * 4 + 0] = - static_cast(0.5f * bggr[(height - 1) * width + j - 1] + 0.5f * bggr[(height - 1) * width + j + 1]); + static_cast(0.5f * bggr[(height - 1) * width + j - 1] + 0.5f * bggr[(height - 1) * width + j + 1]); rgba[((height - 1) * width + j) * 4 + 1] = bggr[(height - 1) * width + j]; rgba[((height - 1) * width + j) * 4 + 2] = bggr[(height - 2) * width + j]; - } else { + } + else { rgba[((height - 1) * width + j) * 4 + 0] = bggr[(height - 1) * width + j]; rgba[((height - 1) * width + j) * 4 + 1] = - static_cast(0.5f * bggr[(height - 1) * width + j - 1] + 0.5f * bggr[(height - 1) * width + j + 1]); + static_cast(0.5f * bggr[(height - 1) * width + j - 1] + 0.5f * bggr[(height - 1) * width + j + 1]); rgba[((height - 1) * width + j) * 4 + 2] = - static_cast(0.5f * bggr[(height - 2) * width + j - 1] + 0.5f * bggr[(height - 2) * width + j + 1]); + static_cast(0.5f * bggr[(height - 2) * width + j - 1] + 0.5f * bggr[(height - 2) * width + j + 1]); } } -#if defined _OPENMP && _OPENMP >= 200711 // OpenMP 3.1 +#if defined(_OPENMP) && (_OPENMP >= 200711) // OpenMP 3.1 if (nThreads > 0) { omp_set_num_threads(static_cast(nThreads)); } @@ -210,15 +214,18 @@ void demosaicBGGRToRGBaBilinearTpl(const T *bggr, T *rgba, unsigned int width, u rgba[(i * width + j) * 4 + 0] = demosaicCheckerBilinear(bggr, width, i, j); rgba[(i * width + j) * 4 + 1] = demosaicCrossBilinear(bggr, width, i, j); rgba[(i * width + j) * 4 + 2] = bggr[i * width + j]; - } else if (i % 2 == 0 && j % 2 != 0) { + } + else if (i % 2 == 0 && j % 2 != 0) { rgba[(i * width + j) * 4 + 0] = demosaicPhiBilinear(bggr, width, i, j); rgba[(i * width + j) * 4 + 1] = bggr[i * width + j]; rgba[(i * width + j) * 4 + 2] = demosaicThetaBilinear(bggr, width, i, j); - } else if (i % 2 != 0 && j % 2 == 0) { + } + else if (i % 2 != 0 && j % 2 == 0) { rgba[(i * width + j) * 4 + 0] = demosaicThetaBilinear(bggr, width, i, j); rgba[(i * width + j) * 4 + 1] = bggr[i * width + j]; rgba[(i * width + j) * 4 + 2] = demosaicPhiBilinear(bggr, width, i, j); - } else { + } + else { rgba[(i * width + j) * 4 + 0] = bggr[i * width + j]; rgba[(i * width + j) * 4 + 1] = demosaicCrossBilinear(bggr, width, i, j); rgba[(i * width + j) * 4 + 2] = demosaicCheckerBilinear(bggr, width, i, j); @@ -262,7 +269,8 @@ void demosaicGBRGToRGBaBilinearTpl(const T *gbrg, T *rgba, unsigned int width, u rgba[j * 4 + 0] = gbrg[width + j]; rgba[j * 4 + 1] = gbrg[j]; rgba[j * 4 + 2] = static_cast(0.5f * gbrg[j - 1] + 0.5f * gbrg[j + 1]); - } else { + } + else { rgba[j * 4 + 0] = static_cast(0.5f * gbrg[width + j - 1] + 0.5f * gbrg[width + j + 1]); rgba[j * 4 + 1] = static_cast(0.5f * gbrg[j - 1] + 0.5f * gbrg[j + 1]); rgba[j * 4 + 2] = gbrg[j]; @@ -275,7 +283,8 @@ void demosaicGBRGToRGBaBilinearTpl(const T *gbrg, T *rgba, unsigned int width, u rgba[i * width * 4 + 0] = static_cast(0.5f * gbrg[(i - 1) * width] + 0.5f * gbrg[(i + 1) * width]); rgba[i * width * 4 + 1] = gbrg[i * width]; rgba[i * width * 4 + 2] = gbrg[i * width + 1]; - } else { + } + else { rgba[i * width * 4 + 0] = gbrg[i * width]; rgba[i * width * 4 + 1] = static_cast(0.5f * gbrg[(i - 1) * width] + 0.5f * gbrg[(i + 1) * width]); rgba[i * width * 4 + 2] = static_cast(0.5f * gbrg[(i - 1) * width + 1] + 0.5f * gbrg[(i + 1) * width + 1]); @@ -286,14 +295,15 @@ void demosaicGBRGToRGBaBilinearTpl(const T *gbrg, T *rgba, unsigned int width, u for (unsigned int i = 1; i < height - 1; i++) { if (i % 2 == 0) { rgba[(i * width + width - 1) * 4 + 0] = - static_cast(0.5f * gbrg[i * width - 2] + 0.5f * gbrg[(i + 2) * width - 2]); + static_cast(0.5f * gbrg[i * width - 2] + 0.5f * gbrg[(i + 2) * width - 2]); rgba[(i * width + width - 1) * 4 + 1] = gbrg[(i + 1) * width - 2]; rgba[(i * width + width - 1) * 4 + 2] = gbrg[(i + 1) * width - 1]; - } else { + } + else { rgba[(i * width + width - 1) * 4 + 0] = gbrg[(i + 1) * width - 2]; rgba[(i * width + width - 1) * 4 + 1] = gbrg[(i + 1) * width - 1]; rgba[(i * width + width - 1) * 4 + 2] = - static_cast(0.5f * gbrg[i * width - 1] + 0.5f * gbrg[(i + 2) * width - 1]); + static_cast(0.5f * gbrg[i * width - 1] + 0.5f * gbrg[(i + 2) * width - 1]); } } @@ -302,18 +312,19 @@ void demosaicGBRGToRGBaBilinearTpl(const T *gbrg, T *rgba, unsigned int width, u if (j % 2 == 0) { rgba[((height - 1) * width + j) * 4 + 0] = gbrg[(height - 1) * width + j]; rgba[((height - 1) * width + j) * 4 + 1] = - static_cast(0.5f * gbrg[(height - 1) * width + j - 1] + 0.5f * gbrg[(height - 1) * width + j + 1]); + static_cast(0.5f * gbrg[(height - 1) * width + j - 1] + 0.5f * gbrg[(height - 1) * width + j + 1]); rgba[((height - 1) * width + j) * 4 + 2] = - static_cast(0.5f * gbrg[(height - 2) * width + j - 1] + 0.5f * gbrg[(height - 2) * width + j + 1]); - } else { + static_cast(0.5f * gbrg[(height - 2) * width + j - 1] + 0.5f * gbrg[(height - 2) * width + j + 1]); + } + else { rgba[((height - 1) * width + j) * 4 + 0] = - static_cast(0.5f * gbrg[(height - 1) * width + j - 1] + 0.5f * gbrg[(height - 1) * width + j + 1]); + static_cast(0.5f * gbrg[(height - 1) * width + j - 1] + 0.5f * gbrg[(height - 1) * width + j + 1]); rgba[((height - 1) * width + j) * 4 + 1] = gbrg[(height - 1) * width + j]; rgba[((height - 1) * width + j) * 4 + 2] = gbrg[(height - 2) * width + j]; } } -#if defined _OPENMP && _OPENMP >= 200711 // OpenMP 3.1 +#if defined(_OPENMP) && (_OPENMP >= 200711) // OpenMP 3.1 if (nThreads > 0) { omp_set_num_threads(static_cast(nThreads)); } @@ -327,15 +338,18 @@ void demosaicGBRGToRGBaBilinearTpl(const T *gbrg, T *rgba, unsigned int width, u rgba[(i * width + j) * 4 + 0] = demosaicPhiBilinear(gbrg, width, i, j); rgba[(i * width + j) * 4 + 1] = gbrg[i * width + j]; rgba[(i * width + j) * 4 + 2] = demosaicThetaBilinear(gbrg, width, i, j); - } else if (i % 2 == 0 && j % 2 != 0) { + } + else if (i % 2 == 0 && j % 2 != 0) { rgba[(i * width + j) * 4 + 0] = demosaicCheckerBilinear(gbrg, width, i, j); rgba[(i * width + j) * 4 + 1] = demosaicCrossBilinear(gbrg, width, i, j); rgba[(i * width + j) * 4 + 2] = gbrg[i * width + j]; - } else if (i % 2 != 0 && j % 2 == 0) { + } + else if (i % 2 != 0 && j % 2 == 0) { rgba[(i * width + j) * 4 + 0] = gbrg[i * width + j]; rgba[(i * width + j) * 4 + 1] = demosaicCrossBilinear(gbrg, width, i, j); rgba[(i * width + j) * 4 + 2] = demosaicCheckerBilinear(gbrg, width, i, j); - } else { + } + else { rgba[(i * width + j) * 4 + 0] = demosaicThetaBilinear(gbrg, width, i, j); rgba[(i * width + j) * 4 + 1] = gbrg[i * width + j]; rgba[(i * width + j) * 4 + 2] = demosaicPhiBilinear(gbrg, width, i, j); @@ -379,7 +393,8 @@ void demosaicGRBGToRGBaBilinearTpl(const T *grbg, T *rgba, unsigned int width, u rgba[j * 4 + 0] = static_cast(0.5f * grbg[j - 1] + 0.5f * grbg[j + 1]); rgba[j * 4 + 1] = grbg[j]; rgba[j * 4 + 2] = grbg[width + j]; - } else { + } + else { rgba[j * 4 + 0] = grbg[j]; rgba[j * 4 + 1] = static_cast(0.5f * grbg[j - 1] + 0.5f * grbg[j + 1]); rgba[j * 4 + 2] = static_cast(0.5f * grbg[width + j - 1] + 0.5f * grbg[width + j + 1]); @@ -392,7 +407,8 @@ void demosaicGRBGToRGBaBilinearTpl(const T *grbg, T *rgba, unsigned int width, u rgba[i * width * 4 + 0] = grbg[i * width + 1]; rgba[i * width * 4 + 1] = grbg[i * width]; rgba[i * width * 4 + 2] = static_cast(0.5f * grbg[(i - 1) * width] + 0.5f * grbg[(i + 1) * width]); - } else { + } + else { rgba[i * width * 4 + 0] = static_cast(0.5f * grbg[(i - 1) * width + 1] + 0.5f * grbg[(i + 1) * width + 1]); rgba[i * width * 4 + 1] = grbg[i * width + 1]; rgba[i * width * 4 + 2] = grbg[i * width]; @@ -405,10 +421,11 @@ void demosaicGRBGToRGBaBilinearTpl(const T *grbg, T *rgba, unsigned int width, u rgba[(i * width + width - 1) * 4 + 0] = grbg[(i + 1) * width - 1]; rgba[(i * width + width - 1) * 4 + 1] = grbg[(i + 1) * width - 2]; rgba[(i * width + width - 1) * 4 + 2] = - static_cast(0.5f * grbg[i * width - 2] + 0.5f * grbg[(i + 2) * width - 2]); - } else { + static_cast(0.5f * grbg[i * width - 2] + 0.5f * grbg[(i + 2) * width - 2]); + } + else { rgba[(i * width + width - 1) * 4 + 0] = - static_cast(0.5f * grbg[i * width - 1] + 0.5f * grbg[(i + 2) * width - 1]); + static_cast(0.5f * grbg[i * width - 1] + 0.5f * grbg[(i + 2) * width - 1]); rgba[(i * width + width - 1) * 4 + 1] = grbg[(i + 1) * width - 1]; rgba[(i * width + width - 1) * 4 + 2] = grbg[(i + 1) * width - 2]; } @@ -418,19 +435,20 @@ void demosaicGRBGToRGBaBilinearTpl(const T *grbg, T *rgba, unsigned int width, u for (unsigned int j = 1; j < width - 1; j++) { if (j % 2 == 0) { rgba[((height - 1) * width + j) * 4 + 0] = - static_cast(0.5f * grbg[(height - 2) * width + j - 1] + 0.5f * grbg[(height - 2) * width + j + 1]); + static_cast(0.5f * grbg[(height - 2) * width + j - 1] + 0.5f * grbg[(height - 2) * width + j + 1]); rgba[((height - 1) * width + j) * 4 + 1] = - static_cast(0.5f * grbg[(height - 1) * width + j - 1] + 0.5f * grbg[(height - 1) * width + j + 1]); + static_cast(0.5f * grbg[(height - 1) * width + j - 1] + 0.5f * grbg[(height - 1) * width + j + 1]); rgba[((height - 1) * width + j) * 4 + 2] = grbg[(height - 1) * width + j]; - } else { + } + else { rgba[((height - 1) * width + j) * 4 + 0] = grbg[(height - 2) * width + j]; rgba[((height - 1) * width + j) * 4 + 1] = grbg[(height - 1) * width + j]; rgba[((height - 1) * width + j) * 4 + 2] = - static_cast(0.5f * grbg[(height - 1) * width + j - 1] + 0.5f * grbg[(height - 1) * width + j + 1]); + static_cast(0.5f * grbg[(height - 1) * width + j - 1] + 0.5f * grbg[(height - 1) * width + j + 1]); } } -#if defined _OPENMP && _OPENMP >= 200711 // OpenMP 3.1 +#if defined(_OPENMP) && (_OPENMP >= 200711) // OpenMP 3.1 if (nThreads > 0) { omp_set_num_threads(static_cast(nThreads)); } @@ -444,15 +462,18 @@ void demosaicGRBGToRGBaBilinearTpl(const T *grbg, T *rgba, unsigned int width, u rgba[(i * width + j) * 4 + 0] = demosaicThetaBilinear(grbg, width, i, j); rgba[(i * width + j) * 4 + 1] = grbg[i * width + j]; rgba[(i * width + j) * 4 + 2] = demosaicPhiBilinear(grbg, width, i, j); - } else if (i % 2 == 0 && j % 2 != 0) { + } + else if (i % 2 == 0 && j % 2 != 0) { rgba[(i * width + j) * 4 + 0] = grbg[i * width + j]; rgba[(i * width + j) * 4 + 1] = demosaicCrossBilinear(grbg, width, i, j); rgba[(i * width + j) * 4 + 2] = demosaicCheckerBilinear(grbg, width, i, j); - } else if (i % 2 != 0 && j % 2 == 0) { + } + else if (i % 2 != 0 && j % 2 == 0) { rgba[(i * width + j) * 4 + 0] = demosaicCheckerBilinear(grbg, width, i, j); rgba[(i * width + j) * 4 + 1] = demosaicCrossBilinear(grbg, width, i, j); rgba[(i * width + j) * 4 + 2] = grbg[i * width + j]; - } else { + } + else { rgba[(i * width + j) * 4 + 0] = demosaicPhiBilinear(grbg, width, i, j); rgba[(i * width + j) * 4 + 1] = grbg[i * width + j]; rgba[(i * width + j) * 4 + 2] = demosaicThetaBilinear(grbg, width, i, j); @@ -496,7 +517,8 @@ void demosaicRGGBToRGBaBilinearTpl(const T *rggb, T *rgba, unsigned int width, u rgba[j * 4 + 0] = rggb[j]; rgba[j * 4 + 1] = static_cast(0.5f * rggb[j - 1] + 0.5f * rggb[j + 1]); rgba[j * 4 + 2] = static_cast(0.5f * rggb[width + j - 1] + 0.5f * rggb[width + j + 1]); - } else { + } + else { rgba[j * 4 + 0] = static_cast(0.5f * rggb[j - 1] + 0.5f * rggb[j + 1]); rgba[j * 4 + 1] = rggb[j]; rgba[j * 4 + 2] = rggb[width + j]; @@ -509,7 +531,8 @@ void demosaicRGGBToRGBaBilinearTpl(const T *rggb, T *rgba, unsigned int width, u rgba[i * width * 4 + 0] = rggb[i * width]; rgba[i * width * 4 + 1] = rggb[i * width + 1]; rgba[i * width * 4 + 2] = static_cast(0.5f * rggb[(i - 1) * width + 1] + 0.5f * rggb[(i + 1) * width + 1]); - } else { + } + else { rgba[i * width * 4 + 0] = static_cast(0.5f * rggb[(i - 1) * width] + 0.5f * rggb[(i + 1) * width]); rgba[i * width * 4 + 1] = rggb[i * width]; rgba[i * width * 4 + 2] = rggb[i * width + 1]; @@ -522,10 +545,11 @@ void demosaicRGGBToRGBaBilinearTpl(const T *rggb, T *rgba, unsigned int width, u rgba[(i * width + width - 1) * 4 + 0] = rggb[(i + 1) * width - 2]; rgba[(i * width + width - 1) * 4 + 1] = rggb[(i + 1) * width - 1]; rgba[(i * width + width - 1) * 4 + 2] = - static_cast(0.5f * rggb[i * width - 1] + 0.5f * rggb[(i + 2) * width - 1]); - } else { + static_cast(0.5f * rggb[i * width - 1] + 0.5f * rggb[(i + 2) * width - 1]); + } + else { rgba[(i * width + width - 1) * 4 + 0] = - static_cast(0.5f * rggb[i * width - 2] + 0.5f * rggb[(i + 2) * width - 2]); + static_cast(0.5f * rggb[i * width - 2] + 0.5f * rggb[(i + 2) * width - 2]); rgba[(i * width + width - 1) * 4 + 1] = rggb[(i + 1) * width - 2]; rgba[(i * width + width - 1) * 4 + 2] = rggb[(i + 1) * width - 1]; } @@ -537,17 +561,18 @@ void demosaicRGGBToRGBaBilinearTpl(const T *rggb, T *rgba, unsigned int width, u rgba[((height - 1) * width + j) * 4 + 0] = rggb[(height - 2) * width + j]; rgba[((height - 1) * width + j) * 4 + 1] = rggb[(height - 1) * width + j]; rgba[((height - 1) * width + j) * 4 + 2] = - static_cast(0.5f * rggb[(height - 1) * width + j - 1] + 0.5f * rggb[(height - 1) * width + j + 1]); - } else { + static_cast(0.5f * rggb[(height - 1) * width + j - 1] + 0.5f * rggb[(height - 1) * width + j + 1]); + } + else { rgba[((height - 1) * width + j) * 4 + 0] = - static_cast(0.5f * rggb[(height - 2) * width + j - 1] + 0.5f * rggb[(height - 2) * width + j + 1]); + static_cast(0.5f * rggb[(height - 2) * width + j - 1] + 0.5f * rggb[(height - 2) * width + j + 1]); rgba[((height - 1) * width + j) * 4 + 1] = - static_cast(0.5f * rggb[(height - 1) * width + j - 1] + 0.5f * rggb[(height - 1) * width + j + 1]); + static_cast(0.5f * rggb[(height - 1) * width + j - 1] + 0.5f * rggb[(height - 1) * width + j + 1]); rgba[((height - 1) * width + j) * 4 + 2] = rggb[(height - 1) * width + j]; } } -#if defined _OPENMP && _OPENMP >= 200711 // OpenMP 3.1 +#if defined(_OPENMP) && (_OPENMP >= 200711) // OpenMP 3.1 if (nThreads > 0) { omp_set_num_threads(static_cast(nThreads)); } @@ -561,15 +586,18 @@ void demosaicRGGBToRGBaBilinearTpl(const T *rggb, T *rgba, unsigned int width, u rgba[(i * width + j) * 4 + 0] = rggb[i * width + j]; rgba[(i * width + j) * 4 + 1] = demosaicCrossBilinear(rggb, width, i, j); rgba[(i * width + j) * 4 + 2] = demosaicCheckerBilinear(rggb, width, i, j); - } else if (i % 2 == 0 && j % 2 != 0) { + } + else if (i % 2 == 0 && j % 2 != 0) { rgba[(i * width + j) * 4 + 0] = demosaicThetaBilinear(rggb, width, i, j); rgba[(i * width + j) * 4 + 1] = rggb[i * width + j]; rgba[(i * width + j) * 4 + 2] = demosaicPhiBilinear(rggb, width, i, j); - } else if (i % 2 != 0 && j % 2 == 0) { + } + else if (i % 2 != 0 && j % 2 == 0) { rgba[(i * width + j) * 4 + 0] = demosaicPhiBilinear(rggb, width, i, j); rgba[(i * width + j) * 4 + 1] = rggb[i * width + j]; rgba[(i * width + j) * 4 + 2] = demosaicThetaBilinear(rggb, width, i, j); - } else { + } + else { rgba[(i * width + j) * 4 + 0] = demosaicCheckerBilinear(rggb, width, i, j); rgba[(i * width + j) * 4 + 1] = demosaicCrossBilinear(rggb, width, i, j); rgba[(i * width + j) * 4 + 2] = rggb[i * width + j]; @@ -614,7 +642,8 @@ void demosaicBGGRToRGBaMalvarTpl(const T *bggr, T *rgba, unsigned int width, uns rgba[j * 4 + 0] = static_cast(0.5f * bggr[width + j - 1] + 0.5f * bggr[width + j + 1]); rgba[j * 4 + 1] = static_cast(0.5f * bggr[j - 1] + 0.5f * bggr[j + 1]); rgba[j * 4 + 2] = bggr[j]; - } else { + } + else { rgba[j * 4 + 0] = bggr[width + j]; rgba[j * 4 + 1] = bggr[j]; rgba[j * 4 + 2] = static_cast(0.5f * bggr[j - 1] + 0.5f * bggr[j + 1]); @@ -627,7 +656,8 @@ void demosaicBGGRToRGBaMalvarTpl(const T *bggr, T *rgba, unsigned int width, uns rgba[(width + j) * 4 + 0] = static_cast(0.5f * bggr[width + j - 1] + 0.5f * bggr[width + j + 1]); rgba[(width + j) * 4 + 1] = bggr[width + j]; rgba[(width + j) * 4 + 2] = static_cast(0.5f * bggr[j] + 0.5f * bggr[2 * width + j]); - } else { + } + else { rgba[(width + j) * 4 + 0] = bggr[width + j]; rgba[(width + j) * 4 + 1] = static_cast(0.25f * bggr[j] + 0.25f * bggr[width + j - 1] + 0.25f * bggr[width + j + 1] + 0.25f * bggr[2 * width + j]); @@ -642,7 +672,8 @@ void demosaicBGGRToRGBaMalvarTpl(const T *bggr, T *rgba, unsigned int width, uns rgba[i * width * 4 + 0] = static_cast(0.5f * bggr[(i - 1) * width + 1] + 0.5f * bggr[(i + 1) * width + 1]); rgba[i * width * 4 + 1] = bggr[i * width + 1]; rgba[i * width * 4 + 2] = bggr[i * width]; - } else { + } + else { rgba[i * width * 4 + 0] = bggr[i * width + 1]; rgba[i * width * 4 + 1] = bggr[i * width]; rgba[i * width * 4 + 2] = static_cast(0.5f * bggr[(i - 1) * width] + 0.5f * bggr[(i + 1) * width]); @@ -653,10 +684,11 @@ void demosaicBGGRToRGBaMalvarTpl(const T *bggr, T *rgba, unsigned int width, uns for (unsigned int i = 1; i < height - 1; i++) { if (i % 2 == 0) { rgba[(i * width + 1) * 4 + 0] = - static_cast(0.5f * bggr[(i - 1) * width + 1] + 0.5f * bggr[(i + 1) * width + 1]); + static_cast(0.5f * bggr[(i - 1) * width + 1] + 0.5f * bggr[(i + 1) * width + 1]); rgba[(i * width + 1) * 4 + 1] = bggr[i * width + 1]; rgba[(i * width + 1) * 4 + 2] = static_cast(0.5f * bggr[i * width] + 0.5f * bggr[i * width + 2]); - } else { + } + else { rgba[(i * width + 1) * 4 + 0] = bggr[i * width + 1]; rgba[(i * width + 1) * 4 + 1] = static_cast(0.25f * bggr[(i - 1) * width + 1] + 0.25f * bggr[i * width] + 0.25f * bggr[i * width + 2] + 0.25f * bggr[(i + 1) * width + 1]); @@ -669,18 +701,19 @@ void demosaicBGGRToRGBaMalvarTpl(const T *bggr, T *rgba, unsigned int width, uns for (unsigned int i = 1; i < height - 1; i++) { if (i % 2 == 0) { rgba[(i * width + width - 2) * 4 + 0] = - static_cast(0.25f * bggr[i * width - 3] + 0.25f * bggr[i * width - 1] + 0.25f * bggr[(i + 2) * width - 3] + - 0.25f * bggr[(i + 2) * width - 1]); + static_cast(0.25f * bggr[i * width - 3] + 0.25f * bggr[i * width - 1] + 0.25f * bggr[(i + 2) * width - 3] + + 0.25f * bggr[(i + 2) * width - 1]); rgba[(i * width + width - 2) * 4 + 1] = - static_cast(0.25f * bggr[i * width - 2] + 0.25f * bggr[(i + 1) * width - 3] + - 0.25f * bggr[(i + 1) * width - 1] + 0.25f * bggr[(i + 2) * width - 2]); + static_cast(0.25f * bggr[i * width - 2] + 0.25f * bggr[(i + 1) * width - 3] + + 0.25f * bggr[(i + 1) * width - 1] + 0.25f * bggr[(i + 2) * width - 2]); rgba[(i * width + width - 2) * 4 + 2] = bggr[(i + 1) * width - 2]; - } else { + } + else { rgba[(i * width + width - 2) * 4 + 0] = - static_cast(0.5f * bggr[(i + 1) * width - 3] + 0.5f * bggr[(i + 1) * width - 1]); + static_cast(0.5f * bggr[(i + 1) * width - 3] + 0.5f * bggr[(i + 1) * width - 1]); rgba[(i * width + width - 2) * 4 + 1] = bggr[(i + 1) * width - 2]; rgba[(i * width + width - 2) * 4 + 2] = - static_cast(0.5f * bggr[i * width - 2] + 0.5f * bggr[(i + 2) * width - 2]); + static_cast(0.5f * bggr[i * width - 2] + 0.5f * bggr[(i + 2) * width - 2]); } } @@ -688,14 +721,15 @@ void demosaicBGGRToRGBaMalvarTpl(const T *bggr, T *rgba, unsigned int width, uns for (unsigned int i = 1; i < height - 1; i++) { if (i % 2 == 0) { rgba[(i * width + width - 1) * 4 + 0] = - static_cast(0.5f * bggr[i * width - 1] + 0.5f * bggr[(i + 2) * width - 1]); + static_cast(0.5f * bggr[i * width - 1] + 0.5f * bggr[(i + 2) * width - 1]); rgba[(i * width + width - 1) * 4 + 1] = bggr[(i + 1) * width - 1]; rgba[(i * width + width - 1) * 4 + 2] = bggr[(i + 1) * width - 2]; - } else { + } + else { rgba[(i * width + width - 1) * 4 + 0] = bggr[(i + 1) * width - 1]; rgba[(i * width + width - 1) * 4 + 1] = bggr[(i + 1) * width - 2]; rgba[(i * width + width - 1) * 4 + 2] = - static_cast(0.5f * bggr[i * width - 2] + 0.5f * bggr[(i + 2) * width - 2]); + static_cast(0.5f * bggr[i * width - 2] + 0.5f * bggr[(i + 2) * width - 2]); } } @@ -703,17 +737,18 @@ void demosaicBGGRToRGBaMalvarTpl(const T *bggr, T *rgba, unsigned int width, uns for (unsigned int j = 1; j < width - 1; j++) { if (j % 2 == 0) { rgba[((height - 2) * width + j) * 4 + 0] = - static_cast(0.25f * bggr[(height - 3) * width + j - 1] + 0.25f * bggr[(height - 3) * width + j + 1] + - 0.25f * bggr[(height - 1) * width + j - 1] + 0.25f * bggr[(height - 1) * width + j + 1]); + static_cast(0.25f * bggr[(height - 3) * width + j - 1] + 0.25f * bggr[(height - 3) * width + j + 1] + + 0.25f * bggr[(height - 1) * width + j - 1] + 0.25f * bggr[(height - 1) * width + j + 1]); rgba[((height - 2) * width + j) * 4 + 1] = - static_cast(0.5f * bggr[(height - 2) * width + j - 1] + 0.5f * bggr[(height - 2) * width + j + 1]); + static_cast(0.5f * bggr[(height - 2) * width + j - 1] + 0.5f * bggr[(height - 2) * width + j + 1]); rgba[((height - 2) * width + j) * 4 + 2] = bggr[(height - 2) * width + j]; - } else { + } + else { rgba[((height - 2) * width + j) * 4 + 0] = - static_cast(0.5f * bggr[(height - 3) * width + j] + 0.5f * bggr[(height - 1) * width + j]); + static_cast(0.5f * bggr[(height - 3) * width + j] + 0.5f * bggr[(height - 1) * width + j]); rgba[((height - 2) * width + j) * 4 + 1] = bggr[(height - 2) * width + j]; rgba[((height - 2) * width + j) * 4 + 2] = - static_cast(0.5f * bggr[(height - 2) * width + j - 1] + 0.5f * bggr[(height - 2) * width + j + 1]); + static_cast(0.5f * bggr[(height - 2) * width + j - 1] + 0.5f * bggr[(height - 2) * width + j + 1]); } } @@ -721,19 +756,20 @@ void demosaicBGGRToRGBaMalvarTpl(const T *bggr, T *rgba, unsigned int width, uns for (unsigned int j = 1; j < width - 1; j++) { if (j % 2 == 0) { rgba[((height - 1) * width + j) * 4 + 0] = - static_cast(0.5f * bggr[(height - 1) * width + j - 1] + 0.5f * bggr[(height - 1) * width + j + 1]); + static_cast(0.5f * bggr[(height - 1) * width + j - 1] + 0.5f * bggr[(height - 1) * width + j + 1]); rgba[((height - 1) * width + j) * 4 + 1] = bggr[(height - 1) * width + j]; rgba[((height - 1) * width + j) * 4 + 2] = bggr[(height - 2) * width + j]; - } else { + } + else { rgba[((height - 1) * width + j) * 4 + 0] = bggr[(height - 1) * width + j]; rgba[((height - 1) * width + j) * 4 + 1] = - static_cast(0.5f * bggr[(height - 1) * width + j - 1] + 0.5f * bggr[(height - 1) * width + j + 1]); + static_cast(0.5f * bggr[(height - 1) * width + j - 1] + 0.5f * bggr[(height - 1) * width + j + 1]); rgba[((height - 1) * width + j) * 4 + 2] = - static_cast(0.5f * bggr[(height - 2) * width + j - 1] + 0.5f * bggr[(height - 2) * width + j + 1]); + static_cast(0.5f * bggr[(height - 2) * width + j - 1] + 0.5f * bggr[(height - 2) * width + j + 1]); } } -#if defined _OPENMP && _OPENMP >= 200711 // OpenMP 3.1 +#if defined(_OPENMP) && (_OPENMP >= 200711) // OpenMP 3.1 if (nThreads > 0) { omp_set_num_threads(static_cast(nThreads)); } @@ -747,15 +783,18 @@ void demosaicBGGRToRGBaMalvarTpl(const T *bggr, T *rgba, unsigned int width, uns rgba[(i * width + j) * 4 + 0] = demosaicCheckerMalvar(bggr, width, i, j); rgba[(i * width + j) * 4 + 1] = demosaicCrossMalvar(bggr, width, i, j); rgba[(i * width + j) * 4 + 2] = bggr[i * width + j]; - } else if (i % 2 == 0 && j % 2 != 0) { + } + else if (i % 2 == 0 && j % 2 != 0) { rgba[(i * width + j) * 4 + 0] = demosaicPhiMalvar(bggr, width, i, j); rgba[(i * width + j) * 4 + 1] = bggr[i * width + j]; rgba[(i * width + j) * 4 + 2] = demosaicThetaMalvar(bggr, width, i, j); - } else if (i % 2 != 0 && j % 2 == 0) { + } + else if (i % 2 != 0 && j % 2 == 0) { rgba[(i * width + j) * 4 + 0] = demosaicThetaMalvar(bggr, width, i, j); rgba[(i * width + j) * 4 + 1] = bggr[i * width + j]; rgba[(i * width + j) * 4 + 2] = demosaicPhiMalvar(bggr, width, i, j); - } else { + } + else { rgba[(i * width + j) * 4 + 0] = bggr[i * width + j]; rgba[(i * width + j) * 4 + 1] = demosaicCrossMalvar(bggr, width, i, j); rgba[(i * width + j) * 4 + 2] = demosaicCheckerMalvar(bggr, width, i, j); @@ -798,7 +837,8 @@ void demosaicGBRGToRGBaMalvarTpl(const T *gbrg, T *rgba, unsigned int width, uns rgba[j * 4 + 0] = gbrg[width + j]; rgba[j * 4 + 1] = gbrg[j]; rgba[j * 4 + 2] = static_cast(0.5f * gbrg[j - 1] + 0.5f * gbrg[j + 1]); - } else { + } + else { rgba[j * 4 + 0] = static_cast(0.5f * gbrg[width + j - 1] + 0.5f * gbrg[width + j + 1]); rgba[j * 4 + 1] = static_cast(0.5f * gbrg[j - 1] + 0.5f * gbrg[j + 1]); rgba[j * 4 + 2] = gbrg[j]; @@ -813,7 +853,8 @@ void demosaicGBRGToRGBaMalvarTpl(const T *gbrg, T *rgba, unsigned int width, uns 0.25f * gbrg[width + j + 1] + 0.25f * gbrg[2 * width + j]); rgba[(width + j) * 4 + 2] = static_cast(0.25f * gbrg[j - 1] + 0.25f * gbrg[j + 1] + 0.25f * gbrg[2 * width + j - 1] + 0.25f * gbrg[2 * width + j + 1]); - } else { + } + else { rgba[(width + j) * 4 + 0] = static_cast(0.5f * gbrg[width + j - 1] + 0.5f * gbrg[width + j + 1]); rgba[(width + j) * 4 + 1] = gbrg[width + j]; rgba[(width + j) * 4 + 2] = static_cast(0.5f * gbrg[j] + 0.5f * gbrg[2 * width + j]); @@ -826,7 +867,8 @@ void demosaicGBRGToRGBaMalvarTpl(const T *gbrg, T *rgba, unsigned int width, uns rgba[i * width * 4 + 0] = static_cast(0.5f * gbrg[(i - 1) * width] + 0.5f * gbrg[(i + 1) * width]); rgba[i * width * 4 + 1] = gbrg[i * width]; rgba[i * width * 4 + 2] = gbrg[i * width + 1]; - } else { + } + else { rgba[i * width * 4 + 0] = gbrg[i * width]; rgba[i * width * 4 + 1] = static_cast(0.5f * gbrg[(i - 1) * width] + 0.5f * gbrg[(i + 1) * width]); rgba[i * width * 4 + 2] = static_cast(0.5f * gbrg[(i - 1) * width + 1] + 0.5f * gbrg[(i + 1) * width + 1]); @@ -841,11 +883,12 @@ void demosaicGBRGToRGBaMalvarTpl(const T *gbrg, T *rgba, unsigned int width, uns rgba[(i * width + 1) * 4 + 1] = static_cast(0.25f * gbrg[(i - 1) * width + 1] + 0.25f * gbrg[i * width] + 0.25f * gbrg[i * width + 2] + 0.5f * gbrg[(i + 1) * width + 1]); rgba[(i * width + 1) * 4 + 2] = gbrg[i * width + 1]; - } else { + } + else { rgba[(i * width + 1) * 4 + 0] = static_cast(0.5f * gbrg[i * width] + 0.5f * gbrg[i * width + 2]); rgba[(i * width + 1) * 4 + 1] = gbrg[i * width + 1]; rgba[(i * width + 1) * 4 + 2] = - static_cast(0.5f * gbrg[(i - 1) * width + 1] + 0.5f * gbrg[(i + 1) * width + 1]); + static_cast(0.5f * gbrg[(i - 1) * width + 1] + 0.5f * gbrg[(i + 1) * width + 1]); } } @@ -853,18 +896,19 @@ void demosaicGBRGToRGBaMalvarTpl(const T *gbrg, T *rgba, unsigned int width, uns for (unsigned int i = 1; i < height - 1; i++) { if (i % 2 == 0) { rgba[(i * width + width - 2) * 4 + 0] = - static_cast(0.5f * gbrg[i * width - 2] + 0.5f * gbrg[(i + 2) * width - 2]); + static_cast(0.5f * gbrg[i * width - 2] + 0.5f * gbrg[(i + 2) * width - 2]); rgba[(i * width + width - 2) * 4 + 1] = gbrg[(i + 1) * width - 2]; rgba[(i * width + width - 2) * 4 + 2] = - static_cast(0.5f * gbrg[(i + 1) * width - 3] + 0.5f * gbrg[(i + 1) * width - 1]); - } else { + static_cast(0.5f * gbrg[(i + 1) * width - 3] + 0.5f * gbrg[(i + 1) * width - 1]); + } + else { rgba[(i * width + width - 2) * 4 + 0] = gbrg[(i + 1) * width - 2]; rgba[(i * width + width - 2) * 4 + 1] = - static_cast(0.25f * gbrg[i * width - 2] + 0.25f * gbrg[(i + 1) * width - 3] + - 0.25f * gbrg[(i + 1) * width - 1] + 0.25f * gbrg[(i + 2) * width - 2]); + static_cast(0.25f * gbrg[i * width - 2] + 0.25f * gbrg[(i + 1) * width - 3] + + 0.25f * gbrg[(i + 1) * width - 1] + 0.25f * gbrg[(i + 2) * width - 2]); rgba[(i * width + width - 2) * 4 + 2] = - static_cast(0.25f * gbrg[i * width - 3] + 0.25f * gbrg[i * width - 1] + 0.25f * gbrg[(i + 2) * width - 3] + - 0.25f * gbrg[(i + 2) * width - 1]); + static_cast(0.25f * gbrg[i * width - 3] + 0.25f * gbrg[i * width - 1] + 0.25f * gbrg[(i + 2) * width - 3] + + 0.25f * gbrg[(i + 2) * width - 1]); } } @@ -872,14 +916,15 @@ void demosaicGBRGToRGBaMalvarTpl(const T *gbrg, T *rgba, unsigned int width, uns for (unsigned int i = 1; i < height - 1; i++) { if (i % 2 == 0) { rgba[(i * width + width - 1) * 4 + 0] = - static_cast(0.5f * gbrg[i * width - 2] + 0.5f * gbrg[(i + 2) * width - 2]); + static_cast(0.5f * gbrg[i * width - 2] + 0.5f * gbrg[(i + 2) * width - 2]); rgba[(i * width + width - 1) * 4 + 1] = gbrg[(i + 1) * width - 2]; rgba[(i * width + width - 1) * 4 + 2] = gbrg[(i + 1) * width - 1]; - } else { + } + else { rgba[(i * width + width - 1) * 4 + 0] = gbrg[(i + 1) * width - 2]; rgba[(i * width + width - 1) * 4 + 1] = gbrg[(i + 1) * width - 1]; rgba[(i * width + width - 1) * 4 + 2] = - static_cast(0.5f * gbrg[i * width - 1] + 0.5f * gbrg[(i + 2) * width - 1]); + static_cast(0.5f * gbrg[i * width - 1] + 0.5f * gbrg[(i + 2) * width - 1]); } } @@ -887,17 +932,18 @@ void demosaicGBRGToRGBaMalvarTpl(const T *gbrg, T *rgba, unsigned int width, uns for (unsigned int j = 1; j < width - 1; j++) { if (j % 2 == 0) { rgba[((height - 2) * width + j) * 4 + 0] = - static_cast(0.5f * gbrg[(height - 3) * width + j] + 0.5f * gbrg[(height - 1) * width + j]); + static_cast(0.5f * gbrg[(height - 3) * width + j] + 0.5f * gbrg[(height - 1) * width + j]); rgba[((height - 2) * width + j) * 4 + 1] = gbrg[(height - 2) * width + j]; rgba[((height - 2) * width + j) * 4 + 2] = - static_cast(0.5f * gbrg[(height - 2) * width + j - 1] + 0.5f * gbrg[(height - 2) * width + j + 1]); - } else { + static_cast(0.5f * gbrg[(height - 2) * width + j - 1] + 0.5f * gbrg[(height - 2) * width + j + 1]); + } + else { rgba[((height - 2) * width + j) * 4 + 0] = - static_cast(0.25f * gbrg[(height - 3) * width + j - 1] + 0.25f * gbrg[(height - 3) * width + j + 1] + - 0.25f * gbrg[(height - 1) * width + j - 1] + 0.25f * gbrg[(height - 1) * width + j + 1]); + static_cast(0.25f * gbrg[(height - 3) * width + j - 1] + 0.25f * gbrg[(height - 3) * width + j + 1] + + 0.25f * gbrg[(height - 1) * width + j - 1] + 0.25f * gbrg[(height - 1) * width + j + 1]); rgba[((height - 2) * width + j) * 4 + 1] = - static_cast(0.25f * gbrg[(height - 3) * width + j] + 0.25f * gbrg[(height - 2) * width + j - 1] + - 0.25f * gbrg[(height - 2) * width + j + 1] + 0.25f * gbrg[(height - 1) * width + j]); + static_cast(0.25f * gbrg[(height - 3) * width + j] + 0.25f * gbrg[(height - 2) * width + j - 1] + + 0.25f * gbrg[(height - 2) * width + j + 1] + 0.25f * gbrg[(height - 1) * width + j]); rgba[((height - 2) * width + j) * 4 + 2] = gbrg[(height - 2) * width + j]; } } @@ -907,18 +953,19 @@ void demosaicGBRGToRGBaMalvarTpl(const T *gbrg, T *rgba, unsigned int width, uns if (j % 2 == 0) { rgba[((height - 1) * width + j) * 4 + 0] = gbrg[(height - 1) * width + j]; rgba[((height - 1) * width + j) * 4 + 1] = - static_cast(0.5f * gbrg[(height - 1) * width + j - 1] + 0.5f * gbrg[(height - 1) * width + j + 1]); + static_cast(0.5f * gbrg[(height - 1) * width + j - 1] + 0.5f * gbrg[(height - 1) * width + j + 1]); rgba[((height - 1) * width + j) * 4 + 2] = - static_cast(0.5f * gbrg[(height - 2) * width + j - 1] + 0.5f * gbrg[(height - 2) * width + j + 1]); - } else { + static_cast(0.5f * gbrg[(height - 2) * width + j - 1] + 0.5f * gbrg[(height - 2) * width + j + 1]); + } + else { rgba[((height - 1) * width + j) * 4 + 0] = - static_cast(0.5f * gbrg[(height - 1) * width + j - 1] + 0.5f * gbrg[(height - 1) * width + j + 1]); + static_cast(0.5f * gbrg[(height - 1) * width + j - 1] + 0.5f * gbrg[(height - 1) * width + j + 1]); rgba[((height - 1) * width + j) * 4 + 1] = gbrg[(height - 1) * width + j]; rgba[((height - 1) * width + j) * 4 + 2] = gbrg[(height - 2) * width + j]; } } -#if defined _OPENMP && _OPENMP >= 200711 // OpenMP 3.1 +#if defined(_OPENMP) && (_OPENMP >= 200711) // OpenMP 3.1 if (nThreads > 0) { omp_set_num_threads(static_cast(nThreads)); } @@ -932,15 +979,18 @@ void demosaicGBRGToRGBaMalvarTpl(const T *gbrg, T *rgba, unsigned int width, uns rgba[(i * width + j) * 4 + 0] = demosaicPhiMalvar(gbrg, width, i, j); rgba[(i * width + j) * 4 + 1] = gbrg[i * width + j]; rgba[(i * width + j) * 4 + 2] = demosaicThetaMalvar(gbrg, width, i, j); - } else if (i % 2 == 0 && j % 2 != 0) { + } + else if (i % 2 == 0 && j % 2 != 0) { rgba[(i * width + j) * 4 + 0] = demosaicCheckerMalvar(gbrg, width, i, j); rgba[(i * width + j) * 4 + 1] = demosaicCrossMalvar(gbrg, width, i, j); rgba[(i * width + j) * 4 + 2] = gbrg[i * width + j]; - } else if (i % 2 != 0 && j % 2 == 0) { + } + else if (i % 2 != 0 && j % 2 == 0) { rgba[(i * width + j) * 4 + 0] = gbrg[i * width + j]; rgba[(i * width + j) * 4 + 1] = demosaicCrossMalvar(gbrg, width, i, j); rgba[(i * width + j) * 4 + 2] = demosaicCheckerMalvar(gbrg, width, i, j); - } else { + } + else { rgba[(i * width + j) * 4 + 0] = demosaicThetaMalvar(gbrg, width, i, j); rgba[(i * width + j) * 4 + 1] = gbrg[i * width + j]; rgba[(i * width + j) * 4 + 2] = demosaicPhiMalvar(gbrg, width, i, j); @@ -983,7 +1033,8 @@ void demosaicGRBGToRGBaMalvarTpl(const T *grbg, T *rgba, unsigned int width, uns rgba[j * 4 + 0] = static_cast(0.5f * grbg[j - 1] + 0.5f * grbg[j + 1]); rgba[j * 4 + 1] = grbg[j]; rgba[j * 4 + 2] = grbg[width + j]; - } else { + } + else { rgba[j * 4 + 0] = grbg[j]; rgba[j * 4 + 1] = static_cast(0.5f * grbg[j - 1] + 0.5f * grbg[j + 1]); rgba[j * 4 + 2] = static_cast(0.5f * grbg[width + j - 1] + 0.5f * grbg[width + j + 1]); @@ -998,7 +1049,8 @@ void demosaicGRBGToRGBaMalvarTpl(const T *grbg, T *rgba, unsigned int width, uns rgba[(width + j) * 4 + 1] = static_cast(0.25f * grbg[j] + 0.25f * grbg[width + j - 1] + 0.25f * grbg[width + j + 1] + 0.25f * grbg[2 * width + j]); rgba[(width + j) * 4 + 2] = grbg[width + j]; - } else { + } + else { rgba[(width + j) * 4 + 0] = static_cast(0.5f * grbg[j] + 0.5f * grbg[2 * width + j]); rgba[(width + j) * 4 + 1] = grbg[width + j]; rgba[(width + j) * 4 + 2] = static_cast(0.5f * grbg[width + j - 1] + 0.5f * grbg[width + j + 1]); @@ -1011,7 +1063,8 @@ void demosaicGRBGToRGBaMalvarTpl(const T *grbg, T *rgba, unsigned int width, uns rgba[i * width * 4 + 0] = grbg[i * width + 1]; rgba[i * width * 4 + 1] = grbg[i * width]; rgba[i * width * 4 + 2] = static_cast(0.5f * grbg[(i - 1) * width] + 0.5f * grbg[(i + 1) * width]); - } else { + } + else { rgba[i * width * 4 + 0] = static_cast(0.5f * grbg[(i - 1) * width + 1] + 0.5f * grbg[(i + 1) * width + 1]); rgba[i * width * 4 + 1] = grbg[i * width + 1]; rgba[i * width * 4 + 2] = grbg[i * width]; @@ -1026,9 +1079,10 @@ void demosaicGRBGToRGBaMalvarTpl(const T *grbg, T *rgba, unsigned int width, uns 0.25f * grbg[i * width + 2] + 0.25f * grbg[(i + 1) * width + 1]); rgba[(i * width + 1) * 4 + 2] = static_cast(0.25f * grbg[(i - 1) * width] + 0.25f * grbg[(i - 1) * width + 2] + 0.25f * grbg[(i + 1) * width] + 0.25f * grbg[(i + 1) * width + 2]); - } else { + } + else { rgba[(i * width + 1) * 4 + 0] = - static_cast(0.5f * grbg[(i - 1) * width + 1] + 0.5f * grbg[(i + 1) * width + 1]); + static_cast(0.5f * grbg[(i - 1) * width + 1] + 0.5f * grbg[(i + 1) * width + 1]); rgba[(i * width + 1) * 4 + 1] = grbg[i * width + 1]; rgba[(i * width + 1) * 4 + 2] = static_cast(0.5f * grbg[i * width] + 0.5f * grbg[i * width + 2]); } @@ -1038,17 +1092,18 @@ void demosaicGRBGToRGBaMalvarTpl(const T *grbg, T *rgba, unsigned int width, uns for (unsigned int i = 1; i < height - 1; i++) { if (i % 2 == 0) { rgba[(i * width + width - 2) * 4 + 0] = - static_cast(0.5f * grbg[(i + 1) * width - 3] + 0.5f * grbg[(i + 1) * width - 1]); + static_cast(0.5f * grbg[(i + 1) * width - 3] + 0.5f * grbg[(i + 1) * width - 1]); rgba[(i * width + width - 2) * 4 + 1] = grbg[(i + 1) * width - 2]; rgba[(i * width + width - 2) * 4 + 2] = - static_cast(0.5f * grbg[i * width - 2] + 0.5f * grbg[(i + 2) * width - 2]); - } else { + static_cast(0.5f * grbg[i * width - 2] + 0.5f * grbg[(i + 2) * width - 2]); + } + else { rgba[(i * width + width - 2) * 4 + 0] = - static_cast(0.25f * grbg[i * width - 3] + 0.25f * grbg[i * width - 1] + 0.25f * grbg[(i + 2) * width - 3] + - 0.25f * grbg[(i + 2) * width - 1]); + static_cast(0.25f * grbg[i * width - 3] + 0.25f * grbg[i * width - 1] + 0.25f * grbg[(i + 2) * width - 3] + + 0.25f * grbg[(i + 2) * width - 1]); rgba[(i * width + width - 2) * 4 + 1] = - static_cast(0.25f * grbg[i * width - 2] + 0.25f * grbg[(i + 1) * width - 3] + - 0.25f * grbg[(i + 1) * width - 1] + 0.25f * grbg[(i + 2) * width - 2]); + static_cast(0.25f * grbg[i * width - 2] + 0.25f * grbg[(i + 1) * width - 3] + + 0.25f * grbg[(i + 1) * width - 1] + 0.25f * grbg[(i + 2) * width - 2]); rgba[(i * width + width - 2) * 4 + 2] = grbg[(i + 1) * width - 2]; } } @@ -1059,10 +1114,11 @@ void demosaicGRBGToRGBaMalvarTpl(const T *grbg, T *rgba, unsigned int width, uns rgba[(i * width + width - 1) * 4 + 0] = grbg[(i + 1) * width - 1]; rgba[(i * width + width - 1) * 4 + 1] = grbg[(i + 1) * width - 2]; rgba[(i * width + width - 1) * 4 + 2] = - static_cast(0.5f * grbg[i * width - 2] + 0.5f * grbg[(i + 2) * width - 2]); - } else { + static_cast(0.5f * grbg[i * width - 2] + 0.5f * grbg[(i + 2) * width - 2]); + } + else { rgba[(i * width + width - 1) * 4 + 0] = - static_cast(0.5f * grbg[i * width - 1] + 0.5f * grbg[(i + 2) * width - 1]); + static_cast(0.5f * grbg[i * width - 1] + 0.5f * grbg[(i + 2) * width - 1]); rgba[(i * width + width - 1) * 4 + 1] = grbg[(i + 1) * width - 1]; rgba[(i * width + width - 1) * 4 + 2] = grbg[(i + 1) * width - 2]; } @@ -1072,18 +1128,19 @@ void demosaicGRBGToRGBaMalvarTpl(const T *grbg, T *rgba, unsigned int width, uns for (unsigned int j = 1; j < width - 1; j++) { if (j % 2 == 0) { rgba[((height - 2) * width + j) * 4 + 0] = - static_cast(0.5f * grbg[(height - 2) * width + j - 1] + 0.5f * grbg[(height - 2) * width + j + 1]); + static_cast(0.5f * grbg[(height - 2) * width + j - 1] + 0.5f * grbg[(height - 2) * width + j + 1]); rgba[((height - 2) * width + j) * 4 + 1] = grbg[(height - 2) * width + j]; rgba[((height - 2) * width + j) * 4 + 2] = - static_cast(0.5f * grbg[(height - 3) * width + j] + 0.5f * grbg[(height - 1) * width + j]); - } else { + static_cast(0.5f * grbg[(height - 3) * width + j] + 0.5f * grbg[(height - 1) * width + j]); + } + else { rgba[((height - 2) * width + j) * 4 + 0] = grbg[(height - 2) * width + j]; rgba[((height - 2) * width + j) * 4 + 1] = - static_cast(0.25f * grbg[(height - 3) * width + j] + 0.25f * grbg[(height - 2) * width + j - 1] + - 0.25f * grbg[(height - 2) * width + j + 1] + 0.25f * grbg[(height - 1) * width + j]); + static_cast(0.25f * grbg[(height - 3) * width + j] + 0.25f * grbg[(height - 2) * width + j - 1] + + 0.25f * grbg[(height - 2) * width + j + 1] + 0.25f * grbg[(height - 1) * width + j]); rgba[((height - 2) * width + j) * 4 + 2] = - static_cast(0.25f * grbg[(height - 3) * width + j - 1] + 0.25f * grbg[(height - 3) * width + j + 1] + - 0.25f * grbg[(height - 1) * width + j - 1] + 0.25f * grbg[(height - 1) * width + j + 1]); + static_cast(0.25f * grbg[(height - 3) * width + j - 1] + 0.25f * grbg[(height - 3) * width + j + 1] + + 0.25f * grbg[(height - 1) * width + j - 1] + 0.25f * grbg[(height - 1) * width + j + 1]); } } @@ -1091,19 +1148,20 @@ void demosaicGRBGToRGBaMalvarTpl(const T *grbg, T *rgba, unsigned int width, uns for (unsigned int j = 1; j < width - 1; j++) { if (j % 2 == 0) { rgba[((height - 1) * width + j) * 4 + 0] = - static_cast(0.5f * grbg[(height - 2) * width + j - 1] + 0.5f * grbg[(height - 2) * width + j + 1]); + static_cast(0.5f * grbg[(height - 2) * width + j - 1] + 0.5f * grbg[(height - 2) * width + j + 1]); rgba[((height - 1) * width + j) * 4 + 1] = - static_cast(0.5f * grbg[(height - 1) * width + j - 1] + 0.5f * grbg[(height - 1) * width + j + 1]); + static_cast(0.5f * grbg[(height - 1) * width + j - 1] + 0.5f * grbg[(height - 1) * width + j + 1]); rgba[((height - 1) * width + j) * 4 + 2] = grbg[(height - 1) * width + j]; - } else { + } + else { rgba[((height - 1) * width + j) * 4 + 0] = grbg[(height - 2) * width + j]; rgba[((height - 1) * width + j) * 4 + 1] = grbg[(height - 1) * width + j]; rgba[((height - 1) * width + j) * 4 + 2] = - static_cast(0.5f * grbg[(height - 1) * width + j - 1] + 0.5f * grbg[(height - 1) * width + j + 1]); + static_cast(0.5f * grbg[(height - 1) * width + j - 1] + 0.5f * grbg[(height - 1) * width + j + 1]); } } -#if defined _OPENMP && _OPENMP >= 200711 // OpenMP 3.1 +#if defined(_OPENMP) && (_OPENMP >= 200711) // OpenMP 3.1 if (nThreads > 0) { omp_set_num_threads(static_cast(nThreads)); } @@ -1117,15 +1175,18 @@ void demosaicGRBGToRGBaMalvarTpl(const T *grbg, T *rgba, unsigned int width, uns rgba[(i * width + j) * 4 + 0] = demosaicThetaMalvar(grbg, width, i, j); rgba[(i * width + j) * 4 + 1] = grbg[i * width + j]; rgba[(i * width + j) * 4 + 2] = demosaicPhiMalvar(grbg, width, i, j); - } else if (i % 2 == 0 && j % 2 != 0) { + } + else if (i % 2 == 0 && j % 2 != 0) { rgba[(i * width + j) * 4 + 0] = grbg[i * width + j]; rgba[(i * width + j) * 4 + 1] = demosaicCrossMalvar(grbg, width, i, j); rgba[(i * width + j) * 4 + 2] = demosaicCheckerMalvar(grbg, width, i, j); - } else if (i % 2 != 0 && j % 2 == 0) { + } + else if (i % 2 != 0 && j % 2 == 0) { rgba[(i * width + j) * 4 + 0] = demosaicCheckerMalvar(grbg, width, i, j); rgba[(i * width + j) * 4 + 1] = demosaicCrossMalvar(grbg, width, i, j); rgba[(i * width + j) * 4 + 2] = grbg[i * width + j]; - } else { + } + else { rgba[(i * width + j) * 4 + 0] = demosaicPhiMalvar(grbg, width, i, j); rgba[(i * width + j) * 4 + 1] = grbg[i * width + j]; rgba[(i * width + j) * 4 + 2] = demosaicThetaMalvar(grbg, width, i, j); @@ -1168,7 +1229,8 @@ void demosaicRGGBToRGBaMalvarTpl(const T *rggb, T *rgba, unsigned int width, uns rgba[j * 4 + 0] = rggb[j]; rgba[j * 4 + 1] = static_cast(0.5f * rggb[j - 1] + 0.5f * rggb[j + 1]); rgba[j * 4 + 2] = static_cast(0.5f * rggb[width + j - 1] + 0.5f * rggb[width + j + 1]); - } else { + } + else { rgba[j * 4 + 0] = static_cast(0.5f * rggb[j - 1] + 0.5f * rggb[j + 1]); rgba[j * 4 + 1] = rggb[j]; rgba[j * 4 + 2] = rggb[width + j]; @@ -1181,7 +1243,8 @@ void demosaicRGGBToRGBaMalvarTpl(const T *rggb, T *rgba, unsigned int width, uns rgba[(width + j) * 4 + 0] = static_cast(0.5f * rggb[j] + 0.5f * rggb[2 * width + j]); rgba[(width + j) * 4 + 1] = rggb[width + j]; rgba[(width + j) * 4 + 2] = static_cast(0.5f * rggb[width + j - 1] + 0.5f * rggb[width + j + 1]); - } else { + } + else { rgba[(width + j) * 4 + 0] = static_cast(0.25f * rggb[j - 1] + 0.25f * rggb[j + 1] + 0.25f * rggb[2 * width + j - 1] + 0.25f * rggb[2 * width + j + 1]); rgba[(width + j) * 4 + 1] = static_cast(0.25f * rggb[j] + 0.25f * rggb[width + j - 1] + @@ -1196,7 +1259,8 @@ void demosaicRGGBToRGBaMalvarTpl(const T *rggb, T *rgba, unsigned int width, uns rgba[i * width * 4 + 0] = rggb[i * width]; rgba[i * width * 4 + 1] = rggb[i * width + 1]; rgba[i * width * 4 + 2] = static_cast(0.5f * rggb[(i - 1) * width + 1] + 0.5f * rggb[(i + 1) * width + 1]); - } else { + } + else { rgba[i * width * 4 + 0] = static_cast(0.5f * rggb[(i - 1) * width] + 0.5f * rggb[(i + 1) * width]); rgba[i * width * 4 + 1] = rggb[i * width]; rgba[i * width * 4 + 2] = rggb[i * width + 1]; @@ -1209,8 +1273,9 @@ void demosaicRGGBToRGBaMalvarTpl(const T *rggb, T *rgba, unsigned int width, uns rgba[(i * width + 1) * 4 + 0] = static_cast(0.5f * rggb[i * width] + 0.5f * rggb[i * width + 2]); rgba[(i * width + 1) * 4 + 1] = rggb[i * width + 1]; rgba[(i * width + 1) * 4 + 2] = - static_cast(0.5f * rggb[(i - 1) * width + 1] + 0.5f * rggb[(i + 1) * width + 1]); - } else { + static_cast(0.5f * rggb[(i - 1) * width + 1] + 0.5f * rggb[(i + 1) * width + 1]); + } + else { rgba[(i * width + 1) * 4 + 0] = static_cast(0.25f * rggb[(i - 1) * width] + 0.25f * rggb[(i - 1) * width + 2] + 0.25f * rggb[(i + 1) * width] + 0.25f * rggb[(i + 1) * width + 2]); rgba[(i * width + 1) * 4 + 1] = static_cast(0.25f * rggb[(i - 1) * width + 1] + 0.25f * rggb[i * width] + @@ -1224,17 +1289,18 @@ void demosaicRGGBToRGBaMalvarTpl(const T *rggb, T *rgba, unsigned int width, uns if (i % 2 == 0) { rgba[(i * width + width - 2) * 4 + 0] = rggb[(i + 1) * width - 2]; rgba[(i * width + width - 2) * 4 + 1] = - static_cast(0.25f * rggb[i * width - 2] + 0.25f * rggb[(i + 1) * width - 3] + - 0.25f * rggb[(i + 1) * width - 1] + 0.25f * rggb[(i + 2) * width - 2]); + static_cast(0.25f * rggb[i * width - 2] + 0.25f * rggb[(i + 1) * width - 3] + + 0.25f * rggb[(i + 1) * width - 1] + 0.25f * rggb[(i + 2) * width - 2]); rgba[(i * width + width - 2) * 4 + 2] = - static_cast(0.25f * rggb[i * width - 3] + 0.25f * rggb[i * width - 1] + 0.25f * rggb[(i + 2) * width - 3] + - 0.25f * rggb[(i + 2) * width - 1]); - } else { + static_cast(0.25f * rggb[i * width - 3] + 0.25f * rggb[i * width - 1] + 0.25f * rggb[(i + 2) * width - 3] + + 0.25f * rggb[(i + 2) * width - 1]); + } + else { rgba[(i * width + width - 2) * 4 + 0] = - static_cast(0.5f * rggb[i * width - 2] + 0.5f * rggb[(i + 2) * width - 2]); + static_cast(0.5f * rggb[i * width - 2] + 0.5f * rggb[(i + 2) * width - 2]); rgba[(i * width + width - 2) * 4 + 1] = rggb[(i + 1) * width - 2]; rgba[(i * width + width - 2) * 4 + 2] = - static_cast(0.5f * rggb[(i + 1) * width - 3] + 0.5f * rggb[(i + 1) * width - 1]); + static_cast(0.5f * rggb[(i + 1) * width - 3] + 0.5f * rggb[(i + 1) * width - 1]); } } @@ -1244,10 +1310,11 @@ void demosaicRGGBToRGBaMalvarTpl(const T *rggb, T *rgba, unsigned int width, uns rgba[(i * width + width - 1) * 4 + 0] = rggb[(i + 1) * width - 2]; rgba[(i * width + width - 1) * 4 + 1] = rggb[(i + 1) * width - 1]; rgba[(i * width + width - 1) * 4 + 2] = - static_cast(0.5f * rggb[i * width - 1] + 0.5f * rggb[(i + 2) * width - 1]); - } else { + static_cast(0.5f * rggb[i * width - 1] + 0.5f * rggb[(i + 2) * width - 1]); + } + else { rgba[(i * width + width - 1) * 4 + 0] = - static_cast(0.5f * rggb[i * width - 2] + 0.5f * rggb[(i + 2) * width - 2]); + static_cast(0.5f * rggb[i * width - 2] + 0.5f * rggb[(i + 2) * width - 2]); rgba[(i * width + width - 1) * 4 + 1] = rggb[(i + 1) * width - 2]; rgba[(i * width + width - 1) * 4 + 2] = rggb[(i + 1) * width - 1]; } @@ -1258,17 +1325,18 @@ void demosaicRGGBToRGBaMalvarTpl(const T *rggb, T *rgba, unsigned int width, uns if (j % 2 == 0) { rgba[((height - 2) * width + j) * 4 + 0] = rggb[(height - 2) * width + j]; rgba[((height - 2) * width + j) * 4 + 1] = - static_cast(0.25f * rggb[(height - 3) * width + j] + 0.25f * rggb[(height - 2) * width + j - 1] + - 0.25f * rggb[(height - 2) * width + j + 1] + 0.25f * rggb[(height - 1) * width + j]); + static_cast(0.25f * rggb[(height - 3) * width + j] + 0.25f * rggb[(height - 2) * width + j - 1] + + 0.25f * rggb[(height - 2) * width + j + 1] + 0.25f * rggb[(height - 1) * width + j]); rgba[((height - 2) * width + j) * 4 + 2] = - static_cast(0.25f * rggb[(height - 3) * width + j - 1] + 0.25f * rggb[(height - 3) * width + j + 1] + - 0.25f * rggb[(height - 1) * width + j - 1] + 0.25f * rggb[(height - 1) * width + j + 1]); - } else { + static_cast(0.25f * rggb[(height - 3) * width + j - 1] + 0.25f * rggb[(height - 3) * width + j + 1] + + 0.25f * rggb[(height - 1) * width + j - 1] + 0.25f * rggb[(height - 1) * width + j + 1]); + } + else { rgba[((height - 2) * width + j) * 4 + 0] = - static_cast(0.5f * rggb[(height - 2) * width + j - 1] + 0.5f * rggb[(height - 2) * width + j + 1]); + static_cast(0.5f * rggb[(height - 2) * width + j - 1] + 0.5f * rggb[(height - 2) * width + j + 1]); rgba[((height - 2) * width + j) * 4 + 1] = rggb[(height - 2) * width + j]; rgba[((height - 2) * width + j) * 4 + 2] = - static_cast(0.5f * rggb[(height - 3) * width + j] + 0.5f * rggb[(height - 1) * width + j]); + static_cast(0.5f * rggb[(height - 3) * width + j] + 0.5f * rggb[(height - 1) * width + j]); } } @@ -1278,17 +1346,18 @@ void demosaicRGGBToRGBaMalvarTpl(const T *rggb, T *rgba, unsigned int width, uns rgba[((height - 1) * width + j) * 4 + 0] = rggb[(height - 2) * width + j]; rgba[((height - 1) * width + j) * 4 + 1] = rggb[(height - 1) * width + j]; rgba[((height - 1) * width + j) * 4 + 2] = - static_cast(0.5f * rggb[(height - 1) * width + j - 1] + 0.5f * rggb[(height - 1) * width + j + 1]); - } else { + static_cast(0.5f * rggb[(height - 1) * width + j - 1] + 0.5f * rggb[(height - 1) * width + j + 1]); + } + else { rgba[((height - 1) * width + j) * 4 + 0] = - static_cast(0.5f * rggb[(height - 2) * width + j - 1] + 0.5f * rggb[(height - 2) * width + j + 1]); + static_cast(0.5f * rggb[(height - 2) * width + j - 1] + 0.5f * rggb[(height - 2) * width + j + 1]); rgba[((height - 1) * width + j) * 4 + 1] = - static_cast(0.5f * rggb[(height - 1) * width + j - 1] + 0.5f * rggb[(height - 1) * width + j + 1]); + static_cast(0.5f * rggb[(height - 1) * width + j - 1] + 0.5f * rggb[(height - 1) * width + j + 1]); rgba[((height - 1) * width + j) * 4 + 2] = rggb[(height - 1) * width + j]; } } -#if defined _OPENMP && _OPENMP >= 200711 // OpenMP 3.1 +#if defined(_OPENMP) && (_OPENMP >= 200711) // OpenMP 3.1 if (nThreads > 0) { omp_set_num_threads(static_cast(nThreads)); } @@ -1302,15 +1371,18 @@ void demosaicRGGBToRGBaMalvarTpl(const T *rggb, T *rgba, unsigned int width, uns rgba[(i * width + j) * 4 + 0] = rggb[i * width + j]; rgba[(i * width + j) * 4 + 1] = demosaicCrossMalvar(rggb, width, i, j); rgba[(i * width + j) * 4 + 2] = demosaicCheckerMalvar(rggb, width, i, j); - } else if (i % 2 == 0 && j % 2 != 0) { + } + else if (i % 2 == 0 && j % 2 != 0) { rgba[(i * width + j) * 4 + 0] = demosaicThetaMalvar(rggb, width, i, j); rgba[(i * width + j) * 4 + 1] = rggb[i * width + j]; rgba[(i * width + j) * 4 + 2] = demosaicPhiMalvar(rggb, width, i, j); - } else if (i % 2 != 0 && j % 2 == 0) { + } + else if (i % 2 != 0 && j % 2 == 0) { rgba[(i * width + j) * 4 + 0] = demosaicPhiMalvar(rggb, width, i, j); rgba[(i * width + j) * 4 + 1] = rggb[i * width + j]; rgba[(i * width + j) * 4 + 2] = demosaicThetaMalvar(rggb, width, i, j); - } else { + } + else { rgba[(i * width + j) * 4 + 0] = demosaicCheckerMalvar(rggb, width, i, j); rgba[(i * width + j) * 4 + 1] = demosaicCrossMalvar(rggb, width, i, j); rgba[(i * width + j) * 4 + 2] = rggb[i * width + j]; diff --git a/modules/core/src/image/vpGaussianFilter.cpp b/modules/core/src/image/vpGaussianFilter.cpp index 4a37ca4183..c58e7e3d40 100644 --- a/modules/core/src/image/vpGaussianFilter.cpp +++ b/modules/core/src/image/vpGaussianFilter.cpp @@ -31,6 +31,9 @@ * Gaussian filter class */ +#include + +#if defined(VISP_HAVE_SIMDLIB) #include #include #include @@ -51,7 +54,6 @@ class vpGaussianFilter::Impl const size_t channels = 4; m_funcPtrRGBa = SimdGaussianBlurInit(width, height, channels, &sigma, &epsilon); } - if (m_deinterleave) { m_red.resize(height, width); m_green.resize(height, width); @@ -146,3 +148,9 @@ void vpGaussianFilter::apply(const vpImage &I, vpImage &I, vpImage &I_blur) { m_impl->apply(I, I_blur); } + +#elif !defined(VISP_BUILD_SHARED_LIBS) +// Work around to avoid warning: libvisp_core.a(vpGaussianFilter.cpp.o) has no symbols +void dummy_vpGaussianFilter() { }; + +#endif diff --git a/modules/core/src/image/vpImageConvert.cpp b/modules/core/src/image/vpImageConvert.cpp index 47eeb943fc..7e12341789 100644 --- a/modules/core/src/image/vpImageConvert.cpp +++ b/modules/core/src/image/vpImageConvert.cpp @@ -41,14 +41,18 @@ #include #include -#if defined _OPENMP +#if defined(_OPENMP) #include #endif + +#include // image #include "private/vpBayerConversion.h" #include "private/vpImageConvert_impl.h" -#include +#if defined(VISP_HAVE_SIMDLIB) +#include +#endif #include bool vpImageConvert::YCbCrLUTcomputed = false; @@ -285,25 +289,25 @@ void vpImageConvert::createDepthHistogram(const vpImage &src_depth, vpIma \param[in] flip : Set to true to vertically flip the converted image. \code -#include -#include -#include -#include - -int main() -{ -#if defined(VISP_HAVE_OPENCV) - vpImage Ic; // A color image - cv::Mat Ip; - - // Read an image on a disk with openCV library - Ip = cv::imread("image.pgm", cv::IMREAD_COLOR); // Second parameter for a BGR encoding. - // Convert the grayscale cv::Mat into vpImage - vpImageConvert::convert(Ip, Ic); - - // ... -#endif -} + #include + #include + #include + #include + + int main() + { + #if defined(VISP_HAVE_OPENCV) + vpImage Ic; // A color image + cv::Mat Ip; + + // Read an image on a disk with openCV library + Ip = cv::imread("image.pgm", cv::IMREAD_COLOR); // Second parameter for a BGR encoding. + // Convert the grayscale cv::Mat into vpImage + vpImageConvert::convert(Ip, Ic); + + // ... + #endif + } \endcode */ void vpImageConvert::convert(const cv::Mat &src, vpImage &dest, bool flip) @@ -326,11 +330,13 @@ void vpImageConvert::convert(const cv::Mat &src, vpImage &dest, bool fli } } else if (src.type() == CV_8UC3) { +#if defined(VISP_HAVE_SIMDLIB) if (src.isContinuous() && !flip) { SimdRgbToBgra(src.data, src.cols, src.rows, src.step[0], reinterpret_cast(dest.bitmap), dest.getWidth() * sizeof(vpRGBa), vpRGBa::alpha_default); } else { +#endif vpRGBa rgbaVal; rgbaVal.A = vpRGBa::alpha_default; for (unsigned int i = 0; i < dest.getRows(); ++i) { @@ -347,18 +353,23 @@ void vpImageConvert::convert(const cv::Mat &src, vpImage &dest, bool fli } } } +#if defined(VISP_HAVE_SIMDLIB) } +#endif } else if (src.type() == CV_8UC1) { +#if defined(VISP_HAVE_SIMDLIB) if (src.isContinuous() && !flip) { SimdGrayToBgra(src.data, src.cols, src.rows, src.step[0], reinterpret_cast(dest.bitmap), dest.getWidth() * sizeof(vpRGBa), vpRGBa::alpha_default); } else { +#endif vpRGBa rgbaVal; for (unsigned int i = 0; i < dest.getRows(); ++i) { for (unsigned int j = 0; j < dest.getCols(); ++j) { rgbaVal = src.at((int)i, (int)j); + rgbaVal.A = vpRGBa::alpha_default; if (flip) { dest[dest.getRows() - i - 1][j] = rgbaVal; } @@ -367,7 +378,9 @@ void vpImageConvert::convert(const cv::Mat &src, vpImage &dest, bool fli } } } +#if defined(VISP_HAVE_SIMDLIB) } +#endif } } @@ -386,24 +399,24 @@ void vpImageConvert::convert(const cv::Mat &src, vpImage &dest, bool fli OpenMP will choose the number of threads. \code -#include -#include -#include - -int main() -{ -#if defined(VISP_HAVE_OPENCV) - vpImage Ig; // A grayscale image - cv::Mat Ip; - - // Read an image on a disk with openCV library - Ip = cv::imread("image.pgm", cv::IMREAD_GRAYSCALE); // Second parameter for a gray level. - // Convert the grayscale cv::Mat into vpImage - vpImageConvert::convert(Ip, Ig); - - // ... -#endif -} + #include + #include + #include + + int main() + { + #if defined(VISP_HAVE_OPENCV) + vpImage Ig; // A grayscale image + cv::Mat Ip; + + // Read an image on a disk with openCV library + Ip = cv::imread("image.pgm", cv::IMREAD_GRAYSCALE); // Second parameter for a gray level. + // Convert the grayscale cv::Mat into vpImage + vpImageConvert::convert(Ip, Ig); + + // ... + #endif + } \endcode */ void vpImageConvert::convert(const cv::Mat &src, vpImage &dest, bool flip, unsigned int nThreads) @@ -3215,7 +3228,7 @@ void vpImageConvert::RGBToRGBa(unsigned char *rgb, unsigned char *rgba, unsigned Converts a RGB image to RGBa. Alpha component is set to vpRGBa::alpha_default. - Flips the image verticaly if needed. + Flips the image vertically if needed. Assumes that rgba is already resized. \note If flip is false, the SIMD lib is used to accelerate processing on x86 and ARM architecture. @@ -3229,12 +3242,14 @@ void vpImageConvert::RGBToRGBa(unsigned char *rgb, unsigned char *rgba, unsigned void vpImageConvert::RGBToRGBa(unsigned char *rgb, unsigned char *rgba, unsigned int width, unsigned int height, bool flip) { +#if defined(VISP_HAVE_SIMDLIB) if (!flip) { SimdBgrToBgra(rgb, width, height, width * 3, rgba, width * 4, vpRGBa::alpha_default); } else { - // if we have to flip the image, we start from the end last scanline so the - // step is negative +#endif +// if we have to flip the image, we start from the end last scanline so the +// step is negative int lineStep = (flip) ? -(int)(width * 3) : (int)(width * 3); // starting source address = last line if we need to flip the image @@ -3254,7 +3269,9 @@ void vpImageConvert::RGBToRGBa(unsigned char *rgb, unsigned char *rgba, unsigned // go to the next line src += lineStep; } +#if defined(VISP_HAVE_SIMDLIB) } +#endif } /*! @@ -3271,11 +3288,24 @@ void vpImageConvert::RGBToRGBa(unsigned char *rgb, unsigned char *rgba, unsigned */ void vpImageConvert::RGBaToRGB(unsigned char *rgba, unsigned char *rgb, unsigned int size) { +#if defined(VISP_HAVE_SIMDLIB) SimdBgraToBgr(rgba, size, 1, size * 4, rgb, size * 3); +#else + unsigned char *pt_input = rgba; + unsigned char *pt_end = rgba + 4 * size; + unsigned char *pt_output = rgb; + + while (pt_input != pt_end) { + *(pt_output++) = *(pt_input++); // R + *(pt_output++) = *(pt_input++); // G + *(pt_output++) = *(pt_input++); // B + pt_input++; + } +#endif } /*! - Convert an RGB image to a greyscale one. + Convert an RGB image to a grey scale one. See Charles Pontyon's Colour FAQ http://www.poynton.com/notes/colour_and_gamma/ColorFAQ.html @@ -3292,8 +3322,8 @@ void vpImageConvert::RGBToGrey(unsigned char *rgb, unsigned char *grey, unsigned } /*! - Converts a RGB image to a greyscale one. - Flips the image verticaly if needed. + Converts a RGB image to a grey scale one. + Flips the image vertically if needed. Assumes that grey is already resized. \note If flip is false, the SIMD lib is used to accelerate processing on x86 and ARM architecture. @@ -3307,10 +3337,12 @@ void vpImageConvert::RGBToGrey(unsigned char *rgb, unsigned char *grey, unsigned void vpImageConvert::RGBToGrey(unsigned char *rgb, unsigned char *grey, unsigned int width, unsigned int height, bool flip) { +#if defined(VISP_HAVE_SIMDLIB) if (!flip) { SimdRgbToGray(rgb, width, height, width * 3, grey, width); } else { +#endif // if we have to flip the image, we start from the end last scanline so // the step is negative int lineStep = (flip) ? -(int)(width * 3) : (int)(width * 3); @@ -3335,11 +3367,13 @@ void vpImageConvert::RGBToGrey(unsigned char *rgb, unsigned char *grey, unsigned // go to the next line src += lineStep; } +#if defined(VISP_HAVE_SIMDLIB) } +#endif } /*! - Convert a RGBa image to a greyscale one. + Convert a RGBa image to a grey scale one. See Charles Pontyon's Colour FAQ http://www.poynton.com/notes/colour_and_gamma/ColorFAQ.html @@ -3355,12 +3389,13 @@ void vpImageConvert::RGBToGrey(unsigned char *rgb, unsigned char *grey, unsigned */ void vpImageConvert::RGBaToGrey(unsigned char *rgba, unsigned char *grey, unsigned int width, unsigned int height, unsigned int -#if defined _OPENMP +#if defined(_OPENMP) nThreads #endif ) { -#if defined _OPENMP +#if defined(VISP_HAVE_SIMDLIB) +#if defined(_OPENMP) if (nThreads > 0) { omp_set_num_threads(static_cast(nThreads)); } @@ -3369,6 +3404,12 @@ void vpImageConvert::RGBaToGrey(unsigned char *rgba, unsigned char *grey, unsign for (int i = 0; i < static_cast(height); i++) { SimdRgbaToGray(rgba + i * width * 4, width, 1, width * 4, grey + i * width, width); } +#else +#if defined(_OPENMP) + (void)nThreads; +#endif + vpImageConvert::RGBaToGrey(rgba, grey, width * height); +#endif } /*! @@ -3387,7 +3428,19 @@ void vpImageConvert::RGBaToGrey(unsigned char *rgba, unsigned char *grey, unsign */ void vpImageConvert::RGBaToGrey(unsigned char *rgba, unsigned char *grey, unsigned int size) { +#if defined(VISP_HAVE_SIMDLIB) SimdRgbaToGray(rgba, size, 1, size * 4, grey, size); +#else + unsigned char *pt_input = rgba; + unsigned char *pt_end = rgba + size * 4; + unsigned char *pt_output = grey; + + while (pt_input != pt_end) { + *pt_output = (unsigned char)(0.2126 * (*pt_input) + 0.7152 * (*(pt_input + 1)) + 0.0722 * (*(pt_input + 2))); + pt_input += 4; + pt_output++; + } +#endif } /*! @@ -3403,7 +3456,11 @@ void vpImageConvert::RGBaToGrey(unsigned char *rgba, unsigned char *grey, unsign */ void vpImageConvert::GreyToRGBa(unsigned char *grey, unsigned char *rgba, unsigned int width, unsigned int height) { +#if defined(VISP_HAVE_SIMDLIB) SimdGrayToBgra(grey, width, height, width, rgba, width * sizeof(vpRGBa), vpRGBa::alpha_default); +#else + vpImageConvert::GreyToRGBa(grey, rgba, width * height); +#endif } /*! @@ -3420,7 +3477,24 @@ void vpImageConvert::GreyToRGBa(unsigned char *grey, unsigned char *rgba, unsign */ void vpImageConvert::GreyToRGBa(unsigned char *grey, unsigned char *rgba, unsigned int size) { +#if defined(VISP_HAVE_SIMDLIB) GreyToRGBa(grey, rgba, size, 1); +#else + unsigned char *pt_input = grey; + unsigned char *pt_end = grey + size; + unsigned char *pt_output = rgba; + + while (pt_input != pt_end) { + unsigned char p = *pt_input; + *(pt_output) = p; // R + *(pt_output + 1) = p; // G + *(pt_output + 2) = p; // B + *(pt_output + 3) = vpRGBa::alpha_default; // A + + pt_input++; + pt_output += 4; + } +#endif } /*! @@ -3435,14 +3509,30 @@ void vpImageConvert::GreyToRGBa(unsigned char *grey, unsigned char *rgba, unsign */ void vpImageConvert::GreyToRGB(unsigned char *grey, unsigned char *rgb, unsigned int size) { +#if defined(VISP_HAVE_SIMDLIB) SimdGrayToBgr(grey, size, 1, size, rgb, size * 3); +#else + unsigned char *pt_input = grey; + unsigned char *pt_end = grey + size; + unsigned char *pt_output = rgb; + + while (pt_input != pt_end) { + unsigned char p = *pt_input; + *(pt_output) = p; // R + *(pt_output + 1) = p; // G + *(pt_output + 2) = p; // B + + pt_input++; + pt_output += 3; + } +#endif } /*! Converts a BGR image to RGBa. The alpha component is set to vpRGBa::alpha_default. - Flips the image verticaly if needed. + Flips the image vertically if needed. Assumes that rgba is already resized. \note If flip is false, the SIMD lib is used to accelerate processing on x86 and ARM architecture. @@ -3456,10 +3546,12 @@ void vpImageConvert::GreyToRGB(unsigned char *grey, unsigned char *rgb, unsigned void vpImageConvert::BGRToRGBa(unsigned char *bgr, unsigned char *rgba, unsigned int width, unsigned int height, bool flip) { +#if defined(VISP_HAVE_SIMDLIB) if (!flip) { SimdRgbToBgra(bgr, width, height, width * 3, rgba, width * sizeof(vpRGBa), vpRGBa::alpha_default); } else { +#endif // if we have to flip the image, we start from the end last scanline so the // step is negative int lineStep = (flip) ? -(int)(width * 3) : (int)(width * 3); @@ -3480,14 +3572,16 @@ void vpImageConvert::BGRToRGBa(unsigned char *bgr, unsigned char *rgba, unsigned // go to the next line src += lineStep; } +#if defined(VISP_HAVE_SIMDLIB) } +#endif } /*! Converts a BGRa image to RGBa. - Flips the image verticaly if needed. - Assumes that rgba is already resized before caling this function. + Flips the image vertically if needed. + Assumes that rgba is already resized before calling this function. \note If flip is false, the SIMD lib is used to accelerate processing on x86 and ARM architecture. @@ -3500,10 +3594,12 @@ void vpImageConvert::BGRToRGBa(unsigned char *bgr, unsigned char *rgba, unsigned void vpImageConvert::BGRaToRGBa(unsigned char *bgra, unsigned char *rgba, unsigned int width, unsigned int height, bool flip) { +#if defined(VISP_HAVE_SIMDLIB) if (!flip) { SimdBgraToRgba(bgra, width, height, width * 4, rgba, width * 4); } else { +#endif // if we have to flip the image, we start from the end last scanline so the // step is negative int lineStep = (flip) ? -(int)(width * 4) : (int)(width * 4); @@ -3524,12 +3620,14 @@ void vpImageConvert::BGRaToRGBa(unsigned char *bgra, unsigned char *rgba, unsign // go to the next line src += lineStep; } +#if defined(VISP_HAVE_SIMDLIB) } +#endif } /*! Converts a BGR image to greyscale. - Flips the image verticaly if needed. + Flips the image vertically if needed. Assumes that grey is already resized. \note If flip is false, the SIMD lib is used to accelerate processing on x86 and ARM architecture. @@ -3544,13 +3642,14 @@ void vpImageConvert::BGRaToRGBa(unsigned char *bgra, unsigned char *rgba, unsign void vpImageConvert::BGRToGrey(unsigned char *bgr, unsigned char *grey, unsigned int width, unsigned int height, bool flip, unsigned int -#if defined _OPENMP +#if defined(_OPENMP) nThreads #endif ) { +#if defined(VISP_HAVE_SIMDLIB) if (!flip) { -#if defined _OPENMP +#if defined(_OPENMP) if (nThreads > 0) { omp_set_num_threads(static_cast(nThreads)); } @@ -3561,6 +3660,7 @@ void vpImageConvert::BGRToGrey(unsigned char *bgr, unsigned char *grey, unsigned } } else { +#endif // if we have to flip the image, we start from the end last scanline so // the step is negative int lineStep = (flip) ? -(int)(width * 3) : (int)(width * 3); @@ -3578,12 +3678,17 @@ void vpImageConvert::BGRToGrey(unsigned char *bgr, unsigned char *grey, unsigned // go to the next line src += lineStep; } +#if defined(VISP_HAVE_SIMDLIB) } +#endif +#if !defined(VISP_HAVE_SIMDLIB) && defined(_OPENMP) + (void)nThreads; +#endif } /*! Converts a BGRa image to greyscale. - Flips the image verticaly if needed. + Flips the image vertically if needed. Assumes that grey is already resized. \note If flip is false, the SIMD lib is used to accelerate processing on x86 and ARM architecture. @@ -3598,13 +3703,14 @@ void vpImageConvert::BGRToGrey(unsigned char *bgr, unsigned char *grey, unsigned void vpImageConvert::BGRaToGrey(unsigned char *bgra, unsigned char *grey, unsigned int width, unsigned int height, bool flip, unsigned int -#if defined _OPENMP +#if defined(_OPENMP) nThreads #endif ) { +#if defined(VISP_HAVE_SIMDLIB) if (!flip) { -#if defined _OPENMP +#if defined(_OPENMP) if (nThreads > 0) { omp_set_num_threads(static_cast(nThreads)); } @@ -3615,6 +3721,7 @@ void vpImageConvert::BGRaToGrey(unsigned char *bgra, unsigned char *grey, unsign } } else { +#endif // if we have to flip the image, we start from the end last scanline so // the step is negative int lineStep = (flip) ? -(int)(width * 4) : (int)(width * 4); @@ -3632,7 +3739,12 @@ void vpImageConvert::BGRaToGrey(unsigned char *bgra, unsigned char *grey, unsign // go to the next line src += lineStep; } +#if defined(VISP_HAVE_SIMDLIB) } +#endif +#if !defined(VISP_HAVE_SIMDLIB) && defined(_OPENMP) + (void)nThreads; +#endif } /*! @@ -3930,32 +4042,33 @@ void vpImageConvert::YCrCbToRGBa(unsigned char *ycrcb, unsigned char *rgba, unsi Example code using split: \code -#include -#include -#include + #include + #include + #include -int main() -{ - vpImage Ic; // A color image + int main() + { + vpImage Ic; // A color image - // Load a color image from the disk - vpImageIo::read(Ic,"image.ppm"); + // Load a color image from the disk + vpImageIo::read(Ic,"image.ppm"); - // Only R and B Channels are desired. - vpImage R, B; + // Only R and B Channels are desired. + vpImage R, B; - // Split Ic color image - // R and B will be resized in split function if needed - vpImageConvert::split(Ic, &R, nullptr, &B, nullptr); + // Split Ic color image + // R and B will be resized in split function if needed + vpImageConvert::split(Ic, &R, nullptr, &B, nullptr); - // Save the the R Channel. - vpImageIo::write(R, "RChannel.pgm"); -} + // Save the the R Channel. + vpImageIo::write(R, "RChannel.pgm"); + } \endcode */ void vpImageConvert::split(const vpImage &src, vpImage *pR, vpImage *pG, vpImage *pB, vpImage *pa) { +#if defined(VISP_HAVE_SIMDLIB) if (src.getSize() > 0) { if (pR) { pR->resize(src.getHeight(), src.getWidth()); @@ -3992,6 +4105,64 @@ void vpImageConvert::split(const vpImage &src, vpImage *p delete[] ptrA; } } +#else + size_t n = src.getNumberOfPixel(); + unsigned int height = src.getHeight(); + unsigned int width = src.getWidth(); + unsigned char *input; + unsigned char *dst; + + vpImage *tabChannel[4]; + + /* incrsrc[0] = 0; //init + incrsrc[1] = 0; //step after the first used channel + incrsrc[2] = 0; //step after the second used channel + incrsrc[3] = 0; + incrsrc[4] = 0; + */ + tabChannel[0] = pR; + tabChannel[1] = pG; + tabChannel[2] = pB; + tabChannel[3] = pa; + + size_t i; /* ordre */ + for (unsigned int j = 0; j < 4; j++) { + if (tabChannel[j] != NULL) { + if (tabChannel[j]->getHeight() != height || tabChannel[j]->getWidth() != width) { + tabChannel[j]->resize(height, width); + } + dst = (unsigned char *)tabChannel[j]->bitmap; + + input = (unsigned char *)src.bitmap + j; + i = 0; + // optimization + if (n >= 4) { + n -= 3; + for (; i < n; i += 4) { + *dst = *input; + input += 4; + dst++; + *dst = *input; + input += 4; + dst++; + *dst = *input; + input += 4; + dst++; + *dst = *input; + input += 4; + dst++; + } + n += 3; + } + + for (; i < n; i++) { + *dst = *input; + input += 4; + dst++; + } + } + } +#endif } /*! @@ -4035,11 +4206,14 @@ void vpImageConvert::merge(const vpImage *R, const vpImagebitmap, width, G->bitmap, width, B->bitmap, width, a->bitmap, width, width, height, reinterpret_cast(RGBa.bitmap), width * sizeof(vpRGBa)); } else { +#endif unsigned int size = width * height; for (unsigned int i = 0; i < size; i++) { if (R != nullptr) { @@ -4058,7 +4232,9 @@ void vpImageConvert::merge(const vpImage *R, const vpImagebitmap[i]; } } +#if defined(VISP_HAVE_SIMDLIB) } +#endif } else { throw vpException(vpException::dimensionError, "Mismatched dimensions!"); diff --git a/modules/core/src/image/vpImageTools.cpp b/modules/core/src/image/vpImageTools.cpp index dcf990084d..459de84589 100644 --- a/modules/core/src/image/vpImageTools.cpp +++ b/modules/core/src/image/vpImageTools.cpp @@ -37,7 +37,9 @@ #include #include +#if defined(VISP_HAVE_SIMDLIB) #include +#endif /*! Change the look up table (LUT) of an image. Considering pixel gray @@ -142,7 +144,14 @@ void vpImageTools::imageDifference(const vpImage &I1, const vpIma Idiff.resize(I1.getHeight(), I1.getWidth()); } +#if defined(VISP_HAVE_SIMDLIB) SimdImageDifference(I1.bitmap, I2.bitmap, I1.getSize(), Idiff.bitmap); +#else + for (unsigned int i = 0; i < I1.getSize(); ++i) { + int diff = I1.bitmap[i] - I2.bitmap[i] + 128; + Idiff.bitmap[i] = static_cast(std::max(std::min(diff, 255), 0)); + } +#endif } /*! @@ -171,8 +180,21 @@ void vpImageTools::imageDifference(const vpImage &I1, const vpImage(I1.bitmap), reinterpret_cast(I2.bitmap), I1.getSize() * 4, reinterpret_cast(Idiff.bitmap)); +#else + for (unsigned int i = 0; i < I1.getSize() * 4; ++i) { + int diffR = I1.bitmap[i].R - I2.bitmap[i].R + 128; + int diffG = I1.bitmap[i].G - I2.bitmap[i].G + 128; + int diffB = I1.bitmap[i].B - I2.bitmap[i].B + 128; + int diffA = I1.bitmap[i].A - I2.bitmap[i].A + 128; + Idiff.bitmap[i].R = static_cast(vpMath::maximum(vpMath::minimum(diffR, 255), 0)); + Idiff.bitmap[i].G = static_cast(vpMath::maximum(vpMath::minimum(diffG, 255), 0)); + Idiff.bitmap[i].B = static_cast(vpMath::maximum(vpMath::minimum(diffB, 255), 0)); + Idiff.bitmap[i].A = static_cast(vpMath::maximum(vpMath::minimum(diffA, 255), 0)); + } +#endif } /*! @@ -287,6 +309,7 @@ void vpImageTools::imageAdd(const vpImage &I1, const vpImage View; View img1(I1.getWidth(), I1.getHeight(), I1.getWidth(), View::Gray8, I1.bitmap); View img2(I2.getWidth(), I2.getHeight(), I2.getWidth(), View::Gray8, I2.bitmap); @@ -294,6 +317,14 @@ void vpImageTools::imageAdd(const vpImage &I1, const vpImage((short int)*ptr_I1 + (short int)*ptr_I2) : *ptr_I1 + *ptr_I2; + } +#endif } /*! @@ -320,6 +351,7 @@ void vpImageTools::imageSubtract(const vpImage &I1, const vpImage Ires.resize(I1.getHeight(), I1.getWidth()); } +#if defined(VISP_HAVE_SIMDLIB) typedef Simd::View View; View img1(I1.getWidth(), I1.getHeight(), I1.getWidth(), View::Gray8, I1.bitmap); View img2(I2.getWidth(), I2.getHeight(), I2.getWidth(), View::Gray8, I2.bitmap); @@ -327,6 +359,16 @@ void vpImageTools::imageSubtract(const vpImage &I1, const vpImage Simd::OperationBinary8u(img1, img2, imgAdd, saturate ? SimdOperationBinary8uSaturatedSubtraction : SimdOperationBinary8uSubtraction); +#else + unsigned char *ptr_I1 = I1.bitmap; + unsigned char *ptr_I2 = I2.bitmap; + unsigned char *ptr_Ires = Ires.bitmap; + for (unsigned int cpt = 0; cpt < Ires.getSize(); cpt++, ++ptr_I1, ++ptr_I2, ++ptr_Ires) { + *ptr_Ires = saturate ? + vpMath::saturate(static_cast(*ptr_I1) - static_cast(*ptr_I2)) : + *ptr_I1 - *ptr_I2; + } +#endif } /*! @@ -351,7 +393,7 @@ void vpImageTools::initUndistortMap(const vpCameraParameters &cam, unsigned int vpCameraParameters::vpCameraParametersProjType projModel = cam.get_projModel(); bool is_KannalaBrandt = - (projModel == vpCameraParameters::ProjWithKannalaBrandtDistortion); // Check the projection model used + (projModel == vpCameraParameters::ProjWithKannalaBrandtDistortion); // Check the projection model used float u0 = static_cast(cam.get_u0()); float v0 = static_cast(cam.get_v0()); @@ -499,7 +541,16 @@ double vpImageTools::normalizedCorrelation(const vpImage &I1, const vpIm double a2 = 0.0; double b2 = 0.0; +#if defined(VISP_HAVE_SIMDLIB) SimdNormalizedCorrelation(I1.bitmap, a, I2.bitmap, b, I1.getSize(), a2, b2, ab, useOptimized); +#else + for (unsigned int cpt = 0; cpt < I1.getSize(); cpt++) { + ab += (I1.bitmap[cpt] - a) * (I2.bitmap[cpt] - b); + a2 += vpMath::sqr(I1.bitmap[cpt] - a); + b2 += vpMath::sqr(I2.bitmap[cpt] - b); + } + (void)useOptimized; +#endif return ab / sqrt(a2 * b2); } @@ -556,7 +607,8 @@ double vpImageTools::interpolate(const vpImage &I, const vpImageP if (x1 == x2) { v1 = I(x1, y1); v2 = I(x1, y2); - } else { + } + else { v1 = (x2 - point.get_i()) * I(x1, y1) + (point.get_i() - x1) * I(x2, y1); v2 = (x2 - point.get_i()) * I(x1, y2) + (point.get_i() - x1) * I(x2, y2); } @@ -674,7 +726,7 @@ void vpImageTools::templateMatching(const vpImage &I, const vpIma I_tpl_double.bitmap[cpt] -= mean2; } -#if defined _OPENMP && _OPENMP >= 200711 // OpenMP 3.1 +#if defined(_OPENMP) && (_OPENMP >= 200711) // OpenMP 3.1 #pragma omp parallel for schedule(dynamic) for (unsigned int i = 0; i < I.getHeight() - height_tpl; i += step_v) { for (unsigned int j = 0; j < I.getWidth() - width_tpl; j += step_u) { @@ -688,17 +740,18 @@ void vpImageTools::templateMatching(const vpImage &I, const vpIma for (unsigned int cpt = 0, idx = 0; cpt < I.getHeight() - height_tpl; cpt += step_v, idx++) { vec_step_v[(size_t)idx] = cpt; } -#if defined _OPENMP // only to disable warning: ignoring #pragma omp parallel [-Wunknown-pragmas] +#if defined(_OPENMP) // only to disable warning: ignoring #pragma omp parallel [-Wunknown-pragmas] #pragma omp parallel for schedule(dynamic) #endif for (int cpt = 0; cpt < end; cpt++) { for (unsigned int j = 0; j < I.getWidth() - width_tpl; j += step_u) { I_score[vec_step_v[cpt]][j] = - normalizedCorrelation(I_double, I_tpl_double, II, IIsq, II_tpl, IIsq_tpl, vec_step_v[cpt], j); + normalizedCorrelation(I_double, I_tpl_double, II, IIsq, II_tpl, IIsq_tpl, vec_step_v[cpt], j); } } #endif - } else { + } + else { vpImage I_cur; for (unsigned int i = 0; i < I.getHeight() - height_tpl; i += step_v) { @@ -743,11 +796,20 @@ double vpImageTools::normalizedCorrelation(const vpImage &I1, const vpIm unsigned int i0, unsigned int j0) { double ab = 0.0; + +#if defined(VISP_HAVE_SIMDLIB) SimdNormalizedCorrelation2(I1.bitmap, I1.getWidth(), I2.bitmap, I2.getWidth(), I2.getHeight(), i0, j0, ab); +#else + for (unsigned int i = 0; i < I2.getHeight(); i++) { + for (unsigned int j = 0; j < I2.getWidth(); j++) { + ab += (I1[i0 + i][j0 + j]) * I2[i][j]; + } + } +#endif unsigned int height_tpl = I2.getHeight(), width_tpl = I2.getWidth(); const double sum1 = - (II[i0 + height_tpl][j0 + width_tpl] + II[i0][j0] - II[i0][j0 + width_tpl] - II[i0 + height_tpl][j0]); + (II[i0 + height_tpl][j0 + width_tpl] + II[i0][j0] - II[i0][j0 + width_tpl] - II[i0 + height_tpl][j0]); const double sum2 = (II_tpl[height_tpl][width_tpl] + II_tpl[0][0] - II_tpl[0][width_tpl] - II_tpl[height_tpl][0]); double a2 = ((IIsq[i0 + I2.getHeight()][j0 + I2.getWidth()] + IIsq[i0][j0] - IIsq[i0][j0 + I2.getWidth()] - @@ -775,7 +837,7 @@ void vpImageTools::remap(const vpImage &I, const vpArray2D & { Iundist.resize(I.getHeight(), I.getWidth()); -#if defined _OPENMP // only to disable warning: ignoring #pragma omp parallel [-Wunknown-pragmas] +#if defined(_OPENMP) // only to disable warning: ignoring #pragma omp parallel [-Wunknown-pragmas] #pragma omp parallel for schedule(dynamic) #endif for (int i_ = 0; i_ < static_cast(I.getHeight()); i_++) { @@ -796,7 +858,8 @@ void vpImageTools::remap(const vpImage &I, const vpArray2D & float value = lerp(col0, col1, dv); Iundist[i][j] = static_cast(value); - } else { + } + else { Iundist[i][j] = 0; } } @@ -818,15 +881,59 @@ void vpImageTools::remap(const vpImage &I, const vpArray2D &mapU, c { Iundist.resize(I.getHeight(), I.getWidth()); -#if defined _OPENMP // only to disable warning: ignoring #pragma omp parallel [-Wunknown-pragmas] +#if defined(_OPENMP) // only to disable warning: ignoring #pragma omp parallel [-Wunknown-pragmas] #pragma omp parallel for schedule(dynamic) #endif for (int i = 0; i < static_cast(I.getHeight()); i++) { +#if defined(VISP_HAVE_SIMDLIB) SimdRemap(reinterpret_cast(I.bitmap), 4, I.getWidth(), I.getHeight(), i * I.getWidth(), mapU.data, mapV.data, mapDu.data, mapDv.data, reinterpret_cast(Iundist.bitmap)); +#else + const unsigned int i_ = static_cast(i); + for (unsigned int j = 0; j < I.getWidth(); j++) { + + int u_round = mapU[i_][j]; + int v_round = mapV[i_][j]; + + float du = mapDu[i_][j]; + float dv = mapDv[i_][j]; + + if (0 <= u_round && 0 <= v_round && u_round < static_cast(I.getWidth()) - 1 + && v_round < static_cast(I.getHeight()) - 1) { + // process interpolation + float col0 = lerp(I[v_round][u_round].R, I[v_round][u_round + 1].R, du); + float col1 = lerp(I[v_round + 1][u_round].R, I[v_round + 1][u_round + 1].R, du); + float value = lerp(col0, col1, dv); + + Iundist[i][j].R = static_cast(value); + + col0 = lerp(I[v_round][u_round].G, I[v_round][u_round + 1].G, du); + col1 = lerp(I[v_round + 1][u_round].G, I[v_round + 1][u_round + 1].G, du); + value = lerp(col0, col1, dv); + + Iundist[i][j].G = static_cast(value); + + col0 = lerp(I[v_round][u_round].B, I[v_round][u_round + 1].B, du); + col1 = lerp(I[v_round + 1][u_round].B, I[v_round + 1][u_round + 1].B, du); + value = lerp(col0, col1, dv); + + Iundist[i][j].B = static_cast(value); + + col0 = lerp(I[v_round][u_round].A, I[v_round][u_round + 1].A, du); + col1 = lerp(I[v_round + 1][u_round].A, I[v_round + 1][u_round + 1].A, du); + value = lerp(col0, col1, dv); + + Iundist[i][j].A = static_cast(value); + } + else { + Iundist[i][j] = 0; + } + } +#endif } } +#if defined(VISP_HAVE_SIMDLIB) void vpImageTools::resizeSimdlib(const vpImage &Isrc, unsigned int resizeWidth, unsigned int resizeHeight, vpImage &Idst, int method) { @@ -850,6 +957,7 @@ void vpImageTools::resizeSimdlib(const vpImage &Isrc, unsigned in Simd::Resize(src, dst, method == INTERPOLATION_LINEAR ? SimdResizeMethodBilinear : SimdResizeMethodArea); } +#endif bool vpImageTools::checkFixedPoint(unsigned int x, unsigned int y, const vpMatrix &T, bool affine) { diff --git a/modules/core/src/math/matrix/vpColVector.cpp b/modules/core/src/math/matrix/vpColVector.cpp index 105f1dc77e..aae6a60a14 100644 --- a/modules/core/src/math/matrix/vpColVector.cpp +++ b/modules/core/src/math/matrix/vpColVector.cpp @@ -53,7 +53,9 @@ #include #include -#include +#if defined(VISP_HAVE_SIMDLIB) +#include +#endif vpColVector vpColVector::operator+(const vpColVector &v) const { @@ -645,8 +647,22 @@ double vpColVector::stdev(const vpColVector &v, bool useBesselCorrection) if (v.data == nullptr || v.size() == 0) { throw(vpException(vpException::dimensionError, "Cannot compute column vector stdev: vector empty")); } - +#if defined(VISP_HAVE_SIMDLIB) return SimdVectorStdev(v.data, v.rowNum, useBesselCorrection); +#else + double mean_value = v.sum() / v.size(); + double sum_squared_diff = 0.0; + for (size_t i = 0; i < v.size(); i++) { + sum_squared_diff += (v[i] - mean_value) * (v[i] - mean_value); + } + + double divisor = static_cast(v.size()); + if (useBesselCorrection) { + divisor = divisor - 1; + } + + return std::sqrt(sum_squared_diff / divisor); +#endif } vpMatrix vpColVector::skew(const vpColVector &v) @@ -800,9 +816,31 @@ int vpColVector::print(std::ostream &s, unsigned int length, char const *intro) return (int)(maxBefore + maxAfter); } -double vpColVector::sum() const { return SimdVectorSum(data, rowNum); } +double vpColVector::sum() const +{ +#if defined(VISP_HAVE_SIMDLIB) + return SimdVectorSum(data, rowNum); +#else + double sum = 0.0; + for (unsigned int i = 0; i < rowNum; ++i) { + sum += (*this)[i]; + } + return sum; +#endif +} -double vpColVector::sumSquare() const { return SimdVectorSumSquare(data, rowNum); } +double vpColVector::sumSquare() const +{ +#if defined(VISP_HAVE_SIMDLIB) + return SimdVectorSumSquare(data, rowNum); +#else + double sum_square = 0.0; + for (unsigned int i = 0; i < rowNum; ++i) { + sum_square += (*this)[i] * (*this)[i]; + } + return sum_square; +#endif +} double vpColVector::frobeniusNorm() const { @@ -819,9 +857,14 @@ vpColVector vpColVector::hadamard(const vpColVector &v) const vpColVector out; out.resize(rowNum, false); - +#if defined(VISP_HAVE_SIMDLIB) SimdVectorHadamard(data, v.data, rowNum, out.data); +#else +#endif + for (unsigned int i = 0; i < dsize; i++) { + out.data[i] = data[i] * v.data[i]; + } return out; } diff --git a/modules/core/src/math/matrix/vpMatrix.cpp b/modules/core/src/math/matrix/vpMatrix.cpp index 83a55a3ba9..1183539505 100644 --- a/modules/core/src/math/matrix/vpMatrix.cpp +++ b/modules/core/src/math/matrix/vpMatrix.cpp @@ -58,7 +58,9 @@ #include #include -#include +#if defined(VISP_HAVE_SIMDLIB) +#include +#endif #ifdef VISP_HAVE_LAPACK #ifdef VISP_HAVE_GSL @@ -480,7 +482,19 @@ void vpMatrix::transpose(vpMatrix &At) const } } else { +#if defined(VISP_HAVE_SIMDLIB) SimdMatTranspose(data, rowNum, colNum, At.data); +#else + // https://stackoverflow.com/a/21548079 + const int tileSize = 32; + for (unsigned int i = 0; i < rowNum; i += tileSize) { + for (unsigned int j = 0; j < colNum; j++) { + for (unsigned int b = 0; b < static_cast(tileSize) && i + b < rowNum; b++) { + At[j][i + b] = (*this)[i + b][j]; + } + } + } +#endif } } @@ -1251,7 +1265,22 @@ vpMatrix vpMatrix::operator*(const vpVelocityTwistMatrix &V) const #endif } else { +#if defined(VISP_HAVE_SIMDLIB) SimdMatMulTwist(data, rowNum, V.data, M.data); +#else + unsigned int VcolNum = V.getCols(); + unsigned int VrowNum = V.getRows(); + for (unsigned int i = 0; i < rowNum; i++) { + double *rowptri = rowPtrs[i]; + double *ci = M[i]; + for (unsigned int j = 0; j < VcolNum; j++) { + double s = 0; + for (unsigned int k = 0; k < VrowNum; k++) + s += rowptri[k] * V[k][j]; + ci[j] = s; + } + } +#endif } return M; @@ -1291,7 +1320,22 @@ vpMatrix vpMatrix::operator*(const vpForceTwistMatrix &V) const #endif } else { +#if defined(VISP_HAVE_SIMDLIB) SimdMatMulTwist(data, rowNum, V.data, M.data); +#else + unsigned int VcolNum = V.getCols(); + unsigned int VrowNum = V.getRows(); + for (unsigned int i = 0; i < rowNum; i++) { + double *rowptri = rowPtrs[i]; + double *ci = M[i]; + for (unsigned int j = 0; j < VcolNum; j++) { + double s = 0; + for (unsigned int k = 0; k < VrowNum; k++) + s += rowptri[k] * V[k][j]; + ci[j] = s; + } + } +#endif } return M; @@ -1759,7 +1803,13 @@ vpMatrix vpMatrix::hadamard(const vpMatrix &m) const vpMatrix out; out.resize(rowNum, colNum, false, false); +#if defined(VISP_HAVE_SIMDLIB) SimdVectorHadamard(data, m.data, dsize, out.data); +#else + for (unsigned int i = 0; i < dsize; ++i) { + out.data[i] = data[i] * m.data[i]; + } +#endif return out; } @@ -4949,1848 +4999,1848 @@ unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThr Using singular value decomposition, we have: - \f[ - {\bf A}_{m\times n} = {\bf U}_{m\times m} \; {\bf S}_{m\times n} \; {\bf - V^\top}_{n\times n} \f] \f[ - {\bf A}_{m\times n} = \left[\begin{array}{ccc}\mbox{Im} ({\bf A}) & | & - \mbox{Ker} ({\bf A}^\top) \end{array} \right] {\bf S}_{m\times n} - \left[ - \begin{array}{c} \left[\mbox{Im} ({\bf A}^\top)\right]^\top \\ - \\ - \hline \\ - \left[\mbox{Ker}({\bf A})\right]^\top \end{array}\right] +\f[ +{\bf A}_ { m\times n } = { \bf U }_ { m\times m } \; {\bf S}_ { m\times n } \; {\bf +V ^\top}_ { n\times n } \f] \f[ +{\bf A}_ { m\times n } = \left[\begin { array }{ccc}\mbox { Im } ({ \bf A }) &| & +\mbox { Ker } ({ \bf A }^\top) \end { array } \right] {\bf S}_ { m\times n } +\left[ + \begin { array }{c} \left[\mbox { Im } ({ \bf A }^\top)\right]^\top \\ + \\ + \hline \\ + \left[\mbox { Ker }({ \bf A })\right]^\top \end { array }\right] \f] - where the diagonal of \f${\bf S}_{m\times n}\f$ corresponds to the matrix + where the diagonal of \f$ { \bf S }_ { m\times n }\f$ corresponds to the matrix \f$A\f$ singular values. - This equation could be reformulated in a minimal way: + This equation could be reformulated in a minimal way : \f[ - {\bf A}_{m\times n} = \mbox{Im} ({\bf A}) \; {\bf S}_{r\times n} - \left[ - \begin{array}{c} \left[\mbox{Im} ({\bf A}^\top)\right]^\top \\ - \\ - \hline \\ - \left[\mbox{Ker}({\bf A})\right]^\top \end{array}\right] - \f] + {\bf A}_ { m\times n } = \mbox { Im } ({ \bf A }) \; {\bf S}_ { r\times n } + \left[ + \begin { array }{c} \left[\mbox { Im } ({ \bf A }^\top)\right]^\top \\ + \\ + \hline \\ + \left[\mbox { Ker }({ \bf A })\right]^\top \end { array }\right] + \f] - where the diagonal of \f${\bf S}_{r\times n}\f$ corresponds to the matrix - \f$A\f$ first r singular values. + where the diagonal of \f$ { \bf S }_ { r\times n }\f$ corresponds to the matrix + \f$A\f$ first r singular values. - The null space of a matrix \f$\bf A\f$ is defined as \f$\mbox{Ker}({\bf A}) - = { {\bf X} : {\bf A}*{\bf X} = {\bf 0}}\f$. + The null space of a matrix \f$\bf A\f$ is defined as \f$\mbox { Ker }({ \bf A }) + = { {\bf X} : {\bf A} *{\bf X} = {\bf 0} }\f$. - \param Ap: The Moore-Penros pseudo inverse \f$ {\bf A}^+ \f$. + \param Ap : The Moore-Penros pseudo inverse \f$ { \bf A }^+\f$. - \param sv: Vector corresponding to matrix \f$A\f$ singular values. The size - of this vector is equal to min(m, n). + \param sv : Vector corresponding to matrix \f$A\f$ singular values.The size + of this vector is equal to min(m, n). - \param[in] rank_in : Known rank of the matrix. + \param[in] rank_in : Known rank of the matrix. - \param imA: \f$\mbox{Im}({\bf A})\f$ that is a m-by-r matrix. + \param imA : \f$\mbox { Im }({ \bf A })\f$ that is a m-by-r matrix. - \param imAt: \f$\mbox{Im}({\bf A}^T)\f$ that is n-by-r matrix. + \param imAt : \f$\mbox { Im }({ \bf A }^T)\f$ that is n-by-r matrix. - \param kerAt: The matrix that contains the null space (kernel) of \f$\bf - A\f$ defined by the matrix \f${\bf X}^T\f$. If matrix \f$\bf A\f$ is full - rank, the dimension of \c kerAt is (0, n), otherwise the dimension is (n-r, - n). This matrix is thus the transpose of \f$\mbox{Ker}({\bf A})\f$. + \param kerAt : The matrix that contains the null space(kernel) of \f$\bf + A\f$ defined by the matrix \f$ { \bf X }^T\f$.If matrix \f$\bf A\f$ is full + rank, the dimension of \c kerAt is(0, n), otherwise the dimension is(n-r, + n).This matrix is thus the transpose of \f$\mbox { Ker }({ \bf A })\f$. - \return The rank of the matrix \f$\bf A\f$. + \return The rank of the matrix \f$\bf A\f$. - Here an example to compute the pseudo-inverse of a 2-by-3 matrix that is rank 2. + Here an example to compute the pseudo-inverse of a 2-by-3 matrix that is rank 2. -\code + \code #include -int main() -{ - vpMatrix A(2, 3); - - A[0][0] = 2; A[0][1] = 3; A[0][2] = 5; - A[1][0] = -4; A[1][1] = 2; A[1][2] = 3; + int main() + { + vpMatrix A(2, 3); - A.print(std::cout, 10, "A: "); + A[0][0] = 2; A[0][1] = 3; A[0][2] = 5; + A[1][0] = -4; A[1][1] = 2; A[1][2] = 3; - vpColVector sv; - vpMatrix A_p, imA, imAt, kerAt; - int rank_in = 2; - int rank_out = A.pseudoInverse(A_p, sv, rank_in, imA, imAt, kerAt); - if (rank_out != rank_in) { - std::cout << "There is a possibility that the pseudo-inverse in wrong." << std::endl; - std::cout << "Are you sure that the matrix rank is " << rank_in << std::endl; - } + A.print(std::cout, 10, "A: "); - A_p.print(std::cout, 10, "A^+ (pseudo-inverse): "); - std::cout << "Rank in : " << rank_in << std::endl; - std::cout << "Rank out: " << rank_out << std::endl; - std::cout << "Singular values: " << sv.t() << std::endl; - imA.print(std::cout, 10, "Im(A): "); - imAt.print(std::cout, 10, "Im(A^T): "); + vpColVector sv; + vpMatrix A_p, imA, imAt, kerAt; + int rank_in = 2; + int rank_out = A.pseudoInverse(A_p, sv, rank_in, imA, imAt, kerAt); + if (rank_out != rank_in) { + std::cout << "There is a possibility that the pseudo-inverse in wrong." << std::endl; + std::cout << "Are you sure that the matrix rank is " << rank_in << std::endl; + } - if (kerAt.size()) { - kerAt.t().print(std::cout, 10, "Ker(A): "); - } - else { - std::cout << "Ker(A) empty " << std::endl; - } + A_p.print(std::cout, 10, "A^+ (pseudo-inverse): "); + std::cout << "Rank in : " << rank_in << std::endl; + std::cout << "Rank out: " << rank_out << std::endl; + std::cout << "Singular values: " << sv.t() << std::endl; + imA.print(std::cout, 10, "Im(A): "); + imAt.print(std::cout, 10, "Im(A^T): "); - // Reconstruct matrix A from ImA, ImAt, KerAt - vpMatrix S(rank, A.getCols()); - for (unsigned int i = 0; i< rank_in; i++) - S[i][i] = sv[i]; - vpMatrix Vt(A.getCols(), A.getCols()); - Vt.insert(imAt.t(), 0, 0); - Vt.insert(kerAt, rank, 0); - (imA * S * Vt).print(std::cout, 10, "Im(A) * S * [Im(A^T) | Ker(A)]^T:"); -} -\endcode + if (kerAt.size()) { + kerAt.t().print(std::cout, 10, "Ker(A): "); + } + else { + std::cout << "Ker(A) empty " << std::endl; + } -Once build, the previous example produces the following output : -\code -A : [2, 3] = -2 3 5 --4 2 3 -A^+(pseudo-inverse) : [3, 2] = -0.117899 -0.190782 -0.065380 0.039657 -0.113612 0.052518 -Rank in : 2 -Rank out : 2 -Singular values : 6.874359351 4.443330227 -Im(A) : [2, 2] = -0.81458 -0.58003 -0.58003 0.81458 -Im(A^T) : [3, 2] = --0.100515 -0.994397 -0.524244 -0.024967 -0.845615 -0.102722 -Ker(A) : [3, 1] = --0.032738 --0.851202 -0.523816 -Im(A) * S *[Im(A^T) | Ker(A)]^T : [2, 3] = -2 3 5 --4 2 3 -\endcode -*/ -int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA, vpMatrix &imAt, - vpMatrix &kerAt) const -{ + // Reconstruct matrix A from ImA, ImAt, KerAt + vpMatrix S(rank, A.getCols()); + for (unsigned int i = 0; i< rank_in; i++) + S[i][i] = sv[i]; + vpMatrix Vt(A.getCols(), A.getCols()); + Vt.insert(imAt.t(), 0, 0); + Vt.insert(kerAt, rank, 0); + (imA * S * Vt).print(std::cout, 10, "Im(A) * S * [Im(A^T) | Ker(A)]^T:"); + } + \endcode + + Once build, the previous example produces the following output : + \code + A : [2, 3] = + 2 3 5 + -4 2 3 + A^+(pseudo-inverse) : [3, 2] = + 0.117899 -0.190782 + 0.065380 0.039657 + 0.113612 0.052518 + Rank in : 2 + Rank out : 2 + Singular values : 6.874359351 4.443330227 + Im(A) : [2, 2] = + 0.81458 -0.58003 + 0.58003 0.81458 + Im(A^T) : [3, 2] = + -0.100515 -0.994397 + 0.524244 -0.024967 + 0.845615 -0.102722 + Ker(A) : [3, 1] = + -0.032738 + -0.851202 + 0.523816 + Im(A) * S *[Im(A^T) | Ker(A)]^T : [2, 3] = + 2 3 5 + -4 2 3 + \endcode + */ + int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA, vpMatrix &imAt, + vpMatrix &kerAt) const + { #if defined(VISP_HAVE_LAPACK) - return pseudoInverseLapack(Ap, sv, rank_in, imA, imAt, kerAt); + return pseudoInverseLapack(Ap, sv, rank_in, imA, imAt, kerAt); #elif defined(VISP_HAVE_EIGEN3) - return pseudoInverseEigen3(Ap, sv, rank_in, imA, imAt, kerAt); + return pseudoInverseEigen3(Ap, sv, rank_in, imA, imAt, kerAt); #elif defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1 - return pseudoInverseOpenCV(Ap, sv, rank_in, imA, imAt, kerAt); + return pseudoInverseOpenCV(Ap, sv, rank_in, imA, imAt, kerAt); #else - (void)Ap; - (void)sv; - (void)svThreshold; - (void)imA; - (void)imAt; - (void)kerAt; - throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. " - "Install Lapack, Eigen3 or OpenCV 3rd party")); + (void)Ap; + (void)sv; + (void)svThreshold; + (void)imA; + (void)imAt; + (void)kerAt; + throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. " + "Install Lapack, Eigen3 or OpenCV 3rd party")); #endif -} - -/*! - Extract a column vector from a matrix. - \warning All the indexes start from 0 in this function. - \param j : Index of the column to extract. If col=0, the first column is extracted. - \param i_begin : Index of the row that gives the location of the first element - of the column vector to extract. - \param column_size : Size of the column vector to extract. - \return The extracted column vector. - - The following example shows how to use this function: - \code -#include -#include + } -int main() -{ - vpMatrix A(4,4); + /*! + Extract a column vector from a matrix. + \warning All the indexes start from 0 in this function. + \param j : Index of the column to extract. If col=0, the first column is extracted. + \param i_begin : Index of the row that gives the location of the first element + of the column vector to extract. + \param column_size : Size of the column vector to extract. + \return The extracted column vector. - for(unsigned int i=0; i < A.getRows(); i++) - for(unsigned int j=0; j < A.getCols(); j++) - A[i][j] = i*A.getCols()+j; + The following example shows how to use this function: + \code + #include + #include - A.print(std::cout, 4); + int main() + { + vpMatrix A(4,4); - vpColVector cv = A.getCol(1, 1, 3); - std::cout << "Column vector: \n" << cv << std::endl; -} - \endcode -It produces the following output: - \code -[4,4]= - 0 1 2 3 - 4 5 6 7 - 8 9 10 11 - 12 13 14 15 -column vector: -5 -9 -13 - \endcode - */ -vpColVector vpMatrix::getCol(unsigned int j, unsigned int i_begin, unsigned int column_size) const -{ - if (i_begin + column_size > getRows() || j >= getCols()) - throw(vpException(vpException::dimensionError, "Unable to extract column %u from the %ux%u matrix", j, getRows(), - getCols())); - vpColVector c(column_size); - for (unsigned int i = 0; i < column_size; i++) - c[i] = (*this)[i_begin + i][j]; - return c; -} + for(unsigned int i=0; i < A.getRows(); i++) + for(unsigned int j=0; j < A.getCols(); j++) + A[i][j] = i*A.getCols()+j; -/*! - Extract a column vector from a matrix. - \warning All the indexes start from 0 in this function. - \param j : Index of the column to extract. If j=0, the first column is extracted. - \return The extracted column vector. + A.print(std::cout, 4); - The following example shows how to use this function: - \code -#include -#include + vpColVector cv = A.getCol(1, 1, 3); + std::cout << "Column vector: \n" << cv << std::endl; + } + \endcode + It produces the following output: + \code + [4,4]= + 0 1 2 3 + 4 5 6 7 + 8 9 10 11 + 12 13 14 15 + column vector: + 5 + 9 + 13 + \endcode + */ + vpColVector vpMatrix::getCol(unsigned int j, unsigned int i_begin, unsigned int column_size) const + { + if (i_begin + column_size > getRows() || j >= getCols()) + throw(vpException(vpException::dimensionError, "Unable to extract column %u from the %ux%u matrix", j, getRows(), + getCols())); + vpColVector c(column_size); + for (unsigned int i = 0; i < column_size; i++) + c[i] = (*this)[i_begin + i][j]; + return c; + } -int main() -{ - vpMatrix A(4,4); + /*! + Extract a column vector from a matrix. + \warning All the indexes start from 0 in this function. + \param j : Index of the column to extract. If j=0, the first column is extracted. + \return The extracted column vector. - for(unsigned int i=0; i < A.getRows(); i++) - for(unsigned int j=0; j < A.getCols(); j++) - A[i][j] = i*A.getCols()+j; + The following example shows how to use this function: + \code + #include + #include - A.print(std::cout, 4); + int main() + { + vpMatrix A(4,4); - vpColVector cv = A.getCol(1); - std::cout << "Column vector: \n" << cv << std::endl; -} - \endcode -It produces the following output: - \code -[4,4]= - 0 1 2 3 - 4 5 6 7 - 8 9 10 11 - 12 13 14 15 -column vector: -1 -5 -9 -13 - \endcode - */ -vpColVector vpMatrix::getCol(unsigned int j) const { return getCol(j, 0, rowNum); } + for(unsigned int i=0; i < A.getRows(); i++) + for(unsigned int j=0; j < A.getCols(); j++) + A[i][j] = i*A.getCols()+j; -/*! - Extract a row vector from a matrix. - \warning All the indexes start from 0 in this function. - \param i : Index of the row to extract. If i=0, the first row is extracted. - \return The extracted row vector. + A.print(std::cout, 4); - The following example shows how to use this function: - \code -#include -#include + vpColVector cv = A.getCol(1); + std::cout << "Column vector: \n" << cv << std::endl; + } + \endcode + It produces the following output: + \code + [4,4]= + 0 1 2 3 + 4 5 6 7 + 8 9 10 11 + 12 13 14 15 + column vector: + 1 + 5 + 9 + 13 + \endcode + */ + vpColVector vpMatrix::getCol(unsigned int j) const { return getCol(j, 0, rowNum); } + + /*! + Extract a row vector from a matrix. + \warning All the indexes start from 0 in this function. + \param i : Index of the row to extract. If i=0, the first row is extracted. + \return The extracted row vector. + + The following example shows how to use this function: + \code + #include + #include + + int main() + { + vpMatrix A(4,4); + + for(unsigned int i=0; i < A.getRows(); i++) + for(unsigned int j=0; j < A.getCols(); j++) + A[i][j] = i*A.getCols()+j; + + A.print(std::cout, 4); + + vpRowVector rv = A.getRow(1); + std::cout << "Row vector: \n" << rv << std::endl; + } \endcode + It produces the following output: + \code + [4,4]= + 0 1 2 3 + 4 5 6 7 + 8 9 10 11 + 12 13 14 15 + Row vector: + 4 5 6 7 + \endcode + */ + vpRowVector vpMatrix::getRow(unsigned int i) const { return getRow(i, 0, colNum); } + + /*! + Extract a row vector from a matrix. + \warning All the indexes start from 0 in this function. + \param i : Index of the row to extract. If i=0, the first row is extracted. + \param j_begin : Index of the column that gives the location of the first + element of the row vector to extract. + \param row_size : Size of the row vector to extract. + \return The extracted row vector. + + The following example shows how to use this function: + \code + #include + #include + + int main() + { + vpMatrix A(4,4); + + for(unsigned int i=0; i < A.getRows(); i++) + for(unsigned int j=0; j < A.getCols(); j++) + A[i][j] = i*A.getCols()+j; + + A.print(std::cout, 4); + + vpRowVector rv = A.getRow(1, 1, 3); + std::cout << "Row vector: \n" << rv << std::endl; + } + \endcode + It produces the following output: + \code + [4,4]= + 0 1 2 3 + 4 5 6 7 + 8 9 10 11 + 12 13 14 15 + Row vector: + 5 6 7 + \endcode + */ + vpRowVector vpMatrix::getRow(unsigned int i, unsigned int j_begin, unsigned int row_size) const + { + if (j_begin + row_size > colNum || i >= rowNum) + throw(vpException(vpException::dimensionError, "Unable to extract a row vector from the matrix")); + + vpRowVector r(row_size); + if (r.data != nullptr && data != nullptr) { + memcpy(r.data, (*this)[i] + j_begin, row_size * sizeof(double)); + } -int main() -{ - vpMatrix A(4,4); + return r; + } - for(unsigned int i=0; i < A.getRows(); i++) - for(unsigned int j=0; j < A.getCols(); j++) - A[i][j] = i*A.getCols()+j; + /*! + Extract a diagonal vector from a matrix. - A.print(std::cout, 4); + \return The diagonal of the matrix. - vpRowVector rv = A.getRow(1); - std::cout << "Row vector: \n" << rv << std::endl; -} \endcode -It produces the following output: - \code -[4,4]= - 0 1 2 3 - 4 5 6 7 - 8 9 10 11 - 12 13 14 15 -Row vector: -4 5 6 7 - \endcode - */ -vpRowVector vpMatrix::getRow(unsigned int i) const { return getRow(i, 0, colNum); } + \warning An empty vector is returned if the matrix is empty. -/*! - Extract a row vector from a matrix. - \warning All the indexes start from 0 in this function. - \param i : Index of the row to extract. If i=0, the first row is extracted. - \param j_begin : Index of the column that gives the location of the first -element of the row vector to extract. - \param row_size : Size of the row vector to extract. - \return The extracted row vector. - - The following example shows how to use this function: - \code -#include -#include + The following example shows how to use this function: + \code + #include -int main() -{ - vpMatrix A(4,4); + int main() + { + vpMatrix A(3,4); - for(unsigned int i=0; i < A.getRows(); i++) - for(unsigned int j=0; j < A.getCols(); j++) - A[i][j] = i*A.getCols()+j; + for(unsigned int i=0; i < A.getRows(); i++) + for(unsigned int j=0; j < A.getCols(); j++) + A[i][j] = i*A.getCols()+j; - A.print(std::cout, 4); + A.print(std::cout, 4); - vpRowVector rv = A.getRow(1, 1, 3); - std::cout << "Row vector: \n" << rv << std::endl; -} - \endcode -It produces the following output: - \code -[4,4]= - 0 1 2 3 - 4 5 6 7 - 8 9 10 11 - 12 13 14 15 -Row vector: -5 6 7 - \endcode - */ -vpRowVector vpMatrix::getRow(unsigned int i, unsigned int j_begin, unsigned int row_size) const -{ - if (j_begin + row_size > colNum || i >= rowNum) - throw(vpException(vpException::dimensionError, "Unable to extract a row vector from the matrix")); + vpColVector diag = A.getDiag(); + std::cout << "Diag vector: \n" << diag.t() << std::endl; + } + \endcode + It produces the following output: + \code + [3,4]= + 0 1 2 3 + 4 5 6 7 + 8 9 10 11 + Diag vector: + 0 5 10 + \endcode + */ + vpColVector vpMatrix::getDiag() const + { + unsigned int min_size = std::min(rowNum, colNum); + vpColVector diag; + + if (min_size > 0) { + diag.resize(min_size, false); + + for (unsigned int i = 0; i < min_size; i++) { + diag[i] = (*this)[i][i]; + } + } - vpRowVector r(row_size); - if (r.data != nullptr && data != nullptr) { - memcpy(r.data, (*this)[i] + j_begin, row_size * sizeof(double)); - } + return diag; + } - return r; -} + /*! + Stack matrix \e B to the end of matrix \e A and return the resulting matrix + [ A B ]^T -/*! - Extract a diagonal vector from a matrix. + \param A : Upper matrix. + \param B : Lower matrix. + \return Stacked matrix [ A B ]^T - \return The diagonal of the matrix. + \warning A and B must have the same number of columns. + */ + vpMatrix vpMatrix::stack(const vpMatrix &A, const vpMatrix &B) + { + vpMatrix C; - \warning An empty vector is returned if the matrix is empty. + vpMatrix::stack(A, B, C); - The following example shows how to use this function: - \code -#include + return C; + } -int main() -{ - vpMatrix A(3,4); + /*! + Stack matrix \e B to the end of matrix \e A and return the resulting matrix + in \e C. + + \param A : Upper matrix. + \param B : Lower matrix. + \param C : Stacked matrix C = [ A B ]^T + + \warning A and B must have the same number of columns. A and C, B and C must + be two different objects. + */ + void vpMatrix::stack(const vpMatrix &A, const vpMatrix &B, vpMatrix &C) + { + unsigned int nra = A.getRows(); + unsigned int nrb = B.getRows(); + + if (nra != 0) { + if (A.getCols() != B.getCols()) { + throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx%d) matrix", A.getRows(), + A.getCols(), B.getRows(), B.getCols())); + } + } - for(unsigned int i=0; i < A.getRows(); i++) - for(unsigned int j=0; j < A.getCols(); j++) - A[i][j] = i*A.getCols()+j; + if (A.data != nullptr && A.data == C.data) { + std::cerr << "A and C must be two different objects!" << std::endl; + return; + } - A.print(std::cout, 4); + if (B.data != nullptr && B.data == C.data) { + std::cerr << "B and C must be two different objects!" << std::endl; + return; + } - vpColVector diag = A.getDiag(); - std::cout << "Diag vector: \n" << diag.t() << std::endl; -} - \endcode -It produces the following output: - \code -[3,4]= - 0 1 2 3 - 4 5 6 7 - 8 9 10 11 -Diag vector: -0 5 10 - \endcode - */ -vpColVector vpMatrix::getDiag() const -{ - unsigned int min_size = std::min(rowNum, colNum); - vpColVector diag; + C.resize(nra + nrb, B.getCols(), false, false); - if (min_size > 0) { - diag.resize(min_size, false); + if (C.data != nullptr && A.data != nullptr && A.size() > 0) { + // Copy A in C + memcpy(C.data, A.data, sizeof(double) * A.size()); + } - for (unsigned int i = 0; i < min_size; i++) { - diag[i] = (*this)[i][i]; + if (C.data != nullptr && B.data != nullptr && B.size() > 0) { + // Copy B in C + memcpy(C.data + A.size(), B.data, sizeof(double) * B.size()); + } } - } - return diag; -} + /*! + Stack row vector \e r to matrix \e A and return the resulting matrix [ A r ]^T -/*! - Stack matrix \e B to the end of matrix \e A and return the resulting matrix - [ A B ]^T + \param A : Upper matrix. + \param r : Lower row vector. + \return Stacked matrix [ A r ]^T - \param A : Upper matrix. - \param B : Lower matrix. - \return Stacked matrix [ A B ]^T + \warning \e A and \e r must have the same number of columns. + */ + vpMatrix vpMatrix::stack(const vpMatrix &A, const vpRowVector &r) + { + vpMatrix C; + vpMatrix::stack(A, r, C); - \warning A and B must have the same number of columns. -*/ -vpMatrix vpMatrix::stack(const vpMatrix &A, const vpMatrix &B) -{ - vpMatrix C; + return C; + } - vpMatrix::stack(A, B, C); + /*! + Stack row vector \e r to the end of matrix \e A and return the resulting + matrix in \e C. + + \param A : Upper matrix. + \param r : Lower row vector. + \param C : Stacked matrix C = [ A r ]^T + + \warning A and r must have the same number of columns. A and C must be two + different objects. + */ + void vpMatrix::stack(const vpMatrix &A, const vpRowVector &r, vpMatrix &C) + { + if (A.data != nullptr && A.data == C.data) { + std::cerr << "A and C must be two different objects!" << std::endl; + return; + } - return C; -} + C = A; + C.stack(r); + } -/*! - Stack matrix \e B to the end of matrix \e A and return the resulting matrix - in \e C. + /*! + Stack column vector \e c to matrix \e A and return the resulting matrix [ A c ] - \param A : Upper matrix. - \param B : Lower matrix. - \param C : Stacked matrix C = [ A B ]^T + \param A : Left matrix. + \param c : Right column vector. + \return Stacked matrix [ A c ] - \warning A and B must have the same number of columns. A and C, B and C must - be two different objects. -*/ -void vpMatrix::stack(const vpMatrix &A, const vpMatrix &B, vpMatrix &C) -{ - unsigned int nra = A.getRows(); - unsigned int nrb = B.getRows(); + \warning \e A and \e c must have the same number of rows. + */ + vpMatrix vpMatrix::stack(const vpMatrix &A, const vpColVector &c) + { + vpMatrix C; + vpMatrix::stack(A, c, C); - if (nra != 0) { - if (A.getCols() != B.getCols()) { - throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx%d) matrix", A.getRows(), - A.getCols(), B.getRows(), B.getCols())); + return C; } - } - if (A.data != nullptr && A.data == C.data) { - std::cerr << "A and C must be two different objects!" << std::endl; - return; - } + /*! + Stack column vector \e c to the end of matrix \e A and return the resulting + matrix in \e C. + + \param A : Left matrix. + \param c : Right column vector. + \param C : Stacked matrix C = [ A c ] + + \warning A and c must have the same number of rows. A and C must be two + different objects. + */ + void vpMatrix::stack(const vpMatrix &A, const vpColVector &c, vpMatrix &C) + { + if (A.data != nullptr && A.data == C.data) { + std::cerr << "A and C must be two different objects!" << std::endl; + return; + } - if (B.data != nullptr && B.data == C.data) { - std::cerr << "B and C must be two different objects!" << std::endl; - return; - } + C = A; + C.stack(c); + } - C.resize(nra + nrb, B.getCols(), false, false); + /*! + Insert matrix B in matrix A at the given position. - if (C.data != nullptr && A.data != nullptr && A.size() > 0) { - // Copy A in C - memcpy(C.data, A.data, sizeof(double) * A.size()); - } + \param A : Main matrix. + \param B : Matrix to insert. + \param r : Index of the row where to add the matrix. + \param c : Index of the column where to add the matrix. + \return Matrix with B insert in A. - if (C.data != nullptr && B.data != nullptr && B.size() > 0) { - // Copy B in C - memcpy(C.data + A.size(), B.data, sizeof(double) * B.size()); - } -} + \warning Throw exception if the sizes of the matrices do not allow the + insertion. + */ + vpMatrix vpMatrix::insert(const vpMatrix &A, const vpMatrix &B, unsigned int r, unsigned int c) + { + vpArray2D C; -/*! - Stack row vector \e r to matrix \e A and return the resulting matrix [ A r ]^T + vpArray2D::insert(A, B, C, r, c); - \param A : Upper matrix. - \param r : Lower row vector. - \return Stacked matrix [ A r ]^T + return vpMatrix(C); + } - \warning \e A and \e r must have the same number of columns. -*/ -vpMatrix vpMatrix::stack(const vpMatrix &A, const vpRowVector &r) -{ - vpMatrix C; - vpMatrix::stack(A, r, C); + /*! + \relates vpMatrix + Insert matrix B in matrix A at the given position. - return C; -} + \param A : Main matrix. + \param B : Matrix to insert. + \param C : Result matrix. + \param r : Index of the row where to insert matrix B. + \param c : Index of the column where to insert matrix B. -/*! - Stack row vector \e r to the end of matrix \e A and return the resulting - matrix in \e C. + \warning Throw exception if the sizes of the matrices do not + allow the insertion. + */ + void vpMatrix::insert(const vpMatrix &A, const vpMatrix &B, vpMatrix &C, unsigned int r, unsigned int c) + { + vpArray2D C_array; - \param A : Upper matrix. - \param r : Lower row vector. - \param C : Stacked matrix C = [ A r ]^T + vpArray2D::insert(A, B, C_array, r, c); - \warning A and r must have the same number of columns. A and C must be two - different objects. -*/ -void vpMatrix::stack(const vpMatrix &A, const vpRowVector &r, vpMatrix &C) -{ - if (A.data != nullptr && A.data == C.data) { - std::cerr << "A and C must be two different objects!" << std::endl; - return; - } + C = C_array; + } - C = A; - C.stack(r); -} + /*! + Juxtapose to matrices C = [ A B ]. -/*! - Stack column vector \e c to matrix \e A and return the resulting matrix [ A c ] + \f$ C = \left( \begin{array}{cc} A & B \end{array}\right) \f$ - \param A : Left matrix. - \param c : Right column vector. - \return Stacked matrix [ A c ] + \param A : Left matrix. + \param B : Right matrix. + \return Juxtaposed matrix C = [ A B ] - \warning \e A and \e c must have the same number of rows. -*/ -vpMatrix vpMatrix::stack(const vpMatrix &A, const vpColVector &c) -{ - vpMatrix C; - vpMatrix::stack(A, c, C); + \warning A and B must have the same number of rows. + */ + vpMatrix vpMatrix::juxtaposeMatrices(const vpMatrix &A, const vpMatrix &B) + { + vpMatrix C; - return C; -} + juxtaposeMatrices(A, B, C); -/*! - Stack column vector \e c to the end of matrix \e A and return the resulting - matrix in \e C. + return C; + } - \param A : Left matrix. - \param c : Right column vector. - \param C : Stacked matrix C = [ A c ] + /*! + \relates vpMatrix + Juxtapose to matrices C = [ A B ]. - \warning A and c must have the same number of rows. A and C must be two - different objects. -*/ -void vpMatrix::stack(const vpMatrix &A, const vpColVector &c, vpMatrix &C) -{ - if (A.data != nullptr && A.data == C.data) { - std::cerr << "A and C must be two different objects!" << std::endl; - return; - } + \f$ C = \left( \begin{array}{cc} A & B \end{array}\right) \f$ - C = A; - C.stack(c); -} + \param A : Left matrix. + \param B : Right matrix. + \param C : Juxtaposed matrix C = [ A B ] -/*! - Insert matrix B in matrix A at the given position. + \warning A and B must have the same number of rows. + */ + void vpMatrix::juxtaposeMatrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C) + { + unsigned int nca = A.getCols(); + unsigned int ncb = B.getCols(); - \param A : Main matrix. - \param B : Matrix to insert. - \param r : Index of the row where to add the matrix. - \param c : Index of the column where to add the matrix. - \return Matrix with B insert in A. + if (nca != 0) { + if (A.getRows() != B.getRows()) { + throw(vpException(vpException::dimensionError, "Cannot juxtapose (%dx%d) matrix with (%dx%d) matrix", A.getRows(), + A.getCols(), B.getRows(), B.getCols())); + } + } - \warning Throw exception if the sizes of the matrices do not allow the - insertion. -*/ -vpMatrix vpMatrix::insert(const vpMatrix &A, const vpMatrix &B, unsigned int r, unsigned int c) -{ - vpArray2D C; + if (B.getRows() == 0 || nca + ncb == 0) { + std::cerr << "B.getRows() == 0 || nca+ncb == 0" << std::endl; + return; + } - vpArray2D::insert(A, B, C, r, c); + C.resize(B.getRows(), nca + ncb, false, false); - return vpMatrix(C); -} + C.insert(A, 0, 0); + C.insert(B, 0, nca); + } -/*! - \relates vpMatrix - Insert matrix B in matrix A at the given position. + //-------------------------------------------------------------------- + // Output + //-------------------------------------------------------------------- + + /*! + + Pretty print a matrix. The data are tabulated. + The common widths before and after the decimal point + are set with respect to the parameter `length`. + + \param s : Stream used for the printing. + + \param length : The suggested width of each matrix element. + If needed, the used `length` grows in order to accommodate the whole integral part, + and shrinks the decimal part to print only `length` digits. + \param intro : The introduction which is printed before the matrix. + Can be set to zero (or omitted), in which case + the introduction is not printed. + + \return Returns the common total width for all matrix elements. + + \sa std::ostream &operator<<(std::ostream &s, const vpArray2D &A) + */ + int vpMatrix::print(std::ostream &s, unsigned int length, const std::string &intro) const + { + typedef std::string::size_type size_type; + + unsigned int m = getRows(); + unsigned int n = getCols(); + + std::vector values(m * n); + std::ostringstream oss; + std::ostringstream ossFixed; + std::ios_base::fmtflags original_flags = oss.flags(); + + ossFixed.setf(std::ios::fixed, std::ios::floatfield); + + size_type maxBefore = 0; // the length of the integral part + size_type maxAfter = 0; // number of decimals plus + // one place for the decimal point + for (unsigned int i = 0; i < m; ++i) { + for (unsigned int j = 0; j < n; ++j) { + oss.str(""); + oss << (*this)[i][j]; + if (oss.str().find("e") != std::string::npos) { + ossFixed.str(""); + ossFixed << (*this)[i][j]; + oss.str(ossFixed.str()); + } + + values[i * n + j] = oss.str(); + size_type thislen = values[i * n + j].size(); + size_type p = values[i * n + j].find('.'); + + if (p == std::string::npos) { + maxBefore = vpMath::maximum(maxBefore, thislen); + // maxAfter remains the same + } + else { + maxBefore = vpMath::maximum(maxBefore, p); + maxAfter = vpMath::maximum(maxAfter, thislen - p); + } + } + } - \param A : Main matrix. - \param B : Matrix to insert. - \param C : Result matrix. - \param r : Index of the row where to insert matrix B. - \param c : Index of the column where to insert matrix B. + size_type totalLength = length; + // increase totalLength according to maxBefore + totalLength = vpMath::maximum(totalLength, maxBefore); + // decrease maxAfter according to totalLength + maxAfter = (std::min)(maxAfter, totalLength - maxBefore); + + if (!intro.empty()) + s << intro; + s << "[" << m << "," << n << "]=\n"; + + for (unsigned int i = 0; i < m; i++) { + s << " "; + for (unsigned int j = 0; j < n; j++) { + size_type p = values[i * n + j].find('.'); + s.setf(std::ios::right, std::ios::adjustfield); + s.width((std::streamsize)maxBefore); + s << values[i * n + j].substr(0, p).c_str(); + + if (maxAfter > 0) { + s.setf(std::ios::left, std::ios::adjustfield); + if (p != std::string::npos) { + s.width((std::streamsize)maxAfter); + s << values[i * n + j].substr(p, maxAfter).c_str(); + } + else { + s.width((std::streamsize)maxAfter); + s << ".0"; + } + } + + s << ' '; + } + s << std::endl; + } - \warning Throw exception if the sizes of the matrices do not - allow the insertion. -*/ -void vpMatrix::insert(const vpMatrix &A, const vpMatrix &B, vpMatrix &C, unsigned int r, unsigned int c) -{ - vpArray2D C_array; + s.flags(original_flags); // restore s to standard state - vpArray2D::insert(A, B, C_array, r, c); + return (int)(maxBefore + maxAfter); + } - C = C_array; -} + /*! + Print using Matlab syntax, to copy/paste in Matlab later. -/*! - Juxtapose to matrices C = [ A B ]. - - \f$ C = \left( \begin{array}{cc} A & B \end{array}\right) \f$ - - \param A : Left matrix. - \param B : Right matrix. - \return Juxtaposed matrix C = [ A B ] - - \warning A and B must have the same number of rows. -*/ -vpMatrix vpMatrix::juxtaposeMatrices(const vpMatrix &A, const vpMatrix &B) -{ - vpMatrix C; - - juxtaposeMatrices(A, B, C); - - return C; -} - -/*! - \relates vpMatrix - Juxtapose to matrices C = [ A B ]. - - \f$ C = \left( \begin{array}{cc} A & B \end{array}\right) \f$ - - \param A : Left matrix. - \param B : Right matrix. - \param C : Juxtaposed matrix C = [ A B ] + The following code + \code + #include - \warning A and B must have the same number of rows. -*/ -void vpMatrix::juxtaposeMatrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C) -{ - unsigned int nca = A.getCols(); - unsigned int ncb = B.getCols(); + int main() + { + vpMatrix M(2,3); + int cpt = 0; + for (unsigned int i=0; i &A) -*/ -int vpMatrix::print(std::ostream &s, unsigned int length, const std::string &intro) const -{ - typedef std::string::size_type size_type; - - unsigned int m = getRows(); - unsigned int n = getCols(); - - std::vector values(m * n); - std::ostringstream oss; - std::ostringstream ossFixed; - std::ios_base::fmtflags original_flags = oss.flags(); - - ossFixed.setf(std::ios::fixed, std::ios::floatfield); - - size_type maxBefore = 0; // the length of the integral part - size_type maxAfter = 0; // number of decimals plus - // one place for the decimal point - for (unsigned int i = 0; i < m; ++i) { - for (unsigned int j = 0; j < n; ++j) { - oss.str(""); - oss << (*this)[i][j]; - if (oss.str().find("e") != std::string::npos) { - ossFixed.str(""); - ossFixed << (*this)[i][j]; - oss.str(ossFixed.str()); - } - - values[i * n + j] = oss.str(); - size_type thislen = values[i * n + j].size(); - size_type p = values[i * n + j].find('.'); - - if (p == std::string::npos) { - maxBefore = vpMath::maximum(maxBefore, thislen); - // maxAfter remains the same - } - else { - maxBefore = vpMath::maximum(maxBefore, p); - maxAfter = vpMath::maximum(maxAfter, thislen - p); - } - } - } - - size_type totalLength = length; - // increase totalLength according to maxBefore - totalLength = vpMath::maximum(totalLength, maxBefore); - // decrease maxAfter according to totalLength - maxAfter = (std::min)(maxAfter, totalLength - maxBefore); - - if (!intro.empty()) - s << intro; - s << "[" << m << "," << n << "]=\n"; - - for (unsigned int i = 0; i < m; i++) { - s << " "; - for (unsigned int j = 0; j < n; j++) { - size_type p = values[i * n + j].find('.'); - s.setf(std::ios::right, std::ios::adjustfield); - s.width((std::streamsize)maxBefore); - s << values[i * n + j].substr(0, p).c_str(); - - if (maxAfter > 0) { - s.setf(std::ios::left, std::ios::adjustfield); - if (p != std::string::npos) { - s.width((std::streamsize)maxAfter); - s << values[i * n + j].substr(p, maxAfter).c_str(); + \endcode + produces this output: + \code + M = [ 0, 1, 2, ; + 3, 4, 5, ] + \endcode + that could be copy/paste in Matlab: + \code + >> M = [ 0, 1, 2, ; + 3, 4, 5, ] + + M = + + 0 1 2 + 3 4 5 + + >> + \endcode + */ + std::ostream &vpMatrix::matlabPrint(std::ostream &os) const + { + os << "[ "; + for (unsigned int i = 0; i < this->getRows(); ++i) { + for (unsigned int j = 0; j < this->getCols(); ++j) { + os << (*this)[i][j] << ", "; + } + if (this->getRows() != i + 1) { + os << ";" << std::endl; } else { - s.width((std::streamsize)maxAfter); - s << ".0"; + os << "]" << std::endl; } } - - s << ' '; + return os; } - s << std::endl; - } - s.flags(original_flags); // restore s to standard state + /*! + Print using Maple syntax, to copy/paste in Maple later. - return (int)(maxBefore + maxAfter); -} + The following code + \code + #include -/*! - Print using Matlab syntax, to copy/paste in Matlab later. + int main() + { + vpMatrix M(2,3); + int cpt = 0; + for (unsigned int i=0; i - -int main() -{ - vpMatrix M(2,3); - int cpt = 0; - for (unsigned int i=0; igetRows(); ++i) { - for (unsigned int j = 0; j < this->getCols(); ++j) { - os << (*this)[i][j] << ", "; - } - if (this->getRows() != i + 1) { - os << ";" << std::endl; + std::cout << "M = "; M.maplePrint(std::cout); } - else { - os << "]" << std::endl; + \endcode + produces this output: + \code + M = ([ + [0, 1, 2, ], + [3, 4, 5, ], + ]) + \endcode + that could be copy/paste in Maple. + + */ + std::ostream &vpMatrix::maplePrint(std::ostream &os) const + { + os << "([ " << std::endl; + for (unsigned int i = 0; i < this->getRows(); ++i) { + os << "["; + for (unsigned int j = 0; j < this->getCols(); ++j) { + os << (*this)[i][j] << ", "; + } + os << "]," << std::endl; + } + os << "])" << std::endl; + return os; } - } - return os; -} -/*! - Print using Maple syntax, to copy/paste in Maple later. + /*! + Print/save a matrix in csv format. - The following code - \code -#include + The following code + \code + #include -int main() -{ - vpMatrix M(2,3); - int cpt = 0; - for (unsigned int i=0; igetRows(); ++i) { - os << "["; - for (unsigned int j = 0; j < this->getCols(); ++j) { - os << (*this)[i][j] << ", "; + ofs.close(); } - os << "]," << std::endl; - } - os << "])" << std::endl; - return os; -} - -/*! - Print/save a matrix in csv format. - - The following code - \code -#include - -int main() -{ - std::ofstream ofs("log.csv", std::ofstream::out); - vpMatrix M(2,3); - int cpt = 0; - for (unsigned int i=0; igetRows(); ++i) { - for (unsigned int j = 0; j < this->getCols(); ++j) { - os << (*this)[i][j]; - if (!(j == (this->getCols() - 1))) - os << ", "; + \endcode + produces log.csv file that contains: + \code + 0, 1, 2 + 3, 4, 5 + \endcode + */ + std::ostream &vpMatrix::csvPrint(std::ostream &os) const + { + for (unsigned int i = 0; i < this->getRows(); ++i) { + for (unsigned int j = 0; j < this->getCols(); ++j) { + os << (*this)[i][j]; + if (!(j == (this->getCols() - 1))) + os << ", "; + } + os << std::endl; + } + return os; } - os << std::endl; - } - return os; -} -/*! - Print to be used as part of a C++ code later. + /*! + Print to be used as part of a C++ code later. - \param os : the stream to be printed in. - \param matrixName : name of the matrix, "A" by default. - \param octet : if false, print using double, if true, print byte per byte - each bytes of the double array. + \param os : the stream to be printed in. + \param matrixName : name of the matrix, "A" by default. + \param octet : if false, print using double, if true, print byte per byte + each bytes of the double array. - The following code shows how to use this function: - \code -#include + The following code shows how to use this function: + \code + #include -int main() -{ - vpMatrix M(2,3); - int cpt = 0; - for (unsigned int i=0; igetRows() << ", " << this->getCols() << "); " << std::endl; - - for (unsigned int i = 0; i < this->getRows(); ++i) { - for (unsigned int j = 0; j < this->getCols(); ++j) { - if (!octet) { - os << matrixName << "[" << i << "][" << j << "] = " << (*this)[i][j] << "; " << std::endl; - } - else { - for (unsigned int k = 0; k < sizeof(double); ++k) { - os << "((unsigned char*)&(" << matrixName << "[" << i << "][" << j << "]) )[" << k << "] = 0x" << std::hex - << (unsigned int)((unsigned char *)&((*this)[i][j]))[k] << "; " << std::endl; + M.cppPrint(std::cout, "M"); + } + \endcode + It produces the following output that could be copy/paste in a C++ code: + \code + vpMatrix M (2, 3); + M[0][0] = 0; + M[0][1] = 1; + M[0][2] = 2; + + M[1][0] = 3; + M[1][1] = 4; + M[1][2] = 5; + + \endcode + */ + std::ostream &vpMatrix::cppPrint(std::ostream &os, const std::string &matrixName, bool octet) const + { + os << "vpMatrix " << matrixName << " (" << this->getRows() << ", " << this->getCols() << "); " << std::endl; + + for (unsigned int i = 0; i < this->getRows(); ++i) { + for (unsigned int j = 0; j < this->getCols(); ++j) { + if (!octet) { + os << matrixName << "[" << i << "][" << j << "] = " << (*this)[i][j] << "; " << std::endl; + } + else { + for (unsigned int k = 0; k < sizeof(double); ++k) { + os << "((unsigned char*)&(" << matrixName << "[" << i << "][" << j << "]) )[" << k << "] = 0x" << std::hex + << (unsigned int)((unsigned char *)&((*this)[i][j]))[k] << "; " << std::endl; + } + } } + os << std::endl; } + return os; } - os << std::endl; - } - return os; -} -/*! - Stack A at the end of the current matrix, or copy if the matrix has no - dimensions : this = [ this A ]^T. -*/ -void vpMatrix::stack(const vpMatrix &A) -{ - if (rowNum == 0) { - *this = A; - } - else if (A.getRows() > 0) { - if (colNum != A.getCols()) { - throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx%d) matrix", rowNum, colNum, - A.getRows(), A.getCols())); - } - - unsigned int rowNumOld = rowNum; - resize(rowNum + A.getRows(), colNum, false, false); - insert(A, rowNumOld, 0); - } -} - -/*! - Stack row vector \e r at the end of the current matrix, or copy if the -matrix has no dimensions: this = [ this r ]^T. + /*! + Stack A at the end of the current matrix, or copy if the matrix has no + dimensions : this = [ this A ]^T. + */ + void vpMatrix::stack(const vpMatrix &A) + { + if (rowNum == 0) { + *this = A; + } + else if (A.getRows() > 0) { + if (colNum != A.getCols()) { + throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx%d) matrix", rowNum, colNum, + A.getRows(), A.getCols())); + } - Here an example for a robot velocity log : -\code -vpMatrix A; -vpColVector v(6); -for(unsigned int i = 0;i<100;i++) -{ - robot.getVelocity(vpRobot::ARTICULAR_FRAME, v); - Velocities.stack(v.t()); -} -\endcode -*/ -void vpMatrix::stack(const vpRowVector &r) -{ - if (rowNum == 0) { - *this = r; - } - else { - if (colNum != r.getCols()) { - throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (1x%d) row vector", rowNum, - colNum, r.getCols())); + unsigned int rowNumOld = rowNum; + resize(rowNum + A.getRows(), colNum, false, false); + insert(A, rowNumOld, 0); + } } - if (r.size() == 0) { - return; + /*! + Stack row vector \e r at the end of the current matrix, or copy if the + matrix has no dimensions: this = [ this r ]^T. + + Here an example for a robot velocity log : + \code + vpMatrix A; + vpColVector v(6); + for(unsigned int i = 0;i<100;i++) + { + robot.getVelocity(vpRobot::ARTICULAR_FRAME, v); + Velocities.stack(v.t()); } + \endcode + */ + void vpMatrix::stack(const vpRowVector &r) + { + if (rowNum == 0) { + *this = r; + } + else { + if (colNum != r.getCols()) { + throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (1x%d) row vector", rowNum, + colNum, r.getCols())); + } - unsigned int oldSize = size(); - resize(rowNum + 1, colNum, false, false); - - if (data != nullptr && r.data != nullptr && data != r.data) { - // Copy r in data - memcpy(data + oldSize, r.data, sizeof(double) * r.size()); - } - } -} + if (r.size() == 0) { + return; + } -/*! - Stack column vector \e c at the right of the current matrix, or copy if the -matrix has no dimensions: this = [ this c ]. + unsigned int oldSize = size(); + resize(rowNum + 1, colNum, false, false); - Here an example for a robot velocity log matrix: -\code -vpMatrix log; -vpColVector v(6); -for(unsigned int i = 0; i<100;i++) -{ - robot.getVelocity(vpRobot::ARTICULAR_FRAME, v); - log.stack(v); -} -\endcode -Here the log matrix has size 6 rows by 100 columns. -*/ -void vpMatrix::stack(const vpColVector &c) -{ - if (colNum == 0) { - *this = c; - } - else { - if (rowNum != c.getRows()) { - throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx1) column vector", rowNum, - colNum, c.getRows())); + if (data != nullptr && r.data != nullptr && data != r.data) { + // Copy r in data + memcpy(data + oldSize, r.data, sizeof(double) * r.size()); + } + } } - if (c.size() == 0) { - return; + /*! + Stack column vector \e c at the right of the current matrix, or copy if the + matrix has no dimensions: this = [ this c ]. + + Here an example for a robot velocity log matrix: + \code + vpMatrix log; + vpColVector v(6); + for(unsigned int i = 0; i<100;i++) + { + robot.getVelocity(vpRobot::ARTICULAR_FRAME, v); + log.stack(v); } - - vpMatrix tmp = *this; - unsigned int oldColNum = colNum; - resize(rowNum, colNum + 1, false, false); - - if (data != nullptr && tmp.data != nullptr && data != tmp.data) { - // Copy c in data - for (unsigned int i = 0; i < rowNum; i++) { - memcpy(data + i * colNum, tmp.data + i * oldColNum, sizeof(double) * oldColNum); - rowPtrs[i][oldColNum] = c[i]; + \endcode + Here the log matrix has size 6 rows by 100 columns. + */ + void vpMatrix::stack(const vpColVector &c) + { + if (colNum == 0) { + *this = c; } - } - } -} + else { + if (rowNum != c.getRows()) { + throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx1) column vector", rowNum, + colNum, c.getRows())); + } -/*! - Insert matrix A at the given position in the current matrix. + if (c.size() == 0) { + return; + } - \warning Throw vpException::dimensionError if the - dimensions of the matrices do not allow the operation. + vpMatrix tmp = *this; + unsigned int oldColNum = colNum; + resize(rowNum, colNum + 1, false, false); - \param A : The matrix to insert. - \param r : The index of the row to begin to insert data. - \param c : The index of the column to begin to insert data. -*/ -void vpMatrix::insert(const vpMatrix &A, unsigned int r, unsigned int c) -{ - if ((r + A.getRows()) <= rowNum && (c + A.getCols()) <= colNum) { - if (A.colNum == colNum && data != nullptr && A.data != nullptr && A.data != data) { - memcpy(data + r * colNum, A.data, sizeof(double) * A.size()); + if (data != nullptr && tmp.data != nullptr && data != tmp.data) { + // Copy c in data + for (unsigned int i = 0; i < rowNum; i++) { + memcpy(data + i * colNum, tmp.data + i * oldColNum, sizeof(double) * oldColNum); + rowPtrs[i][oldColNum] = c[i]; + } + } + } } - else if (data != nullptr && A.data != nullptr && A.data != data) { - for (unsigned int i = r; i < (r + A.getRows()); i++) { - memcpy(data + i * colNum + c, A.data + (i - r) * A.colNum, sizeof(double) * A.colNum); + + /*! + Insert matrix A at the given position in the current matrix. + + \warning Throw vpException::dimensionError if the + dimensions of the matrices do not allow the operation. + + \param A : The matrix to insert. + \param r : The index of the row to begin to insert data. + \param c : The index of the column to begin to insert data. + */ + void vpMatrix::insert(const vpMatrix &A, unsigned int r, unsigned int c) + { + if ((r + A.getRows()) <= rowNum && (c + A.getCols()) <= colNum) { + if (A.colNum == colNum && data != nullptr && A.data != nullptr && A.data != data) { + memcpy(data + r * colNum, A.data, sizeof(double) * A.size()); + } + else if (data != nullptr && A.data != nullptr && A.data != data) { + for (unsigned int i = r; i < (r + A.getRows()); i++) { + memcpy(data + i * colNum + c, A.data + (i - r) * A.colNum, sizeof(double) * A.colNum); + } + } + } + else { + throw vpException(vpException::dimensionError, "Cannot insert (%dx%d) matrix in (%dx%d) matrix at position (%d,%d)", + A.getRows(), A.getCols(), rowNum, colNum, r, c); } } - } - else { - throw vpException(vpException::dimensionError, "Cannot insert (%dx%d) matrix in (%dx%d) matrix at position (%d,%d)", - A.getRows(), A.getCols(), rowNum, colNum, r, c); - } -} -/*! - Compute the eigenvalues of a n-by-n real symmetric matrix using - Lapack 3rd party. + /*! + Compute the eigenvalues of a n-by-n real symmetric matrix using + Lapack 3rd party. - \return The eigenvalues of a n-by-n real symmetric matrix, sorted in ascending order. + \return The eigenvalues of a n-by-n real symmetric matrix, sorted in ascending order. - \exception vpException::dimensionError If the matrix is not square. - \exception vpException::fatalError If the matrix is not symmetric. - \exception vpException::functionNotImplementedError If the Lapack 3rd party - is not detected. + \exception vpException::dimensionError If the matrix is not square. + \exception vpException::fatalError If the matrix is not symmetric. + \exception vpException::functionNotImplementedError If the Lapack 3rd party + is not detected. - Here an example: -\code -#include + Here an example: + \code + #include -#include -#include + #include + #include -int main() -{ - vpMatrix A(3,3); // A is a symmetric matrix - A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; - A[1][0] = 1/2.; A[1][1] = 1/3.; A[1][2] = 1/4.; - A[2][0] = 1/3.; A[2][1] = 1/4.; A[2][2] = 1/5.; - std::cout << "Initial symmetric matrix: \n" << A << std::endl; + int main() + { + vpMatrix A(3,3); // A is a symmetric matrix + A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; + A[1][0] = 1/2.; A[1][1] = 1/3.; A[1][2] = 1/4.; + A[2][0] = 1/3.; A[2][1] = 1/4.; A[2][2] = 1/5.; + std::cout << "Initial symmetric matrix: \n" << A << std::endl; - // Compute the eigen values - vpColVector evalue; // Eigenvalues - evalue = A.eigenValues(); - std::cout << "Eigen values: \n" << evalue << std::endl; -} -\endcode + // Compute the eigen values + vpColVector evalue; // Eigenvalues + evalue = A.eigenValues(); + std::cout << "Eigen values: \n" << evalue << std::endl; + } + \endcode - \sa eigenValues(vpColVector &, vpMatrix &) + \sa eigenValues(vpColVector &, vpMatrix &) -*/ -vpColVector vpMatrix::eigenValues() const -{ - vpColVector evalue(rowNum); // Eigen values + */ + vpColVector vpMatrix::eigenValues() const + { + vpColVector evalue(rowNum); // Eigen values - if (rowNum != colNum) { - throw(vpException(vpException::dimensionError, "Cannot compute eigen values on a non square matrix (%dx%d)", rowNum, - colNum)); - } + if (rowNum != colNum) { + throw(vpException(vpException::dimensionError, "Cannot compute eigen values on a non square matrix (%dx%d)", rowNum, + colNum)); + } - // Check if the matrix is symmetric: At - A = 0 - vpMatrix At_A = (*this).t() - (*this); - for (unsigned int i = 0; i < rowNum; i++) { - for (unsigned int j = 0; j < rowNum; j++) { - // if (At_A[i][j] != 0) { - if (std::fabs(At_A[i][j]) > std::numeric_limits::epsilon()) { - throw(vpException(vpException::fatalError, "Cannot compute eigen values on a non symmetric matrix")); + // Check if the matrix is symmetric: At - A = 0 + vpMatrix At_A = (*this).t() - (*this); + for (unsigned int i = 0; i < rowNum; i++) { + for (unsigned int j = 0; j < rowNum; j++) { + // if (At_A[i][j] != 0) { + if (std::fabs(At_A[i][j]) > std::numeric_limits::epsilon()) { + throw(vpException(vpException::fatalError, "Cannot compute eigen values on a non symmetric matrix")); + } + } } - } - } #if defined(VISP_HAVE_LAPACK) #if defined(VISP_HAVE_GSL) /* be careful of the copy below */ - { - gsl_vector *eval = gsl_vector_alloc(rowNum); - gsl_matrix *evec = gsl_matrix_alloc(rowNum, colNum); - - gsl_eigen_symmv_workspace *w = gsl_eigen_symmv_alloc(rowNum); - gsl_matrix *m = gsl_matrix_alloc(rowNum, colNum); - - unsigned int Atda = (unsigned int)m->tda; - for (unsigned int i = 0; i < rowNum; i++) { - unsigned int k = i * Atda; - for (unsigned int j = 0; j < colNum; j++) - m->data[k + j] = (*this)[i][j]; - } - gsl_eigen_symmv(m, eval, evec, w); + { + gsl_vector *eval = gsl_vector_alloc(rowNum); + gsl_matrix *evec = gsl_matrix_alloc(rowNum, colNum); + + gsl_eigen_symmv_workspace *w = gsl_eigen_symmv_alloc(rowNum); + gsl_matrix *m = gsl_matrix_alloc(rowNum, colNum); + + unsigned int Atda = (unsigned int)m->tda; + for (unsigned int i = 0; i < rowNum; i++) { + unsigned int k = i * Atda; + for (unsigned int j = 0; j < colNum; j++) + m->data[k + j] = (*this)[i][j]; + } + gsl_eigen_symmv(m, eval, evec, w); - gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_ABS_ASC); + gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_ABS_ASC); - for (unsigned int i = 0; i < rowNum; i++) { - evalue[i] = gsl_vector_get(eval, i); - } + for (unsigned int i = 0; i < rowNum; i++) { + evalue[i] = gsl_vector_get(eval, i); + } - gsl_eigen_symmv_free(w); - gsl_vector_free(eval); - gsl_matrix_free(m); - gsl_matrix_free(evec); - } + gsl_eigen_symmv_free(w); + gsl_vector_free(eval); + gsl_matrix_free(m); + gsl_matrix_free(evec); + } #else - { - const char jobz = 'N'; - const char uplo = 'U'; - vpMatrix A = (*this); - vpColVector WORK; - int lwork = -1; - int info = 0; - double wkopt; - vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, &wkopt, lwork, info); - lwork = static_cast(wkopt); - WORK.resize(lwork); - vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, WORK.data, lwork, info); - } + { + const char jobz = 'N'; + const char uplo = 'U'; + vpMatrix A = (*this); + vpColVector WORK; + int lwork = -1; + int info = 0; + double wkopt; + vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, &wkopt, lwork, info); + lwork = static_cast(wkopt); + WORK.resize(lwork); + vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, WORK.data, lwork, info); + } #endif #else - { - throw(vpException(vpException::functionNotImplementedError, "Eigen values computation is not implemented. " - "You should install Lapack 3rd party")); - } + { + throw(vpException(vpException::functionNotImplementedError, "Eigen values computation is not implemented. " + "You should install Lapack 3rd party")); + } #endif - return evalue; -} - -/*! - Compute the eigenvalues of a n-by-n real symmetric matrix using - Lapack 3rd party. + return evalue; + } - \param evalue : Eigenvalues of the matrix, sorted in ascending order. + /*! + Compute the eigenvalues of a n-by-n real symmetric matrix using + Lapack 3rd party. - \param evector : Corresponding eigenvectors of the matrix. + \param evalue : Eigenvalues of the matrix, sorted in ascending order. - \exception vpException::dimensionError If the matrix is not square. - \exception vpException::fatalError If the matrix is not symmetric. - \exception vpException::functionNotImplementedError If Lapack 3rd party is - not detected. + \param evector : Corresponding eigenvectors of the matrix. - Here an example: -\code -#include + \exception vpException::dimensionError If the matrix is not square. + \exception vpException::fatalError If the matrix is not symmetric. + \exception vpException::functionNotImplementedError If Lapack 3rd party is + not detected. -#include -#include + Here an example: + \code + #include -int main() -{ - vpMatrix A(4,4); // A is a symmetric matrix - A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; A[0][3] = 1/4.; - A[1][0] = 1/2.; A[1][1] = 1/3.; A[1][2] = 1/4.; A[1][3] = 1/5.; - A[2][0] = 1/3.; A[2][1] = 1/4.; A[2][2] = 1/5.; A[2][3] = 1/6.; - A[3][0] = 1/4.; A[3][1] = 1/5.; A[3][2] = 1/6.; A[3][3] = 1/7.; - std::cout << "Initial symmetric matrix: \n" << A << std::endl; + #include + #include - vpColVector d; // Eigenvalues - vpMatrix V; // Eigenvectors + int main() + { + vpMatrix A(4,4); // A is a symmetric matrix + A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; A[0][3] = 1/4.; + A[1][0] = 1/2.; A[1][1] = 1/3.; A[1][2] = 1/4.; A[1][3] = 1/5.; + A[2][0] = 1/3.; A[2][1] = 1/4.; A[2][2] = 1/5.; A[2][3] = 1/6.; + A[3][0] = 1/4.; A[3][1] = 1/5.; A[3][2] = 1/6.; A[3][3] = 1/7.; + std::cout << "Initial symmetric matrix: \n" << A << std::endl; - // Compute the eigenvalues and eigenvectors - A.eigenValues(d, V); - std::cout << "Eigen values: \n" << d << std::endl; - std::cout << "Eigen vectors: \n" << V << std::endl; + vpColVector d; // Eigenvalues + vpMatrix V; // Eigenvectors - vpMatrix D; - D.diag(d); // Eigenvalues are on the diagonal + // Compute the eigenvalues and eigenvectors + A.eigenValues(d, V); + std::cout << "Eigen values: \n" << d << std::endl; + std::cout << "Eigen vectors: \n" << V << std::endl; - std::cout << "D: " << D << std::endl; + vpMatrix D; + D.diag(d); // Eigenvalues are on the diagonal - // Verification: A * V = V * D - std::cout << "AV-VD = 0 ? \n" << (A*V) - (V*D) << std::endl; -} -\endcode + std::cout << "D: " << D << std::endl; -\sa eigenValues() -*/ -void vpMatrix::eigenValues(vpColVector &evalue, vpMatrix &evector) const -{ - if (rowNum != colNum) { - throw(vpException(vpException::dimensionError, "Cannot compute eigen values on a non square matrix (%dx%d)", rowNum, - colNum)); - } + // Verification: A * V = V * D + std::cout << "AV-VD = 0 ? \n" << (A*V) - (V*D) << std::endl; + } + \endcode + + \sa eigenValues() + */ + void vpMatrix::eigenValues(vpColVector &evalue, vpMatrix &evector) const + { + if (rowNum != colNum) { + throw(vpException(vpException::dimensionError, "Cannot compute eigen values on a non square matrix (%dx%d)", rowNum, + colNum)); + } - // Check if the matrix is symmetric: At - A = 0 - vpMatrix At_A = (*this).t() - (*this); - for (unsigned int i = 0; i < rowNum; i++) { - for (unsigned int j = 0; j < rowNum; j++) { - // if (At_A[i][j] != 0) { - if (std::fabs(At_A[i][j]) > std::numeric_limits::epsilon()) { - throw(vpException(vpException::fatalError, "Cannot compute eigen values on a non symmetric matrix")); + // Check if the matrix is symmetric: At - A = 0 + vpMatrix At_A = (*this).t() - (*this); + for (unsigned int i = 0; i < rowNum; i++) { + for (unsigned int j = 0; j < rowNum; j++) { + // if (At_A[i][j] != 0) { + if (std::fabs(At_A[i][j]) > std::numeric_limits::epsilon()) { + throw(vpException(vpException::fatalError, "Cannot compute eigen values on a non symmetric matrix")); + } + } } - } - } - // Resize the output matrices - evalue.resize(rowNum); - evector.resize(rowNum, colNum); + // Resize the output matrices + evalue.resize(rowNum); + evector.resize(rowNum, colNum); #if defined(VISP_HAVE_LAPACK) #if defined(VISP_HAVE_GSL) /* be careful of the copy below */ - { - gsl_vector *eval = gsl_vector_alloc(rowNum); - gsl_matrix *evec = gsl_matrix_alloc(rowNum, colNum); + { + gsl_vector *eval = gsl_vector_alloc(rowNum); + gsl_matrix *evec = gsl_matrix_alloc(rowNum, colNum); + + gsl_eigen_symmv_workspace *w = gsl_eigen_symmv_alloc(rowNum); + gsl_matrix *m = gsl_matrix_alloc(rowNum, colNum); + + unsigned int Atda = (unsigned int)m->tda; + for (unsigned int i = 0; i < rowNum; i++) { + unsigned int k = i * Atda; + for (unsigned int j = 0; j < colNum; j++) + m->data[k + j] = (*this)[i][j]; + } + gsl_eigen_symmv(m, eval, evec, w); - gsl_eigen_symmv_workspace *w = gsl_eigen_symmv_alloc(rowNum); - gsl_matrix *m = gsl_matrix_alloc(rowNum, colNum); + gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_ABS_ASC); - unsigned int Atda = (unsigned int)m->tda; - for (unsigned int i = 0; i < rowNum; i++) { - unsigned int k = i * Atda; - for (unsigned int j = 0; j < colNum; j++) - m->data[k + j] = (*this)[i][j]; - } - gsl_eigen_symmv(m, eval, evec, w); - - gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_ABS_ASC); + for (unsigned int i = 0; i < rowNum; i++) { + evalue[i] = gsl_vector_get(eval, i); + } + Atda = (unsigned int)evec->tda; + for (unsigned int i = 0; i < rowNum; i++) { + unsigned int k = i * Atda; + for (unsigned int j = 0; j < rowNum; j++) { + evector[i][j] = evec->data[k + j]; + } + } - for (unsigned int i = 0; i < rowNum; i++) { - evalue[i] = gsl_vector_get(eval, i); - } - Atda = (unsigned int)evec->tda; - for (unsigned int i = 0; i < rowNum; i++) { - unsigned int k = i * Atda; - for (unsigned int j = 0; j < rowNum; j++) { - evector[i][j] = evec->data[k + j]; + gsl_eigen_symmv_free(w); + gsl_vector_free(eval); + gsl_matrix_free(m); + gsl_matrix_free(evec); } - } - - gsl_eigen_symmv_free(w); - gsl_vector_free(eval); - gsl_matrix_free(m); - gsl_matrix_free(evec); - } #else // defined(VISP_HAVE_GSL) - { - const char jobz = 'V'; - const char uplo = 'U'; - vpMatrix A = (*this); - vpColVector WORK; - int lwork = -1; - int info = 0; - double wkopt; - vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, &wkopt, lwork, info); - lwork = static_cast(wkopt); - WORK.resize(lwork); - vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, WORK.data, lwork, info); - evector = A.t(); - } + { + const char jobz = 'V'; + const char uplo = 'U'; + vpMatrix A = (*this); + vpColVector WORK; + int lwork = -1; + int info = 0; + double wkopt; + vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, &wkopt, lwork, info); + lwork = static_cast(wkopt); + WORK.resize(lwork); + vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, WORK.data, lwork, info); + evector = A.t(); + } #endif // defined(VISP_HAVE_GSL) #else - { - throw(vpException(vpException::functionNotImplementedError, "Eigen values computation is not implemented. " - "You should install Lapack 3rd party")); - } + { + throw(vpException(vpException::functionNotImplementedError, "Eigen values computation is not implemented. " + "You should install Lapack 3rd party")); + } #endif -} + } -/*! - Function to compute the null space (the kernel) of a m-by-n matrix \f$\bf - A\f$. + /*! + Function to compute the null space (the kernel) of a m-by-n matrix \f$\bf + A\f$. - The null space of a matrix \f$\bf A\f$ is defined as \f$\mbox{Ker}({\bf A}) - = { {\bf X} : {\bf A}*{\bf X} = {\bf 0}}\f$. + The null space of a matrix \f$\bf A\f$ is defined as \f$\mbox{Ker}({\bf A}) + = { {\bf X} : {\bf A}*{\bf X} = {\bf 0}}\f$. - \param kerAt: The matrix that contains the null space (kernel) of \f$\bf - A\f$ defined by the matrix \f${\bf X}^T\f$. If matrix \f$\bf A\f$ is full - rank, the dimension of \c kerAt is (0, n), otherwise the dimension is (n-r, - n). This matrix is thus the transpose of \f$\mbox{Ker}({\bf A})\f$. + \param kerAt: The matrix that contains the null space (kernel) of \f$\bf + A\f$ defined by the matrix \f${\bf X}^T\f$. If matrix \f$\bf A\f$ is full + rank, the dimension of \c kerAt is (0, n), otherwise the dimension is (n-r, + n). This matrix is thus the transpose of \f$\mbox{Ker}({\bf A})\f$. - \param svThreshold: Threshold used to test the singular values. If - a singular value is lower than this threshold we consider that the - matrix is not full rank. + \param svThreshold: Threshold used to test the singular values. If + a singular value is lower than this threshold we consider that the + matrix is not full rank. - \return The rank of the matrix. -*/ -unsigned int vpMatrix::kernel(vpMatrix &kerAt, double svThreshold) const -{ - unsigned int nbline = getRows(); - unsigned int nbcol = getCols(); + \return The rank of the matrix. + */ + unsigned int vpMatrix::kernel(vpMatrix &kerAt, double svThreshold) const + { + unsigned int nbline = getRows(); + unsigned int nbcol = getCols(); - vpMatrix U, V; // Copy of the matrix, SVD function is destructive - vpColVector sv; - sv.resize(nbcol, false); // singular values - V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition + vpMatrix U, V; // Copy of the matrix, SVD function is destructive + vpColVector sv; + sv.resize(nbcol, false); // singular values + V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition - // Copy and resize matrix to have at least as many rows as columns - // kernel is computed in svd method only if the matrix has more rows than - // columns + // Copy and resize matrix to have at least as many rows as columns + // kernel is computed in svd method only if the matrix has more rows than + // columns - if (nbline < nbcol) - U.resize(nbcol, nbcol, true); - else - U.resize(nbline, nbcol, false); + if (nbline < nbcol) + U.resize(nbcol, nbcol, true); + else + U.resize(nbline, nbcol, false); - U.insert(*this, 0, 0); + U.insert(*this, 0, 0); - U.svd(sv, V); + U.svd(sv, V); - // Compute the highest singular value and rank of the matrix - double maxsv = 0; - for (unsigned int i = 0; i < nbcol; i++) { - if (sv[i] > maxsv) { - maxsv = sv[i]; - } - } + // Compute the highest singular value and rank of the matrix + double maxsv = 0; + for (unsigned int i = 0; i < nbcol; i++) { + if (sv[i] > maxsv) { + maxsv = sv[i]; + } + } - unsigned int rank = 0; - for (unsigned int i = 0; i < nbcol; i++) { - if (sv[i] > maxsv * svThreshold) { - rank++; - } - } + unsigned int rank = 0; + for (unsigned int i = 0; i < nbcol; i++) { + if (sv[i] > maxsv * svThreshold) { + rank++; + } + } - kerAt.resize(nbcol - rank, nbcol); - if (rank != nbcol) { - for (unsigned int j = 0, k = 0; j < nbcol; j++) { - // if( v.col(j) in kernel and non zero ) - if ((sv[j] <= maxsv * svThreshold) && - (std::fabs(V.getCol(j).sumSquare()) > std::numeric_limits::epsilon())) { - for (unsigned int i = 0; i < V.getRows(); i++) { - kerAt[k][i] = V[i][j]; + kerAt.resize(nbcol - rank, nbcol); + if (rank != nbcol) { + for (unsigned int j = 0, k = 0; j < nbcol; j++) { + // if( v.col(j) in kernel and non zero ) + if ((sv[j] <= maxsv * svThreshold) && + (std::fabs(V.getCol(j).sumSquare()) > std::numeric_limits::epsilon())) { + for (unsigned int i = 0; i < V.getRows(); i++) { + kerAt[k][i] = V[i][j]; + } + k++; + } } - k++; } + + return rank; } - } - return rank; -} + /*! + Function to compute the null space (the kernel) of a m-by-n matrix \f$\bf + A\f$. -/*! - Function to compute the null space (the kernel) of a m-by-n matrix \f$\bf - A\f$. + The null space of a matrix \f$\bf A\f$ is defined as \f$\mbox{Ker}({\bf A}) + = { {\bf X} : {\bf A}*{\bf X} = {\bf 0}}\f$. - The null space of a matrix \f$\bf A\f$ is defined as \f$\mbox{Ker}({\bf A}) - = { {\bf X} : {\bf A}*{\bf X} = {\bf 0}}\f$. + \param kerA: The matrix that contains the null space (kernel) of \f$\bf + A\f$. If matrix \f$\bf A\f$ is full rank, the dimension of \c kerA is (n, 0), + otherwise its dimension is (n, n-r). - \param kerA: The matrix that contains the null space (kernel) of \f$\bf - A\f$. If matrix \f$\bf A\f$ is full rank, the dimension of \c kerA is (n, 0), - otherwise its dimension is (n, n-r). + \param svThreshold: Threshold used to test the singular values. The dimension + of kerA corresponds to the number of singular values lower than this threshold - \param svThreshold: Threshold used to test the singular values. The dimension - of kerA corresponds to the number of singular values lower than this threshold + \return The dimension of the nullspace, that is \f$ n - r \f$. + */ + unsigned int vpMatrix::nullSpace(vpMatrix &kerA, double svThreshold) const + { + unsigned int nbrow = getRows(); + unsigned int nbcol = getCols(); - \return The dimension of the nullspace, that is \f$ n - r \f$. -*/ -unsigned int vpMatrix::nullSpace(vpMatrix &kerA, double svThreshold) const -{ - unsigned int nbrow = getRows(); - unsigned int nbcol = getCols(); - - vpMatrix U, V; // Copy of the matrix, SVD function is destructive - vpColVector sv; - sv.resize(nbcol, false); // singular values - V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition + vpMatrix U, V; // Copy of the matrix, SVD function is destructive + vpColVector sv; + sv.resize(nbcol, false); // singular values + V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition - // Copy and resize matrix to have at least as many rows as columns - // kernel is computed in svd method only if the matrix has more rows than - // columns + // Copy and resize matrix to have at least as many rows as columns + // kernel is computed in svd method only if the matrix has more rows than + // columns - if (nbrow < nbcol) - U.resize(nbcol, nbcol, true); - else - U.resize(nbrow, nbcol, false); + if (nbrow < nbcol) + U.resize(nbcol, nbcol, true); + else + U.resize(nbrow, nbcol, false); - U.insert(*this, 0, 0); + U.insert(*this, 0, 0); - U.svd(sv, V); + U.svd(sv, V); - // Compute the highest singular value and rank of the matrix - double maxsv = sv[0]; + // Compute the highest singular value and rank of the matrix + double maxsv = sv[0]; - unsigned int rank = 0; - for (unsigned int i = 0; i < nbcol; i++) { - if (sv[i] > maxsv * svThreshold) { - rank++; - } - } + unsigned int rank = 0; + for (unsigned int i = 0; i < nbcol; i++) { + if (sv[i] > maxsv * svThreshold) { + rank++; + } + } - kerA.resize(nbcol, nbcol - rank); - if (rank != nbcol) { - for (unsigned int j = 0, k = 0; j < nbcol; j++) { - // if( v.col(j) in kernel and non zero ) - if (sv[j] <= maxsv * svThreshold) { - for (unsigned int i = 0; i < nbcol; i++) { - kerA[i][k] = V[i][j]; + kerA.resize(nbcol, nbcol - rank); + if (rank != nbcol) { + for (unsigned int j = 0, k = 0; j < nbcol; j++) { + // if( v.col(j) in kernel and non zero ) + if (sv[j] <= maxsv * svThreshold) { + for (unsigned int i = 0; i < nbcol; i++) { + kerA[i][k] = V[i][j]; + } + k++; + } } - k++; } + + return (nbcol - rank); } - } - return (nbcol - rank); -} + /*! + Function to compute the null space (the kernel) of a m-by-n matrix \f$\bf + A\f$. -/*! - Function to compute the null space (the kernel) of a m-by-n matrix \f$\bf - A\f$. + The null space of a matrix \f$\bf A\f$ is defined as \f$\mbox{Ker}({\bf A}) + = { {\bf X} : {\bf A}*{\bf X} = {\bf 0}}\f$. - The null space of a matrix \f$\bf A\f$ is defined as \f$\mbox{Ker}({\bf A}) - = { {\bf X} : {\bf A}*{\bf X} = {\bf 0}}\f$. + \param kerA: The matrix that contains the null space (kernel) of \f$\bf + A\f$. If matrix \f$\bf A\f$ is full rank, the dimension of \c kerA is (n, 0), + otherwise its dimension is (n, n-r). - \param kerA: The matrix that contains the null space (kernel) of \f$\bf - A\f$. If matrix \f$\bf A\f$ is full rank, the dimension of \c kerA is (n, 0), - otherwise its dimension is (n, n-r). + \param dim: the dimension of the null space when it is known a priori - \param dim: the dimension of the null space when it is known a priori + \return The estimated dimension of the nullspace, that is \f$ n - r \f$, by + using 1e-6 as threshold for the sigular values. + */ + unsigned int vpMatrix::nullSpace(vpMatrix &kerA, int dim) const + { + unsigned int nbrow = getRows(); + unsigned int nbcol = getCols(); + unsigned int dim_ = static_cast(dim); - \return The estimated dimension of the nullspace, that is \f$ n - r \f$, by - using 1e-6 as threshold for the sigular values. -*/ -unsigned int vpMatrix::nullSpace(vpMatrix &kerA, int dim) const -{ - unsigned int nbrow = getRows(); - unsigned int nbcol = getCols(); - unsigned int dim_ = static_cast(dim); + vpMatrix U, V; // Copy of the matrix, SVD function is destructive + vpColVector sv; + sv.resize(nbcol, false); // singular values + V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition - vpMatrix U, V; // Copy of the matrix, SVD function is destructive - vpColVector sv; - sv.resize(nbcol, false); // singular values - V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition + // Copy and resize matrix to have at least as many rows as columns + // kernel is computed in svd method only if the matrix has more rows than + // columns - // Copy and resize matrix to have at least as many rows as columns - // kernel is computed in svd method only if the matrix has more rows than - // columns + if (nbrow < nbcol) + U.resize(nbcol, nbcol, true); + else + U.resize(nbrow, nbcol, false); - if (nbrow < nbcol) - U.resize(nbcol, nbcol, true); - else - U.resize(nbrow, nbcol, false); + U.insert(*this, 0, 0); - U.insert(*this, 0, 0); + U.svd(sv, V); - U.svd(sv, V); + kerA.resize(nbcol, dim_); + if (dim_ != 0) { + unsigned int rank = nbcol - dim_; + for (unsigned int k = 0; k < dim_; k++) { + unsigned int j = k + rank; + for (unsigned int i = 0; i < nbcol; i++) { + kerA[i][k] = V[i][j]; + } + } + } - kerA.resize(nbcol, dim_); - if (dim_ != 0) { - unsigned int rank = nbcol - dim_; - for (unsigned int k = 0; k < dim_; k++) { - unsigned int j = k + rank; + double maxsv = sv[0]; + unsigned int rank = 0; for (unsigned int i = 0; i < nbcol; i++) { - kerA[i][k] = V[i][j]; + if (sv[i] > maxsv * 1e-6) { + rank++; + } } + return (nbcol - rank); } - } - - double maxsv = sv[0]; - unsigned int rank = 0; - for (unsigned int i = 0; i < nbcol; i++) { - if (sv[i] > maxsv * 1e-6) { - rank++; - } - } - return (nbcol - rank); -} - -/*! - Compute the determinant of a n-by-n matrix. - \param method : Method used to compute the determinant. Default LU - decomposition method is faster than the method based on Gaussian - elimination. + /*! + Compute the determinant of a n-by-n matrix. - \return Determinant of the matrix. + \param method : Method used to compute the determinant. Default LU + decomposition method is faster than the method based on Gaussian + elimination. - \code -#include + \return Determinant of the matrix. -#include + \code + #include -int main() -{ - vpMatrix A(3,3); - A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; - A[1][0] = 1/3.; A[1][1] = 1/4.; A[1][2] = 1/5.; - A[2][0] = 1/6.; A[2][1] = 1/7.; A[2][2] = 1/8.; - std::cout << "Initial matrix: \n" << A << std::endl; + #include - // Compute the determinant - std:: cout << "Determinant by default method : " << A.det() << std::endl; - std:: cout << "Determinant by LU decomposition : " << A.detByLU() << std::endl; - std:: cout << "Determinant by LU decomposition (Lapack): " << A.detByLULapack() << std::endl; - std:: cout << "Determinant by LU decomposition (OpenCV): " << A.detByLUOpenCV() << std::endl; - std:: cout << "Determinant by LU decomposition (Eigen3): " << A.detByLUEigen3() << std::endl; -} -\endcode -*/ -double vpMatrix::det(vpDetMethod method) const -{ - double det = 0.; + int main() + { + vpMatrix A(3,3); + A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; + A[1][0] = 1/3.; A[1][1] = 1/4.; A[1][2] = 1/5.; + A[2][0] = 1/6.; A[2][1] = 1/7.; A[2][2] = 1/8.; + std::cout << "Initial matrix: \n" << A << std::endl; - if (method == LU_DECOMPOSITION) { - det = this->detByLU(); - } + // Compute the determinant + std:: cout << "Determinant by default method : " << A.det() << std::endl; + std:: cout << "Determinant by LU decomposition : " << A.detByLU() << std::endl; + std:: cout << "Determinant by LU decomposition (Lapack): " << A.detByLULapack() << std::endl; + std:: cout << "Determinant by LU decomposition (OpenCV): " << A.detByLUOpenCV() << std::endl; + std:: cout << "Determinant by LU decomposition (Eigen3): " << A.detByLUEigen3() << std::endl; + } + \endcode + */ + double vpMatrix::det(vpDetMethod method) const + { + double det = 0.; + + if (method == LU_DECOMPOSITION) { + det = this->detByLU(); + } - return (det); -} + return (det); + } -/*! + /*! - Compute the exponential matrix of a square matrix. + Compute the exponential matrix of a square matrix. - \return Return the exponential matrix. + \return Return the exponential matrix. -*/ -vpMatrix vpMatrix::expm() const -{ - if (colNum != rowNum) { - throw(vpException(vpException::dimensionError, "Cannot compute the exponential of a non square (%dx%d) matrix", - rowNum, colNum)); - } - else { + */ + vpMatrix vpMatrix::expm() const + { + if (colNum != rowNum) { + throw(vpException(vpException::dimensionError, "Cannot compute the exponential of a non square (%dx%d) matrix", + rowNum, colNum)); + } + else { #ifdef VISP_HAVE_GSL - size_t size_ = rowNum * colNum; - double *b = new double[size_]; - for (size_t i = 0; i < size_; i++) - b[i] = 0.; - gsl_matrix_view m = gsl_matrix_view_array(this->data, rowNum, colNum); - gsl_matrix_view em = gsl_matrix_view_array(b, rowNum, colNum); - gsl_linalg_exponential_ss(&m.matrix, &em.matrix, 0); - // gsl_matrix_fprintf(stdout, &em.matrix, "%g"); - vpMatrix expA; - expA.resize(rowNum, colNum, false); - memcpy(expA.data, b, size_ * sizeof(double)); - - delete[] b; - return expA; + size_t size_ = rowNum * colNum; + double *b = new double[size_]; + for (size_t i = 0; i < size_; i++) + b[i] = 0.; + gsl_matrix_view m = gsl_matrix_view_array(this->data, rowNum, colNum); + gsl_matrix_view em = gsl_matrix_view_array(b, rowNum, colNum); + gsl_linalg_exponential_ss(&m.matrix, &em.matrix, 0); + // gsl_matrix_fprintf(stdout, &em.matrix, "%g"); + vpMatrix expA; + expA.resize(rowNum, colNum, false); + memcpy(expA.data, b, size_ * sizeof(double)); + + delete[] b; + return expA; #else - vpMatrix _expE(rowNum, colNum, false); - vpMatrix _expD(rowNum, colNum, false); - vpMatrix _expX(rowNum, colNum, false); - vpMatrix _expcX(rowNum, colNum, false); - vpMatrix _eye(rowNum, colNum, false); - - _eye.eye(); - vpMatrix exp(*this); - - // double f; - int e; - double c = 0.5; - int q = 6; - int p = 1; - - double nA = 0; - for (unsigned int i = 0; i < rowNum; i++) { - double sum = 0; - for (unsigned int j = 0; j < colNum; j++) { - sum += fabs((*this)[i][j]); - } - if (sum > nA || i == 0) { - nA = sum; + vpMatrix _expE(rowNum, colNum, false); + vpMatrix _expD(rowNum, colNum, false); + vpMatrix _expX(rowNum, colNum, false); + vpMatrix _expcX(rowNum, colNum, false); + vpMatrix _eye(rowNum, colNum, false); + + _eye.eye(); + vpMatrix exp(*this); + + // double f; + int e; + double c = 0.5; + int q = 6; + int p = 1; + + double nA = 0; + for (unsigned int i = 0; i < rowNum; i++) { + double sum = 0; + for (unsigned int j = 0; j < colNum; j++) { + sum += fabs((*this)[i][j]); + } + if (sum > nA || i == 0) { + nA = sum; + } + } + + /* f = */ frexp(nA, &e); + // double s = (0 > e+1)?0:e+1; + double s = e + 1; + + double sca = 1.0 / pow(2.0, s); + exp = sca * exp; + _expX = *this; + _expE = c * exp + _eye; + _expD = -c * exp + _eye; + for (int k = 2; k <= q; k++) { + c = c * ((double)(q - k + 1)) / ((double)(k * (2 * q - k + 1))); + _expcX = exp * _expX; + _expX = _expcX; + _expcX = c * _expX; + _expE = _expE + _expcX; + if (p) + _expD = _expD + _expcX; + else + _expD = _expD - _expcX; + p = !p; + } + _expX = _expD.inverseByLU(); + exp = _expX * _expE; + for (int k = 1; k <= s; k++) { + _expE = exp * exp; + exp = _expE; + } + return exp; +#endif } } - /* f = */ frexp(nA, &e); - // double s = (0 > e+1)?0:e+1; - double s = e + 1; - - double sca = 1.0 / pow(2.0, s); - exp = sca * exp; - _expX = *this; - _expE = c * exp + _eye; - _expD = -c * exp + _eye; - for (int k = 2; k <= q; k++) { - c = c * ((double)(q - k + 1)) / ((double)(k * (2 * q - k + 1))); - _expcX = exp * _expX; - _expX = _expcX; - _expcX = c * _expX; - _expE = _expE + _expcX; - if (p) - _expD = _expD + _expcX; - else - _expD = _expD - _expcX; - p = !p; - } - _expX = _expD.inverseByLU(); - exp = _expX * _expE; - for (int k = 1; k <= s; k++) { - _expE = exp * exp; - exp = _expE; + /**************************************************************************************************************/ + /**************************************************************************************************************/ + + // Specific functions + + /* + input:: matrix M(nCols,nRows), nCols > 3, nRows > 3 , nCols == nRows. + + output:: the complement matrix of the element (rowNo,colNo). + This is the matrix obtained from M after elimenating the row rowNo and column + colNo + + example: + 1 2 3 + M = 4 5 6 + 7 8 9 + 1 3 + subblock(M, 1, 1) give the matrix 7 9 + */ + vpMatrix subblock(const vpMatrix &M, unsigned int col, unsigned int row) + { + vpMatrix M_comp; + M_comp.resize(M.getRows() - 1, M.getCols() - 1, false); + + for (unsigned int i = 0; i < col; i++) { + for (unsigned int j = 0; j < row; j++) + M_comp[i][j] = M[i][j]; + for (unsigned int j = row + 1; j < M.getRows(); j++) + M_comp[i][j - 1] = M[i][j]; + } + for (unsigned int i = col + 1; i < M.getCols(); i++) { + for (unsigned int j = 0; j < row; j++) + M_comp[i - 1][j] = M[i][j]; + for (unsigned int j = row + 1; j < M.getRows(); j++) + M_comp[i - 1][j - 1] = M[i][j]; + } + return M_comp; } - return exp; -#endif - } -} -/**************************************************************************************************************/ -/**************************************************************************************************************/ + /*! + \return The condition number, the ratio of the largest singular value of + the matrix to the smallest. -// Specific functions + \param svThreshold: Threshold used to test the singular values. If + a singular value is lower than this threshold we consider that the + matrix is not full rank. -/* -input:: matrix M(nCols,nRows), nCols > 3, nRows > 3 , nCols == nRows. + */ + double vpMatrix::cond(double svThreshold) const + { + unsigned int nbline = getRows(); + unsigned int nbcol = getCols(); -output:: the complement matrix of the element (rowNo,colNo). -This is the matrix obtained from M after elimenating the row rowNo and column -colNo + vpMatrix U, V; // Copy of the matrix, SVD function is destructive + vpColVector sv; + sv.resize(nbcol); // singular values + V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition -example: -1 2 3 -M = 4 5 6 -7 8 9 -1 3 -subblock(M, 1, 1) give the matrix 7 9 -*/ -vpMatrix subblock(const vpMatrix &M, unsigned int col, unsigned int row) -{ - vpMatrix M_comp; - M_comp.resize(M.getRows() - 1, M.getCols() - 1, false); + // Copy and resize matrix to have at least as many rows as columns + // kernel is computed in svd method only if the matrix has more rows than + // columns - for (unsigned int i = 0; i < col; i++) { - for (unsigned int j = 0; j < row; j++) - M_comp[i][j] = M[i][j]; - for (unsigned int j = row + 1; j < M.getRows(); j++) - M_comp[i][j - 1] = M[i][j]; - } - for (unsigned int i = col + 1; i < M.getCols(); i++) { - for (unsigned int j = 0; j < row; j++) - M_comp[i - 1][j] = M[i][j]; - for (unsigned int j = row + 1; j < M.getRows(); j++) - M_comp[i - 1][j - 1] = M[i][j]; - } - return M_comp; -} - -/*! - \return The condition number, the ratio of the largest singular value of - the matrix to the smallest. - - \param svThreshold: Threshold used to test the singular values. If - a singular value is lower than this threshold we consider that the - matrix is not full rank. - - */ -double vpMatrix::cond(double svThreshold) const -{ - unsigned int nbline = getRows(); - unsigned int nbcol = getCols(); + if (nbline < nbcol) + U.resize(nbcol, nbcol, true); + else + U.resize(nbline, nbcol, false); - vpMatrix U, V; // Copy of the matrix, SVD function is destructive - vpColVector sv; - sv.resize(nbcol); // singular values - V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition + U.insert(*this, 0, 0); - // Copy and resize matrix to have at least as many rows as columns - // kernel is computed in svd method only if the matrix has more rows than - // columns + U.svd(sv, V); - if (nbline < nbcol) - U.resize(nbcol, nbcol, true); - else - U.resize(nbline, nbcol, false); + // Compute the highest singular value + double maxsv = 0; + for (unsigned int i = 0; i < nbcol; i++) { + if (sv[i] > maxsv) { + maxsv = sv[i]; + } + } - U.insert(*this, 0, 0); + // Compute the rank of the matrix + unsigned int rank = 0; + for (unsigned int i = 0; i < nbcol; i++) { + if (sv[i] > maxsv * svThreshold) { + rank++; + } + } - U.svd(sv, V); + // Compute the lowest singular value + double minsv = maxsv; + for (unsigned int i = 0; i < rank; i++) { + if (sv[i] < minsv) { + minsv = sv[i]; + } + } - // Compute the highest singular value - double maxsv = 0; - for (unsigned int i = 0; i < nbcol; i++) { - if (sv[i] > maxsv) { - maxsv = sv[i]; + if (std::fabs(minsv) > std::numeric_limits::epsilon()) { + return maxsv / minsv; + } + else { + return std::numeric_limits::infinity(); + } } - } - // Compute the rank of the matrix - unsigned int rank = 0; - for (unsigned int i = 0; i < nbcol; i++) { - if (sv[i] > maxsv * svThreshold) { - rank++; - } - } + /*! + Compute \f${\bf H} + \alpha * diag({\bf H})\f$ + \param H : input Matrix \f${\bf H}\f$. This matrix should be square. + \param alpha : Scalar \f$\alpha\f$ + \param HLM : Resulting operation. + */ + void vpMatrix::computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM) + { + if (H.getCols() != H.getRows()) { + throw(vpException(vpException::dimensionError, "Cannot compute HLM on a non square matrix (%dx%d)", H.getRows(), + H.getCols())); + } - // Compute the lowest singular value - double minsv = maxsv; - for (unsigned int i = 0; i < rank; i++) { - if (sv[i] < minsv) { - minsv = sv[i]; + HLM = H; + for (unsigned int i = 0; i < H.getCols(); i++) { + HLM[i][i] += alpha * H[i][i]; + } } - } - if (std::fabs(minsv) > std::numeric_limits::epsilon()) { - return maxsv / minsv; - } - else { - return std::numeric_limits::infinity(); - } -} + /*! + Compute and return the Frobenius norm (also called Euclidean norm) \f$||A|| = \sqrt{ \sum{A_{ij}^2}}\f$. -/*! - Compute \f${\bf H} + \alpha * diag({\bf H})\f$ - \param H : input Matrix \f${\bf H}\f$. This matrix should be square. - \param alpha : Scalar \f$\alpha\f$ - \param HLM : Resulting operation. - */ -void vpMatrix::computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM) -{ - if (H.getCols() != H.getRows()) { - throw(vpException(vpException::dimensionError, "Cannot compute HLM on a non square matrix (%dx%d)", H.getRows(), - H.getCols())); - } - - HLM = H; - for (unsigned int i = 0; i < H.getCols(); i++) { - HLM[i][i] += alpha * H[i][i]; - } -} - -/*! - Compute and return the Frobenius norm (also called Euclidean norm) \f$||A|| = \sqrt{ \sum{A_{ij}^2}}\f$. - - \return The Frobenius norm (also called Euclidean norm) if the matrix is initialized, 0 otherwise. - - \sa infinityNorm(), inducedL2Norm() -*/ -double vpMatrix::frobeniusNorm() const -{ - double norm = 0.0; - for (unsigned int i = 0; i < dsize; i++) { - double x = *(data + i); - norm += x * x; - } - - return sqrt(norm); -} + \return The Frobenius norm (also called Euclidean norm) if the matrix is initialized, 0 otherwise. -/*! - Compute and return the induced L2 norm \f$||A|| = \Sigma_{max}(A)\f$ which is equal to - the maximum singular value of the matrix. + \sa infinityNorm(), inducedL2Norm() + */ + double vpMatrix::frobeniusNorm() const + { + double norm = 0.0; + for (unsigned int i = 0; i < dsize; i++) { + double x = *(data + i); + norm += x * x; + } - \return The induced L2 norm if the matrix is initialized, 0 otherwise. + return sqrt(norm); + } - \sa infinityNorm(), frobeniusNorm() -*/ -double vpMatrix::inducedL2Norm() const -{ - if (this->dsize != 0) { - vpMatrix v; - vpColVector w; - - vpMatrix M = *this; - - M.svd(w, v); - - double max = w[0]; - unsigned int maxRank = std::min(this->getCols(), this->getRows()); - // The maximum reachable rank is either the number of columns or the number of rows - // of the matrix. - unsigned int boundary = std::min(maxRank, w.size()); - // boundary is here to ensure that the number of singular values used for the com- - // putation of the euclidean norm of the matrix is not greater than the maximum - // reachable rank. Indeed, some svd library pad the singular values vector with 0s - // if the input matrix is non-square. - for (unsigned int i = 0; i < boundary; i++) { - if (max < w[i]) { - max = w[i]; + /*! + Compute and return the induced L2 norm \f$||A|| = \Sigma_{max}(A)\f$ which is equal to + the maximum singular value of the matrix. + + \return The induced L2 norm if the matrix is initialized, 0 otherwise. + + \sa infinityNorm(), frobeniusNorm() + */ + double vpMatrix::inducedL2Norm() const + { + if (this->dsize != 0) { + vpMatrix v; + vpColVector w; + + vpMatrix M = *this; + + M.svd(w, v); + + double max = w[0]; + unsigned int maxRank = std::min(this->getCols(), this->getRows()); + // The maximum reachable rank is either the number of columns or the number of rows + // of the matrix. + unsigned int boundary = std::min(maxRank, w.size()); + // boundary is here to ensure that the number of singular values used for the com- + // putation of the euclidean norm of the matrix is not greater than the maximum + // reachable rank. Indeed, some svd library pad the singular values vector with 0s + // if the input matrix is non-square. + for (unsigned int i = 0; i < boundary; i++) { + if (max < w[i]) { + max = w[i]; + } + } + return max; + } + else { + return 0.; } } - return max; - } - else { - return 0.; - } -} -/*! + /*! - Compute and return the infinity norm \f$ {||A||}_{\infty} = - max\left(\sum_{j=0}^{n}{\mid A_{ij} \mid}\right) \f$ with \f$i \in - \{0, ..., m\}\f$ where \f$(m,n)\f$ is the matrix size. + Compute and return the infinity norm \f$ {||A||}_{\infty} = + max\left(\sum_{j=0}^{n}{\mid A_{ij} \mid}\right) \f$ with \f$i \in + \{0, ..., m\}\f$ where \f$(m,n)\f$ is the matrix size. - \return The infinity norm if the matrix is initialized, 0 otherwise. + \return The infinity norm if the matrix is initialized, 0 otherwise. - \sa frobeniusNorm(), inducedL2Norm() -*/ -double vpMatrix::infinityNorm() const -{ - double norm = 0.0; - for (unsigned int i = 0; i < rowNum; i++) { - double x = 0; - for (unsigned int j = 0; j < colNum; j++) { - x += fabs(*(*(rowPtrs + i) + j)); - } - if (x > norm) { - norm = x; + \sa frobeniusNorm(), inducedL2Norm() + */ + double vpMatrix::infinityNorm() const + { + double norm = 0.0; + for (unsigned int i = 0; i < rowNum; i++) { + double x = 0; + for (unsigned int j = 0; j < colNum; j++) { + x += fabs(*(*(rowPtrs + i) + j)); + } + if (x > norm) { + norm = x; + } + } + return norm; } - } - return norm; -} -/*! - Return the sum square of all the \f$A_{ij}\f$ elements of the matrix \f$A(m, - n)\f$. + /*! + Return the sum square of all the \f$A_{ij}\f$ elements of the matrix \f$A(m, + n)\f$. - \return The value \f$\sum A_{ij}^{2}\f$. - */ -double vpMatrix::sumSquare() const -{ - double sum_square = 0.0; - double x; + \return The value \f$\sum A_{ij}^{2}\f$. + */ + double vpMatrix::sumSquare() const + { + double sum_square = 0.0; + double x; - for (unsigned int i = 0; i < rowNum; i++) { - for (unsigned int j = 0; j < colNum; j++) { - x = rowPtrs[i][j]; - sum_square += x * x; - } - } + for (unsigned int i = 0; i < rowNum; i++) { + for (unsigned int j = 0; j < colNum; j++) { + x = rowPtrs[i][j]; + sum_square += x * x; + } + } - return sum_square; -} + return sum_square; + } #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS) /*! \deprecated This function is deprecated. You should rather use frobeniusNorm(). @@ -6801,89 +6851,89 @@ double vpMatrix::sumSquare() const \sa frobeniusNorm(), infinityNorm(), inducedL2Norm() */ -double vpMatrix::euclideanNorm() const { return frobeniusNorm(); } - -vpMatrix vpMatrix::stackMatrices(const vpColVector &A, const vpColVector &B) -{ - return (vpMatrix)(vpColVector::stack(A, B)); -} - -void vpMatrix::stackMatrices(const vpColVector &A, const vpColVector &B, vpColVector &C) -{ - vpColVector::stack(A, B, C); -} - -vpMatrix vpMatrix::stackMatrices(const vpMatrix &A, const vpRowVector &B) { return vpMatrix::stack(A, B); } - -void vpMatrix::stackMatrices(const vpMatrix &A, const vpRowVector &B, vpMatrix &C) { vpMatrix::stack(A, B, C); } - -/*! - \deprecated This method is deprecated. You should rather use getRow(). - More precisely, the following code: - \code - vpMatrix L; - unsigned int row_index = ...; - ... = L.row(row_index); - \endcode - should be replaced with: - \code - ... = L.getRow(row_index - 1); - \endcode - - \warning Notice row(1) is the 0th row. - This function returns the i-th row of the matrix. - \param i : Index of the row to extract noting that row index start at 1 to get the first row. - -*/ -vpRowVector vpMatrix::row(unsigned int i) -{ - vpRowVector c(getCols()); - - for (unsigned int j = 0; j < getCols(); j++) - c[j] = (*this)[i - 1][j]; - return c; -} + double vpMatrix::euclideanNorm() const { return frobeniusNorm(); } -/*! - \deprecated This method is deprecated. You should rather use getCol(). - More precisely, the following code: - \code - vpMatrix L; - unsigned int column_index = ...; - ... = L.column(column_index); - \endcode - should be replaced with: - \code - ... = L.getCol(column_index - 1); - \endcode + vpMatrix vpMatrix::stackMatrices(const vpColVector &A, const vpColVector &B) + { + return (vpMatrix)(vpColVector::stack(A, B)); + } - \warning Notice column(1) is the 0-th column. - This function returns the j-th columns of the matrix. - \param j : Index of the column to extract noting that column index start at 1 to get the first column. -*/ -vpColVector vpMatrix::column(unsigned int j) -{ - vpColVector c(getRows()); + void vpMatrix::stackMatrices(const vpColVector &A, const vpColVector &B, vpColVector &C) + { + vpColVector::stack(A, B, C); + } - for (unsigned int i = 0; i < getRows(); i++) - c[i] = (*this)[i][j - 1]; - return c; -} + vpMatrix vpMatrix::stackMatrices(const vpMatrix &A, const vpRowVector &B) { return vpMatrix::stack(A, B); } + + void vpMatrix::stackMatrices(const vpMatrix &A, const vpRowVector &B, vpMatrix &C) { vpMatrix::stack(A, B, C); } + + /*! + \deprecated This method is deprecated. You should rather use getRow(). + More precisely, the following code: + \code + vpMatrix L; + unsigned int row_index = ...; + ... = L.row(row_index); + \endcode + should be replaced with: + \code + ... = L.getRow(row_index - 1); + \endcode + + \warning Notice row(1) is the 0th row. + This function returns the i-th row of the matrix. + \param i : Index of the row to extract noting that row index start at 1 to get the first row. + + */ + vpRowVector vpMatrix::row(unsigned int i) + { + vpRowVector c(getCols()); + + for (unsigned int j = 0; j < getCols(); j++) + c[j] = (*this)[i - 1][j]; + return c; + } -/*! - \deprecated You should rather use diag(const double &) + /*! + \deprecated This method is deprecated. You should rather use getCol(). + More precisely, the following code: + \code + vpMatrix L; + unsigned int column_index = ...; + ... = L.column(column_index); + \endcode + should be replaced with: + \code + ... = L.getCol(column_index - 1); + \endcode + + \warning Notice column(1) is the 0-th column. + This function returns the j-th columns of the matrix. + \param j : Index of the column to extract noting that column index start at 1 to get the first column. + */ + vpColVector vpMatrix::column(unsigned int j) + { + vpColVector c(getRows()); + + for (unsigned int i = 0; i < getRows(); i++) + c[i] = (*this)[i][j - 1]; + return c; + } - Set the matrix diagonal elements to \e val. - More generally set M[i][i] = val. -*/ -void vpMatrix::setIdentity(const double &val) -{ - for (unsigned int i = 0; i < rowNum; i++) - for (unsigned int j = 0; j < colNum; j++) - if (i == j) - (*this)[i][j] = val; - else - (*this)[i][j] = 0; -} + /*! + \deprecated You should rather use diag(const double &) + + Set the matrix diagonal elements to \e val. + More generally set M[i][i] = val. + */ + void vpMatrix::setIdentity(const double &val) + { + for (unsigned int i = 0; i < rowNum; i++) + for (unsigned int j = 0; j < colNum; j++) + if (i == j) + (*this)[i][j] = val; + else + (*this)[i][j] = 0; + } #endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS) diff --git a/modules/core/src/math/transformation/vpXmlParserHomogeneousMatrix.cpp b/modules/core/src/math/transformation/vpXmlParserHomogeneousMatrix.cpp index 66ded5db86..10a23d4fb7 100644 --- a/modules/core/src/math/transformation/vpXmlParserHomogeneousMatrix.cpp +++ b/modules/core/src/math/transformation/vpXmlParserHomogeneousMatrix.cpp @@ -44,6 +44,7 @@ */ #include +#if defined(VISP_HAVE_PUGIXML) #include /* ----------------------------- LABEL XML ----------------------------- */ @@ -67,7 +68,8 @@ class vpXmlParserHomogeneousMatrix::Impl private: /* --- XML Code------------------------------------------------------------ */ - enum vpXmlCodeType { + enum vpXmlCodeType + { CODE_XML_BAD = -1, CODE_XML_OTHER, CODE_XML_M, @@ -82,7 +84,7 @@ class vpXmlParserHomogeneousMatrix::Impl }; public: - Impl() : m_M(), m_name() {} + Impl() : m_M(), m_name() { } int parse(vpHomogeneousMatrix &M, const std::string &filename, const std::string &name) { @@ -132,18 +134,20 @@ class vpXmlParserHomogeneousMatrix::Impl if (prop == CODE_XML_M) { if (SEQUENCE_OK == read_matrix(node, name)) nbM++; - } else + } + else back = SEQUENCE_ERROR; } if (nbM == 0) { back = SEQUENCE_ERROR; std::cerr << "No Homogeneous matrix is available" << std::endl << "with name: " << name << std::endl; - } else if (nbM > 1) { + } + else if (nbM > 1) { back = SEQUENCE_ERROR; std::cerr << nbM << " There are more Homogeneous matrix" << std::endl - << "with the same name : " << std::endl - << "precise your choice..." << std::endl; + << "with the same name : " << std::endl + << "precise your choice..." << std::endl; } return back; @@ -206,7 +210,8 @@ class vpXmlParserHomogeneousMatrix::Impl if (!(name == M_name_tmp)) { back = SEQUENCE_ERROR; - } else { + } + else { this->m_M = M_tmp; // std::cout << "Convert in Homogeneous Matrix:"<< std::endl; // std::cout << this-> M << std::endl; @@ -335,8 +340,8 @@ class vpXmlParserHomogeneousMatrix::Impl if (M_isFound) { std::cout << "There is already an homogeneous matrix " << std::endl - << "available in the file with the input name: " << name << "." << std::endl - << "Please delete it manually from the xml file." << std::endl; + << "available in the file with the input name: " << name << "." << std::endl + << "Please delete it manually from the xml file." << std::endl; return SEQUENCE_ERROR; } @@ -459,23 +464,32 @@ class vpXmlParserHomogeneousMatrix::Impl if (!strcmp(str, LABEL_XML_M)) { val_int = CODE_XML_M; - } else if (!strcmp(str, LABEL_XML_M_NAME)) { + } + else if (!strcmp(str, LABEL_XML_M_NAME)) { val_int = CODE_XML_M_NAME; - } else if (!strcmp(str, LABEL_XML_VALUE)) { + } + else if (!strcmp(str, LABEL_XML_VALUE)) { val_int = CODE_XML_VALUE; - } else if (!strcmp(str, LABEL_XML_TX)) { + } + else if (!strcmp(str, LABEL_XML_TX)) { val_int = CODE_XML_TX; - } else if (!strcmp(str, LABEL_XML_TY)) { + } + else if (!strcmp(str, LABEL_XML_TY)) { val_int = CODE_XML_TY; - } else if (!strcmp(str, LABEL_XML_TZ)) { + } + else if (!strcmp(str, LABEL_XML_TZ)) { val_int = CODE_XML_TZ; - } else if (!strcmp(str, LABEL_XML_TUX)) { + } + else if (!strcmp(str, LABEL_XML_TUX)) { val_int = CODE_XML_TUX; - } else if (!strcmp(str, LABEL_XML_TUY)) { + } + else if (!strcmp(str, LABEL_XML_TUY)) { val_int = CODE_XML_TUY; - } else if (!strcmp(str, LABEL_XML_TUZ)) { + } + else if (!strcmp(str, LABEL_XML_TUZ)) { val_int = CODE_XML_TUZ; - } else { + } + else { val_int = CODE_XML_OTHER; } res = val_int; @@ -494,7 +508,7 @@ class vpXmlParserHomogeneousMatrix::Impl }; #endif // DOXYGEN_SHOULD_SKIP_THIS -vpXmlParserHomogeneousMatrix::vpXmlParserHomogeneousMatrix() : m_impl(new Impl()) {} +vpXmlParserHomogeneousMatrix::vpXmlParserHomogeneousMatrix() : m_impl(new Impl()) { } vpXmlParserHomogeneousMatrix::~vpXmlParserHomogeneousMatrix() { delete m_impl; } @@ -539,3 +553,9 @@ void vpXmlParserHomogeneousMatrix::setHomogeneousMatrixName(const std::string &n { m_impl->setHomogeneousMatrixName(name); } + +#elif !defined(VISP_BUILD_SHARED_LIBS) +// Work around to avoid warning: libvisp_core.a(vpXmlParserHomogeneousMatrix.cpp.o) has no symbols +void dummy_vpXmlParserHomogeneousMatrix() { }; + +#endif diff --git a/modules/core/src/tools/cpu-features/vpCPUFeatures.cpp b/modules/core/src/tools/cpu-features/vpCPUFeatures.cpp index 6f760b262b..333d586e08 100644 --- a/modules/core/src/tools/cpu-features/vpCPUFeatures.cpp +++ b/modules/core/src/tools/cpu-features/vpCPUFeatures.cpp @@ -33,8 +33,11 @@ * *****************************************************************************/ +#include #include "x86/cpu_x86.h" -#include +#if defined(VISP_HAVE_SIMDLIB) +#include +#endif #include namespace vpCPUFeatures @@ -56,6 +59,7 @@ bool checkAVX() { return cpu_features.HW_AVX; } bool checkAVX2() { return cpu_features.HW_AVX2; } +#if defined(VISP_HAVE_SIMDLIB) size_t getCPUCacheL1() { return SimdCpuInfo(SimdCpuInfoCacheL1); } size_t getCPUCacheL2() { return SimdCpuInfo(SimdCpuInfoCacheL2); } @@ -63,6 +67,7 @@ size_t getCPUCacheL2() { return SimdCpuInfo(SimdCpuInfoCacheL2); } size_t getCPUCacheL3() { return SimdCpuInfo(SimdCpuInfoCacheL3); } bool checkNeon() { return SimdCpuInfo(SimdCpuInfoNeon) != 0; } +#endif void printCPUInfo() { cpu_features.print(); } } // namespace vpCPUFeatures diff --git a/modules/core/src/tools/geometry/vpXmlParserRectOriented.cpp b/modules/core/src/tools/geometry/vpXmlParserRectOriented.cpp index 2679726ce0..2822e9ef75 100644 --- a/modules/core/src/tools/geometry/vpXmlParserRectOriented.cpp +++ b/modules/core/src/tools/geometry/vpXmlParserRectOriented.cpp @@ -44,14 +44,18 @@ #include #include + +#if defined(VISP_HAVE_PUGIXML) #include + #include #ifndef DOXYGEN_SHOULD_SKIP_THIS class vpXmlParserRectOriented::Impl { private: - enum vpXmlCodeType { + enum vpXmlCodeType + { CODE_XML_BAD = -1, CODE_XML_OTHER, CODE_XML_CENTER_I, @@ -127,7 +131,8 @@ class vpXmlParserRectOriented::Impl root_node = doc.append_child(pugi::node_declaration); root_node.append_attribute("version") = "1.0"; root_node = doc.append_child("config"); - } else if (!append) { + } + else if (!append) { if (!vpIoTools::remove(filename)) throw vpException(vpException::ioError, "Cannot remove existing xml file"); @@ -168,7 +173,7 @@ class vpXmlParserRectOriented::Impl }; #endif // DOXYGEN_SHOULD_SKIP_THIS -vpXmlParserRectOriented::vpXmlParserRectOriented() : m_impl(new Impl()) {} +vpXmlParserRectOriented::vpXmlParserRectOriented() : m_impl(new Impl()) { } vpXmlParserRectOriented::~vpXmlParserRectOriented() { delete m_impl; } @@ -197,3 +202,9 @@ void vpXmlParserRectOriented::save(const std::string &filename, bool append) { m vpRectOriented vpXmlParserRectOriented::getRectangle() const { return m_impl->getRectangle(); } void vpXmlParserRectOriented::setRectangle(const vpRectOriented &rectangle) { m_impl->setRectangle(rectangle); } + +#elif !defined(VISP_BUILD_SHARED_LIBS) +// Work around to avoid warning: libvisp_core.a(vpXmlParserRectOriented.cpp.o) has no symbols +void dummy_vpXmlParserRectOriented() { }; + +#endif diff --git a/modules/core/test/camera/testXmlParserCamera.cpp b/modules/core/test/camera/testXmlParserCamera.cpp index 10e1202de3..f179ef151c 100644 --- a/modules/core/test/camera/testXmlParserCamera.cpp +++ b/modules/core/test/camera/testXmlParserCamera.cpp @@ -44,6 +44,8 @@ int main() { +#if defined(VISP_HAVE_PUGIXML) + #if defined(_WIN32) std::string tmp_dir = "C:/temp/"; #else @@ -266,6 +268,7 @@ int main() } vpIoTools::remove(tmp_dir); +#endif return EXIT_SUCCESS; } diff --git a/modules/core/test/image-with-dataset/perfGaussianFilter.cpp b/modules/core/test/image-with-dataset/perfGaussianFilter.cpp index 570ca6340d..62adcee3c9 100644 --- a/modules/core/test/image-with-dataset/perfGaussianFilter.cpp +++ b/modules/core/test/image-with-dataset/perfGaussianFilter.cpp @@ -35,7 +35,7 @@ #include -#ifdef VISP_HAVE_CATCH2 +#if defined(VISP_HAVE_SIMDLIB) && defined(VISP_HAVE_CATCH2) #define CATCH_CONFIG_ENABLE_BENCHMARKING #define CATCH_CONFIG_RUNNER #include diff --git a/modules/core/test/image-with-dataset/testColorConversion.cpp b/modules/core/test/image-with-dataset/testColorConversion.cpp index 0b86c05976..705aceca7d 100644 --- a/modules/core/test/image-with-dataset/testColorConversion.cpp +++ b/modules/core/test/image-with-dataset/testColorConversion.cpp @@ -964,7 +964,7 @@ TEST_CASE("Bayer conversion", "[image_conversion]") SECTION("BGGR") { const std::string filename = - vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "Bayer/Klimt_Bayer_560x558_BGGR_12bits.raw"); + vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "Bayer/Klimt_Bayer_560x558_BGGR_12bits.raw"); readBinaryFile(filename, buffer); col2im(buffer, I_Bayer_16U); @@ -995,7 +995,7 @@ TEST_CASE("Bayer conversion", "[image_conversion]") SECTION("GBRG") { const std::string filename = - vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "Bayer/Klimt_Bayer_560x558_GBRG_12bits.raw"); + vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "Bayer/Klimt_Bayer_560x558_GBRG_12bits.raw"); readBinaryFile(filename, buffer); col2im(buffer, I_Bayer_16U); @@ -1026,7 +1026,7 @@ TEST_CASE("Bayer conversion", "[image_conversion]") SECTION("GRBG") { const std::string filename = - vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "Bayer/Klimt_Bayer_560x558_GRBG_12bits.raw"); + vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "Bayer/Klimt_Bayer_560x558_GRBG_12bits.raw"); readBinaryFile(filename, buffer); col2im(buffer, I_Bayer_16U); @@ -1057,7 +1057,7 @@ TEST_CASE("Bayer conversion", "[image_conversion]") SECTION("RGGB") { const std::string filename = - vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "Bayer/Klimt_Bayer_560x558_RGGB_12bits.raw"); + vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "Bayer/Klimt_Bayer_560x558_RGGB_12bits.raw"); readBinaryFile(filename, buffer); col2im(buffer, I_Bayer_16U); @@ -1095,7 +1095,7 @@ TEST_CASE("Bayer conversion", "[image_conversion]") SECTION("BGGR") { const std::string filename = - vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "Bayer/Klimt_Bayer_560x558_BGGR_08bits.raw"); + vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "Bayer/Klimt_Bayer_560x558_BGGR_08bits.raw"); std::FILE *f = std::fopen(filename.c_str(), "rb"); size_t sread = std::fread(&buffer[0], sizeof buffer[0], buffer.size(), f); @@ -1128,7 +1128,7 @@ TEST_CASE("Bayer conversion", "[image_conversion]") SECTION("GBRG") { const std::string filename = - vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "Bayer/Klimt_Bayer_560x558_GBRG_08bits.raw"); + vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "Bayer/Klimt_Bayer_560x558_GBRG_08bits.raw"); std::FILE *f = std::fopen(filename.c_str(), "rb"); size_t sread = std::fread(&buffer[0], sizeof buffer[0], buffer.size(), f); @@ -1161,7 +1161,7 @@ TEST_CASE("Bayer conversion", "[image_conversion]") SECTION("GRBG") { const std::string filename = - vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "Bayer/Klimt_Bayer_560x558_GRBG_08bits.raw"); + vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "Bayer/Klimt_Bayer_560x558_GRBG_08bits.raw"); std::FILE *f = std::fopen(filename.c_str(), "rb"); size_t sread = std::fread(&buffer[0], sizeof buffer[0], buffer.size(), f); @@ -1194,7 +1194,7 @@ TEST_CASE("Bayer conversion", "[image_conversion]") SECTION("RGGB") { const std::string filename = - vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "Bayer/Klimt_Bayer_560x558_RGGB_08bits.raw"); + vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "Bayer/Klimt_Bayer_560x558_RGGB_08bits.raw"); std::FILE *f = std::fopen(filename.c_str(), "rb"); size_t sread = std::fread(&buffer[0], sizeof buffer[0], buffer.size(), f); diff --git a/modules/core/test/image-with-dataset/testGaussianFilter.cpp b/modules/core/test/image-with-dataset/testGaussianFilter.cpp index e736cd4807..7c2c1a66a7 100644 --- a/modules/core/test/image-with-dataset/testGaussianFilter.cpp +++ b/modules/core/test/image-with-dataset/testGaussianFilter.cpp @@ -41,7 +41,7 @@ \brief Test Gaussian filter. */ -#if defined(VISP_HAVE_CATCH2) && (VISP_HAVE_DATASET_VERSION >= 0x030400) +#if defined(VISP_HAVE_SIMDLIB) && defined(VISP_HAVE_CATCH2) && (VISP_HAVE_DATASET_VERSION >= 0x030400) #define CATCH_CONFIG_RUNNER #include #include @@ -55,7 +55,7 @@ TEST_CASE("Test vpGaussianFilter (unsigned char)") vpImage I; vpImageIo::read(I, filepath); - std::vector sigmas = {0.5f, 2.0f, 5.0f, 7.0f}; + std::vector sigmas = { 0.5f, 2.0f, 5.0f, 7.0f }; for (auto sigma : sigmas) { vpGaussianFilter gaussianFilter(I.getWidth(), I.getHeight(), sigma); @@ -86,7 +86,7 @@ TEST_CASE("Test vpGaussianFilter (vpRGBa)") vpImage I; vpImageIo::read(I, filepath); - std::vector sigmas = {0.5f, 2.0f, 5.0f, 7.0f}; + std::vector sigmas = { 0.5f, 2.0f, 5.0f, 7.0f }; for (auto sigma : sigmas) { vpGaussianFilter gaussianFilter(I.getWidth(), I.getHeight(), sigma); @@ -112,8 +112,8 @@ TEST_CASE("Test vpGaussianFilter (vpRGBa)") vpImageConvert::convert(I_diff_B, I_diff_B_dbl); std::cout << "sigma: " << sigma << " ; I_diff_R_dbl: " << I_diff_R_dbl.getMeanValue() - << " ; I_diff_G_dbl: " << I_diff_G_dbl.getMeanValue() - << " ; I_diff_B_dbl: " << I_diff_B_dbl.getMeanValue() << std::endl; + << " ; I_diff_G_dbl: " << I_diff_G_dbl.getMeanValue() + << " ; I_diff_B_dbl: " << I_diff_B_dbl.getMeanValue() << std::endl; const double threshold = 1.5; CHECK(I_diff_R_dbl.getMeanValue() < threshold); CHECK(I_diff_G_dbl.getMeanValue() < threshold); @@ -127,7 +127,7 @@ TEST_CASE("Test vpGaussianFilter (vpRGBa + deinterleave)") vpImage I; vpImageIo::read(I, filepath); - std::vector sigmas = {0.5f, 2.0f, 5.0f, 7.0f}; + std::vector sigmas = { 0.5f, 2.0f, 5.0f, 7.0f }; for (auto sigma : sigmas) { const bool deinterleave = true; vpGaussianFilter gaussianFilter(I.getWidth(), I.getHeight(), sigma, deinterleave); @@ -154,8 +154,8 @@ TEST_CASE("Test vpGaussianFilter (vpRGBa + deinterleave)") vpImageConvert::convert(I_diff_B, I_diff_B_dbl); std::cout << "sigma: " << sigma << " ; I_diff_R_dbl: " << I_diff_R_dbl.getMeanValue() - << " ; I_diff_G_dbl: " << I_diff_G_dbl.getMeanValue() - << " ; I_diff_B_dbl: " << I_diff_B_dbl.getMeanValue() << std::endl; + << " ; I_diff_G_dbl: " << I_diff_G_dbl.getMeanValue() + << " ; I_diff_B_dbl: " << I_diff_B_dbl.getMeanValue() << std::endl; const double threshold = 1.5; CHECK(I_diff_R_dbl.getMeanValue() < threshold); CHECK(I_diff_G_dbl.getMeanValue() < threshold); diff --git a/modules/core/test/image-with-dataset/testImageWarp.cpp b/modules/core/test/image-with-dataset/testImageWarp.cpp index eba7b093fc..3bebc67edb 100644 --- a/modules/core/test/image-with-dataset/testImageWarp.cpp +++ b/modules/core/test/image-with-dataset/testImageWarp.cpp @@ -58,10 +58,10 @@ static const double g_threshold_percentage_bilinear = 0.75; static const double g_threshold_percentage_pers = 0.75; static const double g_threshold_percentage_pers_bilinear = 0.65; -static const std::vector interp_methods = {vpImageTools::INTERPOLATION_NEAREST, - vpImageTools::INTERPOLATION_LINEAR}; -static const std::vector interp_names = {"Nearest Neighbor", "Bilinear"}; -static const std::vector suffixes = {"_NN.png", "_bilinear.png"}; +static const std::vector interp_methods = { vpImageTools::INTERPOLATION_NEAREST, + vpImageTools::INTERPOLATION_LINEAR }; +static const std::vector interp_names = { "Nearest Neighbor", "Bilinear" }; +static const std::vector suffixes = { "_NN.png", "_bilinear.png" }; bool almostEqual(const vpImage &I1, const vpImage &I2, double threshold_val, double threshold_percentage, double &percentage) @@ -180,6 +180,7 @@ TEST_CASE("Affine warp on grayscale", "[warp_image]") vpIoTools::getViSPImagesDataPath(), std::string("warp/cv_warp_affine_rot_45_gray" + suffixes[i])); REQUIRE(vpIoTools::checkFilename(refImgPath)); vpImage I_ref_opencv; + vpImageIo::read(I_ref_opencv, refImgPath); vpImage I_affine; @@ -548,10 +549,10 @@ TEST_CASE("Perspective warp on grayscale", "[warp_image]") double percentage = 0.0; bool equal = - almostEqual(I_ref, I_perspective, g_threshold_value, - (i == 0) ? g_threshold_percentage_pers : g_threshold_percentage_pers_bilinear, percentage); + almostEqual(I_ref, I_perspective, g_threshold_value, + (i == 0) ? g_threshold_percentage_pers : g_threshold_percentage_pers_bilinear, percentage); std::cout << "Percentage valid pixels (Homography " << interp_names[i] << " Ref): " << percentage - << std::endl; + << std::endl; CHECK(equal); } @@ -568,10 +569,10 @@ TEST_CASE("Perspective warp on grayscale", "[warp_image]") double percentage = 0.0; bool equal = - almostEqual(I_ref_opencv, I_perspective, g_threshold_value, - (i == 0) ? g_threshold_percentage_pers : g_threshold_percentage_pers_bilinear, percentage); + almostEqual(I_ref_opencv, I_perspective, g_threshold_value, + (i == 0) ? g_threshold_percentage_pers : g_threshold_percentage_pers_bilinear, percentage); std::cout << "Percentage valid pixels (Homography " << interp_names[i] << " OpenCV): " << percentage - << std::endl; + << std::endl; CHECK(equal); } @@ -588,10 +589,10 @@ TEST_CASE("Perspective warp on grayscale", "[warp_image]") double percentage = 0.0; bool equal = - almostEqual(I_ref_pil, I_perspective, g_threshold_value, - (i == 0) ? g_threshold_percentage_pers : g_threshold_percentage_pers_bilinear, percentage); + almostEqual(I_ref_pil, I_perspective, g_threshold_value, + (i == 0) ? g_threshold_percentage_pers : g_threshold_percentage_pers_bilinear, percentage); std::cout << "Percentage valid pixels (Homography " << interp_names[i] << " PIL): " << percentage - << std::endl; + << std::endl; CHECK(equal); } } @@ -660,10 +661,10 @@ TEST_CASE("Perspective warp on color", "[warp_image]") double percentage = 0.0; bool equal = - almostEqual(I_ref, I_perspective, g_threshold_value, - (i == 0) ? g_threshold_percentage_pers : g_threshold_percentage_pers_bilinear, percentage); + almostEqual(I_ref, I_perspective, g_threshold_value, + (i == 0) ? g_threshold_percentage_pers : g_threshold_percentage_pers_bilinear, percentage); std::cout << "Percentage valid pixels (Homography " << interp_names[i] << " Ref): " << percentage - << std::endl; + << std::endl; CHECK(equal); } @@ -680,10 +681,10 @@ TEST_CASE("Perspective warp on color", "[warp_image]") double percentage = 0.0; bool equal = - almostEqual(I_ref_opencv, I_perspective, g_threshold_value, - (i == 0) ? g_threshold_percentage_pers : g_threshold_percentage_pers_bilinear, percentage); + almostEqual(I_ref_opencv, I_perspective, g_threshold_value, + (i == 0) ? g_threshold_percentage_pers : g_threshold_percentage_pers_bilinear, percentage); std::cout << "Percentage valid pixels (Homography " << interp_names[i] << " OpenCV): " << percentage - << std::endl; + << std::endl; CHECK(equal); } @@ -700,10 +701,10 @@ TEST_CASE("Perspective warp on color", "[warp_image]") double percentage = 0.0; bool equal = - almostEqual(I_ref_pil, I_perspective, g_threshold_value, - (i == 0) ? g_threshold_percentage_pers : g_threshold_percentage_pers_bilinear, percentage); + almostEqual(I_ref_pil, I_perspective, g_threshold_value, + (i == 0) ? g_threshold_percentage_pers : g_threshold_percentage_pers_bilinear, percentage); std::cout << "Percentage valid pixels (Homography " << interp_names[i] << " PIL): " << percentage - << std::endl; + << std::endl; CHECK(equal); } } diff --git a/modules/core/test/image-with-dataset/testIoEXR.cpp b/modules/core/test/image-with-dataset/testIoEXR.cpp index ead98edbe9..88d0001736 100644 --- a/modules/core/test/image-with-dataset/testIoEXR.cpp +++ b/modules/core/test/image-with-dataset/testIoEXR.cpp @@ -41,7 +41,9 @@ #include -#if defined(VISP_HAVE_CATCH2) && (VISP_HAVE_DATASET_VERSION >= 0x030600) +// EXR format is only supported when OpenCV or TinyEXR 3rd parties are available +#if defined(VISP_HAVE_CATCH2) && (VISP_HAVE_DATASET_VERSION >= 0x030600) && \ + defined(HAVE_OPENCV_IMGCODECS) && defined(VISP_HAVE_TINYEXR) #define CATCH_CONFIG_RUNNER #include diff --git a/modules/core/test/math/testXmlParserHomogeneousMatrix.cpp b/modules/core/test/math/testXmlParserHomogeneousMatrix.cpp index 4f35ee6966..ffc21d3a3d 100644 --- a/modules/core/test/math/testXmlParserHomogeneousMatrix.cpp +++ b/modules/core/test/math/testXmlParserHomogeneousMatrix.cpp @@ -44,6 +44,7 @@ int main() { +#if defined(VISP_HAVE_PUGIXML) #if defined(_WIN32) std::string tmp_dir = "C:/temp/"; #else @@ -89,6 +90,7 @@ int main() } vpIoTools::remove(tmp_dir); +#endif return EXIT_SUCCESS; } diff --git a/modules/core/test/tools/geometry/testXmlParserRectOriented.cpp b/modules/core/test/tools/geometry/testXmlParserRectOriented.cpp index e2d75f902f..17b5c4b7e5 100644 --- a/modules/core/test/tools/geometry/testXmlParserRectOriented.cpp +++ b/modules/core/test/tools/geometry/testXmlParserRectOriented.cpp @@ -44,6 +44,7 @@ int main() { +#if defined(VISP_HAVE_PUGIXML) #if defined(_WIN32) std::string tmp_dir = "C:/temp/"; #else @@ -85,6 +86,7 @@ int main() } vpIoTools::remove(tmp_dir); +#endif return EXIT_SUCCESS; } diff --git a/modules/gui/src/plot/vpPlotGraph.cpp b/modules/gui/src/plot/vpPlotGraph.cpp index 4d11ea617a..8ba2d3c383 100644 --- a/modules/gui/src/plot/vpPlotGraph.cpp +++ b/modules/gui/src/plot/vpPlotGraph.cpp @@ -41,7 +41,6 @@ #include #include #include -//#include #include #include @@ -171,22 +170,7 @@ void vpPlotGraph::findPose() iP[3].set_ij(dHeight - 1, 0); double x = 0, y = 0; -#if 0 - // Modified by FS to remove dependency with visp_vision (pose) module - vpPose pose; - pose.clearPoint(); - for (unsigned int i = 0; i < 4; i++) { - vpPixelMeterConversion::convertPoint(cam, iP[i], x, y); - point_[i].set_x(x); - point_[i].set_y(y); - pose.addPoint(point_[i]); - } - - // Pose by Dementhon or Lagrange provides an initialization of the non linear virtual visual-servoing pose estimation - pose.computePose(vpPose::DEMENTHON_LAGRANGE_VIRTUAL_VS, cMo); - -#else // Instead of pose computation we use an approximation double Z = 0; for (unsigned int i = 0; i < 4; i++) { @@ -195,7 +179,6 @@ void vpPlotGraph::findPose() Z = vpMath::maximum(Z, point_[i].get_oY() / y); } cMo[2][3] = Z; -#endif } void vpPlotGraph::computeGraphParameters() diff --git a/modules/imgproc/src/vpImgproc.cpp b/modules/imgproc/src/vpImgproc.cpp index d7d48eb2bb..f3adc5ddc3 100644 --- a/modules/imgproc/src/vpImgproc.cpp +++ b/modules/imgproc/src/vpImgproc.cpp @@ -411,10 +411,18 @@ void stretchContrastHSV(const vpImage &I1, vpImage &I2) void unsharpMask(vpImage &I, float sigma, double weight) { if (weight < 1.0 && weight >= 0.0) { +#if defined(VISP_HAVE_SIMDLIB) // Gaussian blurred image vpGaussianFilter gaussian_filter(I.getWidth(), I.getHeight(), sigma); vpImage I_blurred; gaussian_filter.apply(I, I_blurred); +#else + // Gaussian blurred image + vpImage I_blurred; + unsigned int size = 7; + (void)sigma; + vpImageFilter::gaussianBlur(I, I_blurred, size); +#endif // Unsharp mask for (unsigned int cpt = 0; cpt < I.getSize(); cpt++) { @@ -434,17 +442,35 @@ void unsharpMask(const vpImage &I, vpImage &Ires, void unsharpMask(vpImage &I, float sigma, double weight) { if (weight < 1.0 && weight >= 0.0) { +#if defined(VISP_HAVE_SIMDLIB) // Gaussian blurred image vpGaussianFilter gaussian_filter(I.getWidth(), I.getHeight(), sigma); vpImage I_blurred; gaussian_filter.apply(I, I_blurred); +#else + // Gaussian blurred image + vpImage I_blurred_R, I_blurred_G, I_blurred_B; + vpImage I_R, I_G, I_B; + unsigned int size = 7; + (void)sigma; + + vpImageConvert::split(I, &I_R, &I_G, &I_B); + vpImageFilter::gaussianBlur(I_R, I_blurred_R, size); + vpImageFilter::gaussianBlur(I_G, I_blurred_G, size); + vpImageFilter::gaussianBlur(I_B, I_blurred_B, size); +#endif // Unsharp mask for (unsigned int cpt = 0; cpt < I.getSize(); cpt++) { +#if defined(VISP_HAVE_SIMDLIB) double val_R = (I.bitmap[cpt].R - weight * I_blurred.bitmap[cpt].R) / (1 - weight); double val_G = (I.bitmap[cpt].G - weight * I_blurred.bitmap[cpt].G) / (1 - weight); double val_B = (I.bitmap[cpt].B - weight * I_blurred.bitmap[cpt].B) / (1 - weight); - +#else + double val_R = (I.bitmap[cpt].R - weight * I_blurred_R.bitmap[cpt]) / (1 - weight); + double val_G = (I.bitmap[cpt].G - weight * I_blurred_G.bitmap[cpt]) / (1 - weight); + double val_B = (I.bitmap[cpt].B - weight * I_blurred_B.bitmap[cpt]) / (1 - weight); +#endif I.bitmap[cpt].R = vpMath::saturate(val_R); I.bitmap[cpt].G = vpMath::saturate(val_G); I.bitmap[cpt].B = vpMath::saturate(val_B); diff --git a/modules/imgproc/test/with-dataset/testImgproc.cpp b/modules/imgproc/test/with-dataset/testImgproc.cpp index 50b60a772d..ba009c23fe 100644 --- a/modules/imgproc/test/with-dataset/testImgproc.cpp +++ b/modules/imgproc/test/with-dataset/testImgproc.cpp @@ -70,7 +70,7 @@ SYNOPSIS\n\ %s [-i ] [-o ]\n\ [-h]\n \ ", - name); +name); fprintf(stdout, "\n\ OPTIONS: Default\n\ @@ -200,7 +200,8 @@ int main(int argc, const char **argv) try { // Create the dirname vpIoTools::makeDirectory(opath); - } catch (...) { + } + catch (...) { usage(argv[0], nullptr, ipath, opt_opath, username); std::cerr << std::endl << "ERROR:" << std::endl; std::cerr << " Cannot create " << opath << std::endl; @@ -215,8 +216,8 @@ int main(int argc, const char **argv) if (ipath != env_ipath) { std::cout << std::endl << "WARNING: " << std::endl; std::cout << " Since -i " - << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl - << " we skip the environment variable." << std::endl; + << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl + << " we skip the environment variable." << std::endl; } } @@ -225,9 +226,9 @@ int main(int argc, const char **argv) usage(argv[0], nullptr, ipath, opt_opath, username); std::cerr << std::endl << "ERROR:" << std::endl; std::cerr << " Use -i option or set VISP_INPUT_IMAGE_PATH " << std::endl - << " environment variable to specify the location of the " << std::endl - << " image path where test images are located." << std::endl - << std::endl; + << " environment variable to specify the location of the " << std::endl + << " image path where test images are located." << std::endl + << std::endl; return EXIT_FAILURE; } @@ -327,7 +328,7 @@ int main(int argc, const char **argv) filename = vpIoTools::createFilePath(opath, "Klimt_unsharp_mask.ppm"); vpImageIo::write(I_color_unsharp_mask, filename); - // CLAHE +// CLAHE vpImage I_color_clahe; t = vpTime::measureTimeMs(); vp::clahe(I_color, I_color_clahe, 50); @@ -419,7 +420,8 @@ int main(int argc, const char **argv) vpImageIo::write(I_clahe, filename); return EXIT_SUCCESS; - } catch (const vpException &e) { + } + catch (const vpException &e) { std::cerr << "Catch an exception: " << e.what() << std::endl; return EXIT_FAILURE; } diff --git a/modules/io/CMakeLists.txt b/modules/io/CMakeLists.txt index 0c0e900226..f93ece86d2 100644 --- a/modules/io/CMakeLists.txt +++ b/modules/io/CMakeLists.txt @@ -57,8 +57,10 @@ if(USE_PNG) add_definitions(${PNG_DEFINITIONS}) endif() -# stb_image is private -include_directories(${STBIMAGE_INCLUDE_DIRS}) +if(WITH_STBIMAGE) + # stb_image is private + include_directories(${STBIMAGE_INCLUDE_DIRS}) +endif() if(WITH_CATCH2) # catch2 is private @@ -70,10 +72,11 @@ if(USE_NLOHMANN_JSON) list(APPEND opt_incs ${_inc_dirs}) endif() -# simdlib is always enabled since it contains fallback code to plain C++ code -# Simd lib is private -include_directories(${SIMDLIB_INCLUDE_DIRS}) -list(APPEND opt_libs_private ${SIMDLIB_LIBRARIES}) +if(WITH_SIMDLIB) + # Simd lib is private + include_directories(${SIMDLIB_INCLUDE_DIRS}) + list(APPEND opt_libs_private ${SIMDLIB_LIBRARIES}) +endif() # TinyEXR lib is private include_directories(${TINYEXR_INCLUDE_DIRS}) diff --git a/modules/io/include/visp3/io/vpImageIo.h b/modules/io/include/visp3/io/vpImageIo.h index bf4eb446d0..49be1de4a8 100644 --- a/modules/io/include/visp3/io/vpImageIo.h +++ b/modules/io/include/visp3/io/vpImageIo.h @@ -98,7 +98,8 @@ int main() class VISP_EXPORT vpImageIo { private: - typedef enum { + typedef enum + { FORMAT_PGM, FORMAT_PPM, FORMAT_JPEG, @@ -118,10 +119,11 @@ class VISP_EXPORT vpImageIo public: //! Image IO backend for only jpeg and png formats image loading and saving - enum vpImageIoBackendType { + enum vpImageIoBackendType + { IO_DEFAULT_BACKEND, //!< Default backend - IO_SYSTEM_LIB_BACKEND, //!< Use system libraries like libpng or libjpeg - IO_OPENCV_BACKEND, //!< Use OpenCV + IO_SYSTEM_LIB_BACKEND, //!< Use system libraries like libpng or libjpeg-turbo + IO_OPENCV_BACKEND, //!< Use OpenCV imgcodecs module IO_SIMDLIB_BACKEND, //!< Use embedded simd library IO_STB_IMAGE_BACKEND //!< Use embedded stb_image library }; diff --git a/modules/io/src/image/private/vpImageIoBackend.h b/modules/io/src/image/private/vpImageIoBackend.h index dc823322dc..bff9f1dbf5 100644 --- a/modules/io/src/image/private/vpImageIoBackend.h +++ b/modules/io/src/image/private/vpImageIoBackend.h @@ -85,21 +85,29 @@ void writeOpenCV(const vpImage &I, const std::string &filename, int qual void writeOpenCV(const vpImage &I, const std::string &filename); void writeOpenCV(const vpImage &I, const std::string &filename); +#if defined(VISP_HAVE_SIMDLIB) // Simd lib void readSimdlib(vpImage &I, const std::string &filename); void readSimdlib(vpImage &I, const std::string &filename); -// TinyEXR lib -void readEXRTiny(vpImage &I, const std::string &filename); -void readEXRTiny(vpImage &I, const std::string &filename); - void writeJPEGSimdlib(const vpImage &I, const std::string &filename, int quality); void writeJPEGSimdlib(const vpImage &I, const std::string &filename, int quality); void writePNGSimdlib(const vpImage &I, const std::string &filename); void writePNGSimdlib(const vpImage &I, const std::string &filename); +#endif + +#if defined(VISP_HAVE_TINYEXR) +// TinyEXR lib +void readEXRTiny(vpImage &I, const std::string &filename); +void readEXRTiny(vpImage &I, const std::string &filename); -// stb lib +void writeEXRTiny(const vpImage &I, const std::string &filename); +void writeEXRTiny(const vpImage &I, const std::string &filename); +#endif + +#if defined(VISP_HAVE_STBIMAGE) +// stb_image lib void readStb(vpImage &I, const std::string &filename); void readStb(vpImage &I, const std::string &filename); @@ -108,9 +116,6 @@ void writeJPEGStb(const vpImage &I, const std::string &filename, int qua void writePNGStb(const vpImage &I, const std::string &filename); void writePNGStb(const vpImage &I, const std::string &filename); - -// TinyEXR lib -void writeEXRTiny(const vpImage &I, const std::string &filename); -void writeEXRTiny(const vpImage &I, const std::string &filename); +#endif #endif diff --git a/modules/io/src/image/private/vpImageIoSimd.cpp b/modules/io/src/image/private/vpImageIoSimd.cpp index 7c4238008c..3c63abaa85 100644 --- a/modules/io/src/image/private/vpImageIoSimd.cpp +++ b/modules/io/src/image/private/vpImageIoSimd.cpp @@ -36,6 +36,9 @@ \brief Simd backend for JPEG and PNG image I/O operations. */ +#include + +#if defined(VISP_HAVE_SIMDLIB) #include "vpImageIoBackend.h" #include @@ -88,3 +91,4 @@ void writePNGSimdlib(const vpImage &I, const std::string &filename) SimdImageSaveToFile((const uint8_t *)I.bitmap, I.getWidth() * 4, I.getWidth(), I.getHeight(), SimdPixelFormatRgba32, SimdImageFilePng, 90, filename.c_str()); } +#endif diff --git a/modules/io/src/image/private/vpImageIoStb.cpp b/modules/io/src/image/private/vpImageIoStb.cpp index 40d52ba5e8..253b3a986a 100644 --- a/modules/io/src/image/private/vpImageIoStb.cpp +++ b/modules/io/src/image/private/vpImageIoStb.cpp @@ -36,6 +36,10 @@ \brief stb backend for JPEG and PNG image I/O operations. */ +#include + +#if defined(VISP_HAVE_STBIMAGE) + #include "vpImageIoBackend.h" #if defined __SSE2__ || defined _M_X64 || (defined _M_IX86_FP && _M_IX86_FP >= 2) @@ -111,3 +115,5 @@ void writePNGStb(const vpImage &I, const std::string &filename) throw(vpImageException(vpImageException::ioError, "PNG write error: %s", filename.c_str())); } } + +#endif diff --git a/modules/io/src/image/private/vpImageIoTinyEXR.cpp b/modules/io/src/image/private/vpImageIoTinyEXR.cpp index be16de13f3..add5590864 100644 --- a/modules/io/src/image/private/vpImageIoTinyEXR.cpp +++ b/modules/io/src/image/private/vpImageIoTinyEXR.cpp @@ -36,6 +36,11 @@ \brief TinyEXR backend for EXR image I/O operations. */ + +#include + +#if defined(VISP_HAVE_STBIMAGE) && defined(VISP_HAVE_TINYEXR) + #include "vpImageIoBackend.h" #define TINYEXR_USE_MINIZ 0 @@ -313,3 +318,5 @@ void writeEXRTiny(const vpImage &I, const std::string &filename) free(header.requested_pixel_types); free(header.pixel_types); } + +#endif diff --git a/modules/io/src/image/vpImageIo.cpp b/modules/io/src/image/vpImageIo.cpp index 9980c2f3d2..ea0d7b6168 100644 --- a/modules/io/src/image/vpImageIo.cpp +++ b/modules/io/src/image/vpImageIo.cpp @@ -396,15 +396,13 @@ void vpImageIo::readJPEG(vpImage &I, const std::string &filename, { if (backend == IO_SYSTEM_LIB_BACKEND) { #if !defined(VISP_HAVE_JPEG) - std::string message = - "Libjpeg backend is not available to read file \"" + filename + "\": switch to stb_image backend"; + // Libjpeg backend is not available to read file \"" + filename + "\": switch to stb_image backend backend = IO_STB_IMAGE_BACKEND; #endif } else if (backend == IO_OPENCV_BACKEND) { #if !(defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGCODECS)) - std::string message = - "OpenCV backend is not available to read file \"" + filename + "\": switch to stb_image backend"; + // OpenCV backend is not available to read file \"" + filename + "\": switch to stb_image backend backend = IO_STB_IMAGE_BACKEND; #endif } @@ -413,26 +411,57 @@ void vpImageIo::readJPEG(vpImage &I, const std::string &filename, backend = IO_OPENCV_BACKEND; #elif defined(VISP_HAVE_JPEG) backend = IO_SYSTEM_LIB_BACKEND; -#else +#elif defined(VISP_HAVE_SIMDLIB) + backend = IO_SIMDLIB_BACKEND; +#elif defined(VISP_HAVE_STBIMAGE) backend = IO_STB_IMAGE_BACKEND; +#else + (void)I; + (void)filename; + std::string message = "Cannot read file \"" + filename + "\": no backend available"; + throw(vpImageException(vpImageException::ioError, message)); #endif } if (backend == IO_SYSTEM_LIB_BACKEND) { #if defined(VISP_HAVE_JPEG) readJPEGLibjpeg(I, filename); +#else + (void)I; + (void)filename; + std::string message = "Cannot read file \"" + filename + "\": jpeg library backend is not available"; + throw(vpImageException(vpImageException::ioError, message)); #endif } else if (backend == IO_OPENCV_BACKEND) { #if ((VISP_HAVE_OPENCV_VERSION >= 0x030000) && defined(HAVE_OPENCV_IMGCODECS)) || ((VISP_HAVE_OPENCV_VERSION < 0x030000) && defined(HAVE_OPENCV_HIGHGUI) && defined(HAVE_OPENCV_IMGPROC)) readOpenCV(I, filename); +#else + (void)I; + (void)filename; + std::string message = "Cannot read file \"" + filename + "\": OpenCV library backend is not available"; + throw(vpImageException(vpImageException::ioError, message)); #endif } else if (backend == IO_STB_IMAGE_BACKEND) { +#if defined(VISP_HAVE_STBIMAGE) readStb(I, filename); +#else + (void)I; + (void)filename; + std::string message = "Cannot read file \"" + filename + "\": stb_image backend is not available"; + throw(vpImageException(vpImageException::ioError, message)); +#endif } else if (backend == IO_SIMDLIB_BACKEND) { +#if defined(VISP_HAVE_SIMDLIB) readSimdlib(I, filename); +#else + (void)I; + (void)filename; + std::string message = "Cannot read file \"" + filename + "\": Simd library backend is not available"; + throw(vpImageException(vpImageException::ioError, message)); +#endif } } @@ -467,26 +496,57 @@ void vpImageIo::readJPEG(vpImage &I, const std::string &filename, int ba backend = IO_OPENCV_BACKEND; #elif defined(VISP_HAVE_JPEG) backend = IO_SYSTEM_LIB_BACKEND; -#else +#elif defined(VISP_HAVE_SIMDLIB) + backend = IO_SIMDLIB_BACKEND; +#elif defined(VISP_HAVE_STBIMAGE) backend = IO_STB_IMAGE_BACKEND; +#else + (void)I; + (void)filename; + std::string message = "Cannot read file \"" + filename + "\": no backend available"; + throw(vpImageException(vpImageException::ioError, message)); #endif } if (backend == IO_SYSTEM_LIB_BACKEND) { #if defined(VISP_HAVE_JPEG) readJPEGLibjpeg(I, filename); +#else + (void)I; + (void)filename; + std::string message = "Cannot read file \"" + filename + "\": jpeg library backend is not available"; + throw(vpImageException(vpImageException::ioError, message)); #endif } else if (backend == IO_OPENCV_BACKEND) { #if ((VISP_HAVE_OPENCV_VERSION >= 0x030000) && defined(HAVE_OPENCV_IMGCODECS)) || ((VISP_HAVE_OPENCV_VERSION < 0x030000) && defined(HAVE_OPENCV_HIGHGUI) && defined(HAVE_OPENCV_IMGPROC)) readOpenCV(I, filename); +#else + (void)I; + (void)filename; + std::string message = "Cannot read file \"" + filename + "\": OpenCV library backend is not available"; + throw(vpImageException(vpImageException::ioError, message)); #endif } else if (backend == IO_STB_IMAGE_BACKEND) { +#if defined(VISP_HAVE_STBIMAGE) readStb(I, filename); +#else + (void)I; + (void)filename; + std::string message = "Cannot read file \"" + filename + "\": stb_image backend is not available"; + throw(vpImageException(vpImageException::ioError, message)); +#endif } else if (backend == IO_SIMDLIB_BACKEND) { +#if defined(VISP_HAVE_SIMDLIB) readSimdlib(I, filename); +#else + (void)I; + (void)filename; + std::string message = "Cannot read file \"" + filename + "\": Simd library backend is not available"; + throw(vpImageException(vpImageException::ioError, message)); +#endif } } @@ -521,26 +581,57 @@ void vpImageIo::readPNG(vpImage &I, const std::string &filename, backend = IO_SYSTEM_LIB_BACKEND; #elif defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGCODECS) backend = IO_OPENCV_BACKEND; -#else +#elif defined(VISP_HAVE_SIMDLIB) + backend = IO_SIMDLIB_BACKEND; +#elif defined(VISP_HAVE_STBIMAGE) backend = IO_STB_IMAGE_BACKEND; +#else + (void)I; + (void)filename; + std::string message = "Cannot read file \"" + filename + "\": no backend available"; + throw(vpImageException(vpImageException::ioError, message)); #endif } if (backend == IO_SYSTEM_LIB_BACKEND) { #if defined(VISP_HAVE_PNG) readPNGLibpng(I, filename); +#else + (void)I; + (void)filename; + std::string message = "Cannot read file \"" + filename + "\": png library backend is not available"; + throw(vpImageException(vpImageException::ioError, message)); #endif } else if (backend == IO_OPENCV_BACKEND) { #if ((VISP_HAVE_OPENCV_VERSION >= 0x030000) && defined(HAVE_OPENCV_IMGCODECS)) || ((VISP_HAVE_OPENCV_VERSION < 0x030000) && defined(HAVE_OPENCV_HIGHGUI) && defined(HAVE_OPENCV_IMGPROC)) readOpenCV(I, filename); +#else + (void)I; + (void)filename; + std::string message = "Cannot read file \"" + filename + "\": OpenCV library backend is not available"; + throw(vpImageException(vpImageException::ioError, message)); #endif } else if (backend == IO_STB_IMAGE_BACKEND) { +#if defined(VISP_HAVE_STBIMAGE) readStb(I, filename); +#else + (void)I; + (void)filename; + std::string message = "Cannot read file \"" + filename + "\": stb_image backend is not available"; + throw(vpImageException(vpImageException::ioError, message)); +#endif } else if (backend == IO_SIMDLIB_BACKEND) { +#if defined(VISP_HAVE_SIMDLIB) readSimdlib(I, filename); +#else + (void)I; + (void)filename; + std::string message = "Cannot read file \"" + filename + "\": Simd library backend is not available"; + throw(vpImageException(vpImageException::ioError, message)); +#endif } } @@ -573,26 +664,57 @@ void vpImageIo::readPNG(vpImage &I, const std::string &filename, int bac else if (backend == IO_DEFAULT_BACKEND) { #if ((VISP_HAVE_OPENCV_VERSION >= 0x030000) && defined(HAVE_OPENCV_IMGCODECS)) || ((VISP_HAVE_OPENCV_VERSION < 0x030000) && defined(HAVE_OPENCV_HIGHGUI) && defined(HAVE_OPENCV_IMGPROC)) backend = IO_OPENCV_BACKEND; -#else +#elif defined(VISP_HAVE_SIMDLIB) + backend = IO_SIMDLIB_BACKEND; +#elif defined(VISP_HAVE_STBIMAGE) backend = IO_STB_IMAGE_BACKEND; +#else + (void)I; + (void)filename; + std::string message = "Cannot read file \"" + filename + "\": no backend available"; + throw(vpImageException(vpImageException::ioError, message)); #endif } if (backend == IO_SYSTEM_LIB_BACKEND) { #if defined(VISP_HAVE_PNG) readPNGLibpng(I, filename); +#else + (void)I; + (void)filename; + std::string message = "Cannot read file \"" + filename + "\": png library backend is not available"; + throw(vpImageException(vpImageException::ioError, message)); #endif } else if (backend == IO_OPENCV_BACKEND) { #if ((VISP_HAVE_OPENCV_VERSION >= 0x030000) && defined(HAVE_OPENCV_IMGCODECS)) || ((VISP_HAVE_OPENCV_VERSION < 0x030000) && defined(HAVE_OPENCV_HIGHGUI) && defined(HAVE_OPENCV_IMGPROC)) readOpenCV(I, filename); +#else + (void)I; + (void)filename; + std::string message = "Cannot read file \"" + filename + "\": OpenCV library backend is not available"; + throw(vpImageException(vpImageException::ioError, message)); #endif } else if (backend == IO_STB_IMAGE_BACKEND) { +#if defined(VISP_HAVE_STBIMAGE) readStb(I, filename); +#else + (void)I; + (void)filename; + std::string message = "Cannot read file \"" + filename + "\": stb_image backend is not available"; + throw(vpImageException(vpImageException::ioError, message)); +#endif } else if (backend == IO_SIMDLIB_BACKEND) { +#if defined(VISP_HAVE_SIMDLIB) readSimdlib(I, filename); +#else + (void)I; + (void)filename; + std::string message = "Cannot read file \"" + filename + "\": Simd library backend is not available"; + throw(vpImageException(vpImageException::ioError, message)); +#endif } } @@ -607,25 +729,41 @@ void vpImageIo::readPNG(vpImage &I, const std::string &filename, int bac void vpImageIo::readEXR(vpImage &I, const std::string &filename, int backend) { if (backend == IO_SYSTEM_LIB_BACKEND || backend == IO_SIMDLIB_BACKEND || backend == IO_STB_IMAGE_BACKEND) { - std::string message = - "This backend cannot read file \"" + filename + "\": switch to the default TinyEXR backend"; + // This backend cannot read file \"" + filename + "\": switch to the default TinyEXR backend backend = IO_DEFAULT_BACKEND; } else if (backend == IO_OPENCV_BACKEND) { #if !(defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGCODECS)) - std::string message = - "OpenCV backend is not available to read file \"" + filename + "\": switch to the default TinyEXR backend"; + // OpenCV backend is not available to read file \"" + filename + "\": switch to the default TinyEXR backend backend = IO_DEFAULT_BACKEND; #endif } + else if (backend == IO_DEFAULT_BACKEND) { +#if !defined(VISP_HAVE_TINYEXR) + // TinyEXR backend is not available to read file \"" + filename + "\": switch to the OpenCV backend + backend = IO_OPENCV_BACKEND; +#endif + } if (backend == IO_OPENCV_BACKEND) { #if ((VISP_HAVE_OPENCV_VERSION >= 0x030000) && defined(HAVE_OPENCV_IMGCODECS)) || ((VISP_HAVE_OPENCV_VERSION < 0x030000) && defined(HAVE_OPENCV_HIGHGUI) && defined(HAVE_OPENCV_IMGPROC)) readOpenCV(I, filename); +#else + (void)I; + (void)filename; + std::string message = "Cannot read file \"" + filename + "\": OpenCV backend is not available"; + throw(vpImageException(vpImageException::ioError, message)); #endif } else if (backend == IO_DEFAULT_BACKEND) { +#if defined(VISP_HAVE_TINYEXR) readEXRTiny(I, filename); +#else + (void)I; + (void)filename; + std::string message = "Cannot read file \"" + filename + "\": Default TinyEXR backend is not available"; + throw(vpImageException(vpImageException::ioError, message)); +#endif } } @@ -640,25 +778,41 @@ void vpImageIo::readEXR(vpImage &I, const std::string &filename, int back void vpImageIo::readEXR(vpImage &I, const std::string &filename, int backend) { if (backend == IO_SYSTEM_LIB_BACKEND || backend == IO_SIMDLIB_BACKEND || backend == IO_STB_IMAGE_BACKEND) { - std::string message = - "This backend cannot read file \"" + filename + "\": switch to the default TinyEXR backend"; + // This backend cannot read file \"" + filename + "\": switch to the default TinyEXR backend backend = IO_DEFAULT_BACKEND; } else if (backend == IO_OPENCV_BACKEND) { #if !(defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGCODECS)) - std::string message = - "OpenCV backend is not available to read file \"" + filename + "\": switch to the default TinyEXR backend"; + // OpenCV backend is not available to read file \"" + filename + "\": switch to the default TinyEXR backend backend = IO_DEFAULT_BACKEND; #endif } + else if (backend == IO_DEFAULT_BACKEND) { +#if !defined(VISP_HAVE_TINYEXR) + // TinyEXR backend is not available to read file \"" + filename + "\": switch to the OpenCV backend + backend = IO_OPENCV_BACKEND; +#endif + } if (backend == IO_OPENCV_BACKEND) { #if ((VISP_HAVE_OPENCV_VERSION >= 0x030000) && defined(HAVE_OPENCV_IMGCODECS)) || ((VISP_HAVE_OPENCV_VERSION < 0x030000) && defined(HAVE_OPENCV_HIGHGUI) && defined(HAVE_OPENCV_IMGPROC)) readOpenCV(I, filename); +#else + (void)I; + (void)filename; + std::string message = "Cannot read file \"" + filename + "\": OpenCV backend is not available"; + throw(vpImageException(vpImageException::ioError, message)); #endif } else if (backend == IO_DEFAULT_BACKEND) { +#if defined(VISP_HAVE_TINYEXR) readEXRTiny(I, filename); +#else + (void)I; + (void)filename; + std::string message = "Cannot read file \"" + filename + "\": TinyEXR backend is not available"; + throw(vpImageException(vpImageException::ioError, message)); +#endif } } @@ -677,14 +831,24 @@ void vpImageIo::writeJPEG(const vpImage &I, const std::string &fi { if (backend == IO_SYSTEM_LIB_BACKEND) { #if !defined(VISP_HAVE_JPEG) +#if defined(VISP_HAVE_SIMDLIB) std::string message = "Libjpeg backend is not available to save file \"" + filename + "\": switch to simd backend"; backend = IO_SIMDLIB_BACKEND; +#else + std::string message = "Libjpeg backend is not available to save file \"" + filename + "\": switch to stb_image backend"; + backend = IO_STB_IMAGE_BACKEND; +#endif #endif } else if (backend == IO_OPENCV_BACKEND) { #if !(defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGCODECS)) +#if defined(VISP_HAVE_SIMDLIB) std::string message = "OpenCV backend is not available to save file \"" + filename + "\": switch to simd backend"; backend = IO_SIMDLIB_BACKEND; +#else + std::string message = "OpenCV backend is not available to save file \"" + filename + "\": switch to stb_image backend"; + backend = IO_STB_IMAGE_BACKEND; +#endif #endif } else if (backend == IO_DEFAULT_BACKEND) { @@ -692,26 +856,61 @@ void vpImageIo::writeJPEG(const vpImage &I, const std::string &fi backend = IO_SYSTEM_LIB_BACKEND; #elif defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGCODECS) backend = IO_OPENCV_BACKEND; -#else +#elif defined(VISP_HAVE_SIMDLIB) backend = IO_SIMDLIB_BACKEND; +#elif defined(VISP_HAVE_STBIMAGE) + backend = IO_STB_IMAGE_BACKEND; +#else + (void)I; + (void)filename; + std::string message = "Cannot save file \"" + filename + "\": no backend available"; + throw(vpImageException(vpImageException::ioError, message)); #endif } if (backend == IO_SYSTEM_LIB_BACKEND) { #if defined(VISP_HAVE_JPEG) writeJPEGLibjpeg(I, filename, quality); +#else + (void)I; + (void)filename; + (void)quality; + std::string message = "Cannot save file \"" + filename + "\": jpeg backend is not available"; + throw(vpImageException(vpImageException::ioError, message)); #endif } else if (backend == IO_OPENCV_BACKEND) { #if ((VISP_HAVE_OPENCV_VERSION >= 0x030000) && defined(HAVE_OPENCV_IMGCODECS)) || ((VISP_HAVE_OPENCV_VERSION < 0x030000) && defined(HAVE_OPENCV_HIGHGUI) && defined(HAVE_OPENCV_IMGPROC)) writeOpenCV(I, filename, quality); +#else + (void)I; + (void)filename; + (void)quality; + std::string message = "Cannot save file \"" + filename + "\": OpenCV backend is not available"; + throw(vpImageException(vpImageException::ioError, message)); #endif } else if (backend == IO_SIMDLIB_BACKEND) { +#if defined(VISP_HAVE_SIMDLIB) writeJPEGSimdlib(I, filename, quality); +#else + (void)I; + (void)filename; + (void)quality; + std::string message = "Cannot save file \"" + filename + "\": Simd library backend is not available"; + throw(vpImageException(vpImageException::ioError, message)); +#endif } else if (backend == IO_STB_IMAGE_BACKEND) { +#if defined(VISP_HAVE_STBIMAGE) writeJPEGStb(I, filename, quality); +#else + (void)I; + (void)filename; + (void)quality; + std::string message = "Cannot save file \"" + filename + "\": stb_image backend is not available"; + throw(vpImageException(vpImageException::ioError, message)); +#endif } } @@ -730,14 +929,24 @@ void vpImageIo::writeJPEG(const vpImage &I, const std::string &filename, { if (backend == IO_SYSTEM_LIB_BACKEND) { #if !defined(VISP_HAVE_JPEG) +#if defined(VISP_HAVE_SIMDLIB) std::string message = "Libjpeg backend is not available to save file \"" + filename + "\": switch to simd backend"; backend = IO_SIMDLIB_BACKEND; +#else + std::string message = "Libjpeg backend is not available to save file \"" + filename + "\": switch to stb_image backend"; + backend = IO_STB_IMAGE_BACKEND; +#endif #endif } else if (backend == IO_OPENCV_BACKEND) { #if !(defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGCODECS)) +#if defined(VISP_HAVE_SIMDLIB) std::string message = "OpenCV backend is not available to save file \"" + filename + "\": switch to simd backend"; backend = IO_SIMDLIB_BACKEND; +#else + std::string message = "OpenCV backend is not available to save file \"" + filename + "\": switch to stb_image backend"; + backend = IO_STB_IMAGE_BACKEND; +#endif #endif } else if (backend == IO_DEFAULT_BACKEND) { @@ -745,26 +954,61 @@ void vpImageIo::writeJPEG(const vpImage &I, const std::string &filename, backend = IO_SYSTEM_LIB_BACKEND; #elif defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGCODECS) backend = IO_OPENCV_BACKEND; -#else +#elif defined(VISP_HAVE_SIMDLIB) backend = IO_SIMDLIB_BACKEND; +#elif defined(VISP_HAVE_STBIMAGE) + backend = IO_STB_IMAGE_BACKEND; +#else + (void)I; + (void)filename; + std::string message = "Cannot save file \"" + filename + "\": no backend available"; + throw(vpImageException(vpImageException::ioError, message)); #endif } if (backend == IO_SYSTEM_LIB_BACKEND) { #if defined(VISP_HAVE_JPEG) writeJPEGLibjpeg(I, filename, quality); +#else + (void)I; + (void)filename; + (void)quality; + std::string message = "Cannot save file \"" + filename + "\": jpeg library backend is not available"; + throw(vpImageException(vpImageException::ioError, message)); #endif } else if (backend == IO_OPENCV_BACKEND) { #if ((VISP_HAVE_OPENCV_VERSION >= 0x030000) && defined(HAVE_OPENCV_IMGCODECS)) || ((VISP_HAVE_OPENCV_VERSION < 0x030000) && defined(HAVE_OPENCV_HIGHGUI) && defined(HAVE_OPENCV_IMGPROC)) writeOpenCV(I, filename, quality); +#else + (void)I; + (void)filename; + (void)quality; + std::string message = "Cannot save file \"" + filename + "\": OpenCV library backend is not available"; + throw(vpImageException(vpImageException::ioError, message)); #endif } else if (backend == IO_SIMDLIB_BACKEND) { +#if defined(VISP_HAVE_SIMDLIB) writeJPEGSimdlib(I, filename, quality); +#else + (void)I; + (void)filename; + (void)quality; + std::string message = "Cannot save file \"" + filename + "\": Simd library backend is not available"; + throw(vpImageException(vpImageException::ioError, message)); +#endif } else if (backend == IO_STB_IMAGE_BACKEND) { +#if defined(VISP_HAVE_STBIMAGE) writeJPEGStb(I, filename, quality); +#else + (void)I; + (void)filename; + (void)quality; + std::string message = "Cannot save file \"" + filename + "\": stb_image backend is not available"; + throw(vpImageException(vpImageException::ioError, message)); +#endif } } @@ -782,38 +1026,79 @@ void vpImageIo::writePNG(const vpImage &I, const std::string &fil { if (backend == IO_SYSTEM_LIB_BACKEND) { #if !defined(VISP_HAVE_PNG) +#if defined(VISP_HAVE_SIMDLIB) std::string message = "Libpng backend is not available to save file \"" + filename + "\": switch to simd backend"; backend = IO_SIMDLIB_BACKEND; +#else + std::string message = "Libpng backend is not available to save file \"" + filename + "\": switch to stb_image backend"; + backend = IO_STB_IMAGE_BACKEND; +#endif #endif } else if (backend == IO_OPENCV_BACKEND) { #if !(defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGCODECS)) +#if defined(VISP_HAVE_SIMDLIB) std::string message = "OpenCV backend is not available to save file \"" + filename + "\": switch to simd backend"; backend = IO_SIMDLIB_BACKEND; +#else + std::string message = "OpenCV backend is not available to save file \"" + filename + "\": switch to stb_image backend"; + backend = IO_STB_IMAGE_BACKEND; +#endif #endif } else if (backend == IO_DEFAULT_BACKEND) { #if ((VISP_HAVE_OPENCV_VERSION >= 0x030000) && defined(HAVE_OPENCV_IMGCODECS)) || ((VISP_HAVE_OPENCV_VERSION < 0x030000) && defined(HAVE_OPENCV_HIGHGUI) && defined(HAVE_OPENCV_IMGPROC)) backend = IO_OPENCV_BACKEND; -#else +#elif defined(VISP_HAVE_SIMDLIB) backend = IO_SIMDLIB_BACKEND; +#elif defined(VISP_HAVE_STBIMAGE) + backend = IO_STB_IMAGE_BACKEND; +#else + (void)I; + (void)filename; + std::string message = "Cannot save file \"" + filename + "\": no backend available"; + throw(vpImageException(vpImageException::ioError, message)); #endif } if (backend == IO_OPENCV_BACKEND) { #if ((VISP_HAVE_OPENCV_VERSION >= 0x030000) && defined(HAVE_OPENCV_IMGCODECS)) || ((VISP_HAVE_OPENCV_VERSION < 0x030000) && defined(HAVE_OPENCV_HIGHGUI) && defined(HAVE_OPENCV_IMGPROC)) writeOpenCV(I, filename, 90); +#else + (void)I; + (void)filename; + std::string message = "Cannot save file \"" + filename + "\": OpenCV library backend is not available"; + throw(vpImageException(vpImageException::ioError, message)); #endif } else if (backend == IO_SIMDLIB_BACKEND) { +#if defined(VISP_HAVE_SIMDLIB) writePNGSimdlib(I, filename); +#else + (void)I; + (void)filename; + std::string message = "Cannot save file \"" + filename + "\": Simd library backend is not available"; + throw(vpImageException(vpImageException::ioError, message)); +#endif } else if (backend == IO_STB_IMAGE_BACKEND) { +#if defined(VISP_HAVE_STBIMAGE) writePNGStb(I, filename); +#else + (void)I; + (void)filename; + std::string message = "Cannot save file \"" + filename + "\": stb_image backend is not available"; + throw(vpImageException(vpImageException::ioError, message)); +#endif } else if (backend == IO_SYSTEM_LIB_BACKEND) { #if defined(VISP_HAVE_PNG) writePNGLibpng(I, filename); +#else + (void)I; + (void)filename; + std::string message = "Cannot save file \"" + filename + "\": png library backend is not available"; + throw(vpImageException(vpImageException::ioError, message)); #endif } } @@ -832,38 +1117,79 @@ void vpImageIo::writePNG(const vpImage &I, const std::string &filename, { if (backend == IO_SYSTEM_LIB_BACKEND) { #if !defined(VISP_HAVE_PNG) +#if defined(VISP_HAVE_SIMDLIB) std::string message = "Libpng backend is not available to save file \"" + filename + "\": switch to simd backend"; backend = IO_SIMDLIB_BACKEND; +#else + std::string message = "Libpng backend is not available to save file \"" + filename + "\": switch to stb_image backend"; + backend = IO_STB_IMAGE_BACKEND; +#endif #endif } else if (backend == IO_OPENCV_BACKEND) { #if !(defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGCODECS)) +#if defined(VISP_HAVE_SIMDLIB) std::string message = "OpenCV backend is not available to save file \"" + filename + "\": switch to simd backend"; backend = IO_SIMDLIB_BACKEND; +#else + std::string message = "OpenCV backend is not available to save file \"" + filename + "\": switch to stb_image backend"; + backend = IO_STB_IMAGE_BACKEND; +#endif #endif } else if (backend == IO_DEFAULT_BACKEND) { #if ((VISP_HAVE_OPENCV_VERSION >= 0x030000) && defined(HAVE_OPENCV_IMGCODECS)) || ((VISP_HAVE_OPENCV_VERSION < 0x030000) && defined(HAVE_OPENCV_HIGHGUI) && defined(HAVE_OPENCV_IMGPROC)) backend = IO_OPENCV_BACKEND; -#else +#elif defined(VISP_HAVE_SIMDLIB) backend = IO_SIMDLIB_BACKEND; +#elif defined(VISP_HAVE_STBIMAGE) + backend = IO_STB_IMAGE_BACKEND; +#else + (void)I; + (void)filename; + std::string message = "Cannot save file \"" + filename + "\": no backend available"; + throw(vpImageException(vpImageException::ioError, message)); #endif } if (backend == IO_OPENCV_BACKEND) { #if ((VISP_HAVE_OPENCV_VERSION >= 0x030000) && defined(HAVE_OPENCV_IMGCODECS)) || ((VISP_HAVE_OPENCV_VERSION < 0x030000) && defined(HAVE_OPENCV_HIGHGUI) && defined(HAVE_OPENCV_IMGPROC)) writeOpenCV(I, filename, 90); +#else + (void)I; + (void)filename; + std::string message = "Cannot save file \"" + filename + "\": OpenCV backend is not available"; + throw(vpImageException(vpImageException::ioError, message)); #endif } else if (backend == IO_SIMDLIB_BACKEND) { +#if defined(VISP_HAVE_SIMDLIB) writePNGSimdlib(I, filename); +#else + (void)I; + (void)filename; + std::string message = "Cannot save file \"" + filename + "\": Simd library backend is not available"; + throw(vpImageException(vpImageException::ioError, message)); +#endif } else if (backend == IO_STB_IMAGE_BACKEND) { +#if defined(VISP_HAVE_STBIMAGE) writePNGStb(I, filename); +#else + (void)I; + (void)filename; + std::string message = "Cannot save file \"" + filename + "\": stb_image backend is not available"; + throw(vpImageException(vpImageException::ioError, message)); +#endif } else if (backend == IO_SYSTEM_LIB_BACKEND) { #if defined(VISP_HAVE_PNG) writePNGLibpng(I, filename); +#else + (void)I; + (void)filename; + std::string message = "Cannot save file \"" + filename + "\": libpng backend is not available"; + throw(vpImageException(vpImageException::ioError, message)); #endif } } @@ -879,26 +1205,42 @@ void vpImageIo::writePNG(const vpImage &I, const std::string &filename, void vpImageIo::writeEXR(const vpImage &I, const std::string &filename, int backend) { if (backend == IO_SYSTEM_LIB_BACKEND || backend == IO_SIMDLIB_BACKEND || backend == IO_STB_IMAGE_BACKEND) { - std::string message = - "This backend cannot save file \"" + filename + "\": switch to the default TinyEXR backend"; + // This backend cannot save file \"" + filename + "\": switch to the default TinyEXR backend backend = IO_DEFAULT_BACKEND; } else if (backend == IO_OPENCV_BACKEND) { #if !(defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGCODECS)) (void)I; - std::string message = - "OpenCV backend is not available to save file \"" + filename + "\": switch to the default TinyEXR backend"; + // OpenCV backend is not available to save file \"" + filename + "\": switch to the default TinyEXR backend backend = IO_DEFAULT_BACKEND; #endif } + else if (backend == IO_DEFAULT_BACKEND) { +#if !defined(VISP_HAVE_TINYEXR) + // TinyEXR backend is not available to save file \"" + filename + "\": switch to the OpenCV backend + backend = IO_OPENCV_BACKEND; +#endif + } if (backend == IO_OPENCV_BACKEND) { #if ((VISP_HAVE_OPENCV_VERSION >= 0x030000) && defined(HAVE_OPENCV_IMGCODECS)) || ((VISP_HAVE_OPENCV_VERSION < 0x030000) && defined(HAVE_OPENCV_HIGHGUI) && defined(HAVE_OPENCV_IMGPROC)) writeOpenCV(I, filename); +#else + (void)I; + (void)filename; + std::string message = "Cannot save file \"" + filename + "\": OpenCV backend is not available"; + throw(vpImageException(vpImageException::ioError, message)); #endif } else if (backend == IO_DEFAULT_BACKEND) { +#if defined(VISP_HAVE_TINYEXR) writeEXRTiny(I, filename); +#else + (void)I; + (void)filename; + std::string message = "Cannot save file \"" + filename + "\": TinyEXR backend is not available"; + throw(vpImageException(vpImageException::ioError, message)); +#endif } } @@ -913,26 +1255,41 @@ void vpImageIo::writeEXR(const vpImage &I, const std::string &filename, i void vpImageIo::writeEXR(const vpImage &I, const std::string &filename, int backend) { if (backend == IO_SYSTEM_LIB_BACKEND || backend == IO_SIMDLIB_BACKEND || backend == IO_STB_IMAGE_BACKEND) { - std::string message = - "This backend cannot save file \"" + filename + "\": switch to the default TinyEXR backend"; + // This backend cannot save file \"" + filename + "\": switch to the default TinyEXR backend backend = IO_DEFAULT_BACKEND; } else if (backend == IO_OPENCV_BACKEND) { #if !(defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGCODECS)) - (void)I; - std::string message = - "OpenCV backend is not available to save file \"" + filename + "\": switch to the default TinyEXR backend"; + // OpenCV backend is not available to save file \"" + filename + "\": switch to the default TinyEXR backend backend = IO_DEFAULT_BACKEND; #endif } + else if (backend == IO_DEFAULT_BACKEND) { +#if !defined(VISP_HAVE_TINYEXR) + // TinyEXR backend is not available to save file \"" + filename + "\": switch to the OpenCV backend + backend = IO_OPENCV_BACKEND; +#endif + } if (backend == IO_OPENCV_BACKEND) { #if ((VISP_HAVE_OPENCV_VERSION >= 0x030000) && defined(HAVE_OPENCV_IMGCODECS)) || ((VISP_HAVE_OPENCV_VERSION < 0x030000) && defined(HAVE_OPENCV_HIGHGUI) && defined(HAVE_OPENCV_IMGPROC)) writeOpenCV(I, filename); +#else + (void)I; + (void)filename; + std::string message = "Cannot save file \"" + filename + "\": OpenCV backend is not available"; + throw(vpImageException(vpImageException::ioError, message)); #endif } else if (backend == IO_DEFAULT_BACKEND) { +#if defined(VISP_HAVE_TINYEXR) writeEXRTiny(I, filename); +#else + (void)I; + (void)filename; + std::string message = "Cannot save file \"" + filename + "\": TinyEXR backend is not available"; + throw(vpImageException(vpImageException::ioError, message)); +#endif } } diff --git a/modules/io/test/image-with-dataset/perfImageLoadSave.cpp b/modules/io/test/image-with-dataset/perfImageLoadSave.cpp index 6da75491db..93d5d3dfd1 100644 --- a/modules/io/test/image-with-dataset/perfImageLoadSave.cpp +++ b/modules/io/test/image-with-dataset/perfImageLoadSave.cpp @@ -45,23 +45,26 @@ #include static std::string ipath = vpIoTools::getViSPImagesDataPath(); -static std::vector paths{ +static std::vector paths { ipath + "/Solvay/Solvay_conference_1927_Version2_640x440", ipath + "/Solvay/Solvay_conference_1927_Version2_1024x705", ipath + "/Solvay/Solvay_conference_1927_Version2_1280x881", ipath + "/Solvay/Solvay_conference_1927_Version2_2126x1463", }; -static std::vector names{"Solvay (640x440)", "Solvay (1024x705)", "Solvay (1280x881)", - "Solvay (2126x1463)"}; +static std::vector names { "Solvay (640x440)", "Solvay (1024x705)", "Solvay (1280x881)", + "Solvay (2126x1463)" }; static std::vector backends { #if defined(VISP_HAVE_JPEG) && defined(VISP_HAVE_PNG) vpImageIo::IO_SYSTEM_LIB_BACKEND, #endif #if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGCODECS) - vpImageIo::IO_OPENCV_BACKEND, + vpImageIo::IO_OPENCV_BACKEND, #endif - vpImageIo::IO_SIMDLIB_BACKEND, vpImageIo::IO_STB_IMAGE_BACKEND +#if defined(VISP_HAVE_SIMDLIB) + vpImageIo::IO_SIMDLIB_BACKEND, +#endif + vpImageIo::IO_STB_IMAGE_BACKEND }; static std::vector backendNamesJpeg { @@ -161,7 +164,7 @@ TEST_CASE("Benchmark grayscale JPEG image saving", "[benchmark]") { std::string tmp_dir = vpIoTools::makeTempDirectory(vpIoTools::getTempPath()); std::string directory_filename_tmp = - tmp_dir + "/vpIoTools_perfImageLoadSave_" + vpTime::getDateTime("%Y-%m-%d_%H.%M.%S"); + tmp_dir + "/vpIoTools_perfImageLoadSave_" + vpTime::getDateTime("%Y-%m-%d_%H.%M.%S"); vpIoTools::makeDirectory(directory_filename_tmp); REQUIRE(vpIoTools::checkDirectory(directory_filename_tmp)); @@ -188,7 +191,7 @@ TEST_CASE("Benchmark RGBA JPEG image saving", "[benchmark]") { std::string tmp_dir = vpIoTools::makeTempDirectory(vpIoTools::getTempPath()); std::string directory_filename_tmp = - tmp_dir + "/vpIoTools_perfImageLoadSave_" + vpTime::getDateTime("%Y-%m-%d_%H.%M.%S"); + tmp_dir + "/vpIoTools_perfImageLoadSave_" + vpTime::getDateTime("%Y-%m-%d_%H.%M.%S"); vpIoTools::makeDirectory(directory_filename_tmp); REQUIRE(vpIoTools::checkDirectory(directory_filename_tmp)); @@ -215,7 +218,7 @@ TEST_CASE("Benchmark grayscale PNG image saving", "[benchmark]") { std::string tmp_dir = vpIoTools::makeTempDirectory(vpIoTools::getTempPath()); std::string directory_filename_tmp = - tmp_dir + "/vpIoTools_perfImageLoadSave_" + vpTime::getDateTime("%Y-%m-%d_%H.%M.%S"); + tmp_dir + "/vpIoTools_perfImageLoadSave_" + vpTime::getDateTime("%Y-%m-%d_%H.%M.%S"); vpIoTools::makeDirectory(directory_filename_tmp); REQUIRE(vpIoTools::checkDirectory(directory_filename_tmp)); @@ -242,7 +245,7 @@ TEST_CASE("Benchmark RGBA PNG image saving", "[benchmark]") { std::string tmp_dir = vpIoTools::makeTempDirectory(vpIoTools::getTempPath()); std::string directory_filename_tmp = - tmp_dir + "/vpIoTools_perfImageLoadSave_" + vpTime::getDateTime("%Y-%m-%d_%H.%M.%S"); + tmp_dir + "/vpIoTools_perfImageLoadSave_" + vpTime::getDateTime("%Y-%m-%d_%H.%M.%S"); vpIoTools::makeDirectory(directory_filename_tmp); REQUIRE(vpIoTools::checkDirectory(directory_filename_tmp)); @@ -273,12 +276,12 @@ int main(int argc, char *argv[]) // Build a new parser on top of Catch's using namespace Catch::clara; auto cli = session.cli() // Get Catch's composite command line parser - | Opt(runBenchmark) // bind variable to a new option, with a hint string - ["--benchmark"] // the option names it will respond to - ("Run benchmark") | - Opt(nThreads, "nThreads")["--nThreads"]("Number of threads"); + | Opt(runBenchmark) // bind variable to a new option, with a hint string + ["--benchmark"] // the option names it will respond to + ("Run benchmark") | + Opt(nThreads, "nThreads")["--nThreads"]("Number of threads"); - // Now pass the new composite back to Catch so it uses that +// Now pass the new composite back to Catch so it uses that session.cli(cli); // Let Catch (using Clara) parse the command line @@ -286,7 +289,7 @@ int main(int argc, char *argv[]) if (runBenchmark) { std::cout << "nThreads: " << nThreads << " / available threads: " << std::thread::hardware_concurrency() - << std::endl; + << std::endl; int numFailed = session.run(); diff --git a/modules/io/test/image-with-dataset/testImageLoadSave.cpp b/modules/io/test/image-with-dataset/testImageLoadSave.cpp index 2b856f01eb..50e4def6ee 100644 --- a/modules/io/test/image-with-dataset/testImageLoadSave.cpp +++ b/modules/io/test/image-with-dataset/testImageLoadSave.cpp @@ -56,9 +56,14 @@ static const std::vector backends vpImageIo::IO_SYSTEM_LIB_BACKEND, #endif #if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGCODECS) - vpImageIo::IO_OPENCV_BACKEND, + vpImageIo::IO_OPENCV_BACKEND, +#endif +#if defined VISP_HAVE_SIMDLIB + vpImageIo::IO_SIMDLIB_BACKEND, +#endif +#if defined VISP_HAVE_STBIMAGE + vpImageIo::IO_STB_IMAGE_BACKEND #endif - vpImageIo::IO_SIMDLIB_BACKEND, vpImageIo::IO_STB_IMAGE_BACKEND }; static const std::vector backendNamesJpeg { @@ -209,7 +214,7 @@ TEST_CASE("Test grayscale JPEG image saving", "[image_I/O]") { std::string tmp_dir = vpIoTools::makeTempDirectory(vpIoTools::getTempPath()); std::string directory_filename_tmp = - tmp_dir + "/vpIoTools_perfImageLoadSave_" + vpTime::getDateTime("%Y-%m-%d_%H.%M.%S"); + tmp_dir + "/vpIoTools_perfImageLoadSave_" + vpTime::getDateTime("%Y-%m-%d_%H.%M.%S"); vpIoTools::makeDirectory(directory_filename_tmp); REQUIRE(vpIoTools::checkDirectory(directory_filename_tmp)); @@ -235,7 +240,7 @@ TEST_CASE("Test RGBA JPEG image saving", "[image_I/O]") { std::string tmp_dir = vpIoTools::makeTempDirectory(vpIoTools::getTempPath()); std::string directory_filename_tmp = - tmp_dir + "/vpIoTools_perfImageLoadSave_" + vpTime::getDateTime("%Y-%m-%d_%H.%M.%S"); + tmp_dir + "/vpIoTools_perfImageLoadSave_" + vpTime::getDateTime("%Y-%m-%d_%H.%M.%S"); vpIoTools::makeDirectory(directory_filename_tmp); REQUIRE(vpIoTools::checkDirectory(directory_filename_tmp)); @@ -261,7 +266,7 @@ TEST_CASE("Test grayscale PNG image saving", "[image_I/O]") { std::string tmp_dir = vpIoTools::makeTempDirectory(vpIoTools::getTempPath()); std::string directory_filename_tmp = - tmp_dir + "/vpIoTools_perfImageLoadSave_" + vpTime::getDateTime("%Y-%m-%d_%H.%M.%S"); + tmp_dir + "/vpIoTools_perfImageLoadSave_" + vpTime::getDateTime("%Y-%m-%d_%H.%M.%S"); vpIoTools::makeDirectory(directory_filename_tmp); REQUIRE(vpIoTools::checkDirectory(directory_filename_tmp)); @@ -287,7 +292,7 @@ TEST_CASE("Test RGBA PNG image saving", "[image_I/O]") { std::string tmp_dir = vpIoTools::makeTempDirectory(vpIoTools::getTempPath()); std::string directory_filename_tmp = - tmp_dir + "/vpIoTools_perfImageLoadSave_" + vpTime::getDateTime("%Y-%m-%d_%H.%M.%S"); + tmp_dir + "/vpIoTools_perfImageLoadSave_" + vpTime::getDateTime("%Y-%m-%d_%H.%M.%S"); vpIoTools::makeDirectory(directory_filename_tmp); REQUIRE(vpIoTools::checkDirectory(directory_filename_tmp)); diff --git a/modules/robot/src/real-robot/afma6/vpAfma6.cpp b/modules/robot/src/real-robot/afma6/vpAfma6.cpp index 600ee8e7cc..1462909d95 100644 --- a/modules/robot/src/real-robot/afma6/vpAfma6.cpp +++ b/modules/robot/src/real-robot/afma6/vpAfma6.cpp @@ -58,45 +58,45 @@ /* --- STATIC ------------------------------------------------------------ */ /* ---------------------------------------------------------------------- */ -static const char *opt_Afma6[] = {"JOINT_MAX", "JOINT_MIN", "LONG_56", "COUPL_56", - "CAMERA", "eMc_ROT_XYZ", "eMc_TRANS_XYZ", nullptr}; +static const char *opt_Afma6[] = { "JOINT_MAX", "JOINT_MIN", "LONG_56", "COUPL_56", + "CAMERA", "eMc_ROT_XYZ", "eMc_TRANS_XYZ", nullptr }; #ifdef VISP_HAVE_AFMA6_DATA const std::string vpAfma6::CONST_AFMA6_FILENAME = - std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_Afma6.cnf"); +std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_Afma6.cnf"); const std::string vpAfma6::CONST_EMC_CCMOP_WITHOUT_DISTORTION_FILENAME = - std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_ccmop_without_distortion_Afma6.cnf"); +std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_ccmop_without_distortion_Afma6.cnf"); const std::string vpAfma6::CONST_EMC_CCMOP_WITH_DISTORTION_FILENAME = - std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_ccmop_with_distortion_Afma6.cnf"); +std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_ccmop_with_distortion_Afma6.cnf"); const std::string vpAfma6::CONST_EMC_GRIPPER_WITHOUT_DISTORTION_FILENAME = - std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_gripper_without_distortion_Afma6.cnf"); +std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_gripper_without_distortion_Afma6.cnf"); const std::string vpAfma6::CONST_EMC_GRIPPER_WITH_DISTORTION_FILENAME = - std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_gripper_with_distortion_Afma6.cnf"); +std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_gripper_with_distortion_Afma6.cnf"); const std::string vpAfma6::CONST_EMC_VACUUM_WITHOUT_DISTORTION_FILENAME = - std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_vacuum_without_distortion_Afma6.cnf"); +std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_vacuum_without_distortion_Afma6.cnf"); const std::string vpAfma6::CONST_EMC_VACUUM_WITH_DISTORTION_FILENAME = - std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_vacuum_with_distortion_Afma6.cnf"); +std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_vacuum_with_distortion_Afma6.cnf"); const std::string vpAfma6::CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME = - std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_generic_without_distortion_Afma6.cnf"); +std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_generic_without_distortion_Afma6.cnf"); const std::string vpAfma6::CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME = - std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_generic_with_distortion_Afma6.cnf"); +std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_generic_with_distortion_Afma6.cnf"); const std::string vpAfma6::CONST_EMC_INTEL_D435_WITHOUT_DISTORTION_FILENAME = - std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_Intel_D435_without_distortion_Afma6.cnf"); +std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_Intel_D435_without_distortion_Afma6.cnf"); const std::string vpAfma6::CONST_EMC_INTEL_D435_WITH_DISTORTION_FILENAME = - std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_Intel_D435_with_distortion_Afma6.cnf"); +std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_Intel_D435_with_distortion_Afma6.cnf"); const std::string vpAfma6::CONST_CAMERA_AFMA6_FILENAME = - std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_camera_Afma6.xml"); +std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_camera_Afma6.xml"); #endif // VISP_HAVE_AFMA6_DATA @@ -117,7 +117,7 @@ const unsigned int vpAfma6::njoint = 6; */ vpAfma6::vpAfma6() : _coupl_56(0), _long_56(0), _etc(), _erc(), _eMc(), tool_current(vpAfma6::defaultTool), - projModel(vpCameraParameters::perspectiveProjWithoutDistortion) + projModel(vpCameraParameters::perspectiveProjWithoutDistortion) { // Set the default parameters in case of the config files are not available. @@ -641,7 +641,8 @@ int vpAfma6::getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q q_[1][5] += vpMath::rad(10); q_[1][3] -= vpMath::rad(10); } - } else if (fMe[2][2] <= -.99999) { + } + else if (fMe[2][2] <= -.99999) { vpTRACE("singularity\n"); q_[0][4] = q_[1][4] = -M_PI / 2; t = atan2(fMe[1][1], fMe[1][0]); @@ -657,7 +658,8 @@ int vpAfma6::getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q q_[1][5] += vpMath::rad(10); q_[1][3] += vpMath::rad(10); } - } else { + } + else { q_[0][3] = atan2(-fMe[0][2], fMe[1][2]); if (q_[0][3] >= 0.0) q_[1][3] = q_[0][3] - M_PI; @@ -694,10 +696,10 @@ int vpAfma6::getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q if (verbose) { if (i < 3) std::cout << "Joint " << i << " not in limits: " << this->_joint_min[i] << " < " << q_[j][i] << " < " - << this->_joint_max[i] << std::endl; + << this->_joint_max[i] << std::endl; else std::cout << "Joint " << i << " not in limits: " << vpMath::deg(this->_joint_min[i]) << " < " - << vpMath::deg(q_[j][i]) << " < " << vpMath::deg(this->_joint_max[i]) << std::endl; + << vpMath::deg(q_[j][i]) << " < " << vpMath::deg(this->_joint_max[i]) << std::endl; } ok[j] = 0; } @@ -708,17 +710,20 @@ int vpAfma6::getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q std::cout << "No solution..." << std::endl; nbsol = 0; return nbsol; - } else if (ok[1] == 1) { + } + else if (ok[1] == 1) { for (unsigned int i = 0; i < 6; i++) cord[i] = q_[1][i]; nbsol = 1; } - } else { + } + else { if (ok[1] == 0) { for (unsigned int i = 0; i < 6; i++) cord[i] = q_[0][i]; nbsol = 1; - } else { + } + else { nbsol = 2; // vpTRACE("2 solutions\n"); for (int j = 0; j < 2; j++) { @@ -733,7 +738,8 @@ int vpAfma6::getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q else for (unsigned int i = 0; i < 6; i++) cord[i] = q_[1][i]; - } else { + } + else { if (d[0] <= d[1]) for (unsigned int i = 0; i < 6; i++) cord[i] = q_[1][i]; @@ -1125,12 +1131,12 @@ void vpAfma6::parseConfigFile(const std::string &filename) switch (code) { case 0: ss >> this->_joint_max[0] >> this->_joint_max[1] >> this->_joint_max[2] >> this->_joint_max[3] >> - this->_joint_max[4] >> this->_joint_max[5]; + this->_joint_max[4] >> this->_joint_max[5]; break; case 1: ss >> this->_joint_min[0] >> this->_joint_min[1] >> this->_joint_min[2] >> this->_joint_min[3] >> - this->_joint_min[4] >> this->_joint_min[5]; + this->_joint_min[4] >> this->_joint_min[5]; break; case 2: @@ -1255,12 +1261,12 @@ parameters are not found. void vpAfma6::getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height) const { -#if defined(VISP_HAVE_AFMA6_DATA) +#if defined(VISP_HAVE_AFMA6_DATA) && defined(VISP_HAVE_PUGIXML) vpXmlParserCamera parser; switch (getToolType()) { case vpAfma6::TOOL_CCMOP: { std::cout << "Get camera parameters for camera \"" << vpAfma6::CONST_CCMOP_CAMERA_NAME << "\"" << std::endl - << "from the XML file: \"" << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\"" << std::endl; + << "from the XML file: \"" << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\"" << std::endl; if (parser.parse(cam, vpAfma6::CONST_CAMERA_AFMA6_FILENAME, vpAfma6::CONST_CCMOP_CAMERA_NAME, projModel, image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) { throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters."); @@ -1269,7 +1275,7 @@ void vpAfma6::getCameraParameters(vpCameraParameters &cam, const unsigned int &i } case vpAfma6::TOOL_GRIPPER: { std::cout << "Get camera parameters for camera \"" << vpAfma6::CONST_GRIPPER_CAMERA_NAME << "\"" << std::endl - << "from the XML file: \"" << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\"" << std::endl; + << "from the XML file: \"" << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\"" << std::endl; if (parser.parse(cam, vpAfma6::CONST_CAMERA_AFMA6_FILENAME, vpAfma6::CONST_GRIPPER_CAMERA_NAME, projModel, image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) { throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters."); @@ -1278,7 +1284,7 @@ void vpAfma6::getCameraParameters(vpCameraParameters &cam, const unsigned int &i } case vpAfma6::TOOL_VACUUM: { std::cout << "Get camera parameters for camera \"" << vpAfma6::CONST_VACUUM_CAMERA_NAME << "\"" << std::endl - << "from the XML file: \"" << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\"" << std::endl; + << "from the XML file: \"" << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\"" << std::endl; if (parser.parse(cam, vpAfma6::CONST_CAMERA_AFMA6_FILENAME, vpAfma6::CONST_VACUUM_CAMERA_NAME, projModel, image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) { throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters."); @@ -1287,7 +1293,7 @@ void vpAfma6::getCameraParameters(vpCameraParameters &cam, const unsigned int &i } case vpAfma6::TOOL_INTEL_D435_CAMERA: { std::cout << "Get camera parameters for camera \"" << vpAfma6::CONST_INTEL_D435_CAMERA_NAME << "\"" << std::endl - << "from the XML file: \"" << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\"" << std::endl; + << "from the XML file: \"" << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\"" << std::endl; if (parser.parse(cam, vpAfma6::CONST_CAMERA_AFMA6_FILENAME, vpAfma6::CONST_INTEL_D435_CAMERA_NAME, projModel, image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) { throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters."); @@ -1296,7 +1302,7 @@ void vpAfma6::getCameraParameters(vpCameraParameters &cam, const unsigned int &i } case vpAfma6::TOOL_GENERIC_CAMERA: { std::cout << "Get camera parameters for camera \"" << vpAfma6::CONST_GENERIC_CAMERA_NAME << "\"" << std::endl - << "from the XML file: \"" << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\"" << std::endl; + << "from the XML file: \"" << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\"" << std::endl; if (parser.parse(cam, vpAfma6::CONST_CAMERA_AFMA6_FILENAME, vpAfma6::CONST_GENERIC_CAMERA_NAME, projModel, image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) { throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters."); @@ -1323,7 +1329,7 @@ void vpAfma6::getCameraParameters(vpCameraParameters &cam, const unsigned int &i // Set default intrinsic camera parameters for 640x480 images if (image_width == 640 && image_height == 480) { std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_CCMOP_CAMERA_NAME << "\"" - << std::endl; + << std::endl; switch (this->projModel) { case vpCameraParameters::perspectiveProjWithoutDistortion: cam.initPersProjWithoutDistortion(1108.0, 1110.0, 314.5, 243.2); @@ -1336,7 +1342,8 @@ void vpAfma6::getCameraParameters(vpCameraParameters &cam, const unsigned int &i "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet."); break; } - } else { + } + else { vpTRACE("Cannot get default intrinsic camera parameters for this image " "resolution"); throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters."); @@ -1347,7 +1354,7 @@ void vpAfma6::getCameraParameters(vpCameraParameters &cam, const unsigned int &i // Set default intrinsic camera parameters for 640x480 images if (image_width == 640 && image_height == 480) { std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_GRIPPER_CAMERA_NAME << "\"" - << std::endl; + << std::endl; switch (this->projModel) { case vpCameraParameters::perspectiveProjWithoutDistortion: cam.initPersProjWithoutDistortion(850.9, 853.0, 311.1, 243.6); @@ -1360,7 +1367,8 @@ void vpAfma6::getCameraParameters(vpCameraParameters &cam, const unsigned int &i "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet."); break; } - } else { + } + else { vpTRACE("Cannot get default intrinsic camera parameters for this image " "resolution"); throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters."); @@ -1371,7 +1379,7 @@ void vpAfma6::getCameraParameters(vpCameraParameters &cam, const unsigned int &i // Set default intrinsic camera parameters for 640x480 images if (image_width == 640 && image_height == 480) { std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_VACUUM_CAMERA_NAME << "\"" - << std::endl; + << std::endl; switch (this->projModel) { case vpCameraParameters::perspectiveProjWithoutDistortion: cam.initPersProjWithoutDistortion(853.5, 856.0, 307.8, 236.8); @@ -1384,7 +1392,8 @@ void vpAfma6::getCameraParameters(vpCameraParameters &cam, const unsigned int &i "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet."); break; } - } else { + } + else { vpTRACE("Cannot get default intrinsic camera parameters for this image " "resolution"); throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters."); @@ -1395,7 +1404,7 @@ void vpAfma6::getCameraParameters(vpCameraParameters &cam, const unsigned int &i // Set default intrinsic camera parameters for 640x480 images if (image_width == 640 && image_height == 480) { std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_INTEL_D435_CAMERA_NAME << "\"" - << std::endl; + << std::endl; switch (this->projModel) { case vpCameraParameters::perspectiveProjWithoutDistortion: cam.initPersProjWithoutDistortion(605.4, 605.6, 328.6, 241.0); @@ -1408,7 +1417,8 @@ void vpAfma6::getCameraParameters(vpCameraParameters &cam, const unsigned int &i "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet."); break; } - } else { + } + else { vpTRACE("Cannot get default intrinsic camera parameters for this image " "resolution"); throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters."); @@ -1419,7 +1429,7 @@ void vpAfma6::getCameraParameters(vpCameraParameters &cam, const unsigned int &i // Set default intrinsic camera parameters for 640x480 images if (image_width == 640 && image_height == 480) { std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_GENERIC_CAMERA_NAME << "\"" - << std::endl; + << std::endl; switch (this->projModel) { case vpCameraParameters::perspectiveProjWithoutDistortion: cam.initPersProjWithoutDistortion(853.5, 856.0, 307.8, 236.8); @@ -1432,7 +1442,8 @@ void vpAfma6::getCameraParameters(vpCameraParameters &cam, const unsigned int &i "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet."); break; } - } else { + } + else { vpTRACE("Cannot get default intrinsic camera parameters for this image " "resolution"); throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters."); @@ -1557,25 +1568,25 @@ VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpAfma6 &afma6) vpRxyzVector rxyz(eRc); os << "Joint Max:" << std::endl - << "\t" << afma6._joint_max[0] << "\t" << afma6._joint_max[1] << "\t" << afma6._joint_max[2] << "\t" - << afma6._joint_max[3] << "\t" << afma6._joint_max[4] << "\t" << afma6._joint_max[5] << "\t" << std::endl + << "\t" << afma6._joint_max[0] << "\t" << afma6._joint_max[1] << "\t" << afma6._joint_max[2] << "\t" + << afma6._joint_max[3] << "\t" << afma6._joint_max[4] << "\t" << afma6._joint_max[5] << "\t" << std::endl - << "Joint Min: " << std::endl - << "\t" << afma6._joint_min[0] << "\t" << afma6._joint_min[1] << "\t" << afma6._joint_min[2] << "\t" - << afma6._joint_min[3] << "\t" << afma6._joint_min[4] << "\t" << afma6._joint_min[5] << "\t" << std::endl + << "Joint Min: " << std::endl + << "\t" << afma6._joint_min[0] << "\t" << afma6._joint_min[1] << "\t" << afma6._joint_min[2] << "\t" + << afma6._joint_min[3] << "\t" << afma6._joint_min[4] << "\t" << afma6._joint_min[5] << "\t" << std::endl - << "Long 5-6: " << std::endl - << "\t" << afma6._long_56 << "\t" << std::endl + << "Long 5-6: " << std::endl + << "\t" << afma6._long_56 << "\t" << std::endl - << "Coupling 5-6:" << std::endl - << "\t" << afma6._coupl_56 << "\t" << std::endl + << "Coupling 5-6:" << std::endl + << "\t" << afma6._coupl_56 << "\t" << std::endl - << "eMc: " << std::endl - << "\tTranslation (m): " << afma6._eMc[0][3] << " " << afma6._eMc[1][3] << " " << afma6._eMc[2][3] << "\t" - << std::endl - << "\tRotation Rxyz (rad) : " << rxyz[0] << " " << rxyz[1] << " " << rxyz[2] << "\t" << std::endl - << "\tRotation Rxyz (deg) : " << vpMath::deg(rxyz[0]) << " " << vpMath::deg(rxyz[1]) << " " << vpMath::deg(rxyz[2]) - << "\t" << std::endl; + << "eMc: " << std::endl + << "\tTranslation (m): " << afma6._eMc[0][3] << " " << afma6._eMc[1][3] << " " << afma6._eMc[2][3] << "\t" + << std::endl + << "\tRotation Rxyz (rad) : " << rxyz[0] << " " << rxyz[1] << " " << rxyz[2] << "\t" << std::endl + << "\tRotation Rxyz (deg) : " << vpMath::deg(rxyz[0]) << " " << vpMath::deg(rxyz[1]) << " " << vpMath::deg(rxyz[2]) + << "\t" << std::endl; return os; } diff --git a/modules/robot/src/real-robot/viper/vpViper650.cpp b/modules/robot/src/real-robot/viper/vpViper650.cpp index 2531b8d986..d19b519ead 100644 --- a/modules/robot/src/real-robot/viper/vpViper650.cpp +++ b/modules/robot/src/real-robot/viper/vpViper650.cpp @@ -46,40 +46,40 @@ #include #include -static const char *opt_viper650[] = {"CAMERA", "eMc_ROT_XYZ", "eMc_TRANS_XYZ", nullptr}; +static const char *opt_viper650[] = { "CAMERA", "eMc_ROT_XYZ", "eMc_TRANS_XYZ", nullptr }; #ifdef VISP_HAVE_VIPER650_DATA const std::string vpViper650::CONST_EMC_MARLIN_F033C_WITHOUT_DISTORTION_FILENAME = - std::string(VISP_VIPER650_DATA_PATH) + - std::string("/include/const_eMc_MarlinF033C_without_distortion_Viper650.cnf"); +std::string(VISP_VIPER650_DATA_PATH) + +std::string("/include/const_eMc_MarlinF033C_without_distortion_Viper650.cnf"); const std::string vpViper650::CONST_EMC_MARLIN_F033C_WITH_DISTORTION_FILENAME = - std::string(VISP_VIPER650_DATA_PATH) + std::string("/include/const_eMc_MarlinF033C_with_distortion_Viper650.cnf"); +std::string(VISP_VIPER650_DATA_PATH) + std::string("/include/const_eMc_MarlinF033C_with_distortion_Viper650.cnf"); const std::string vpViper650::CONST_EMC_PTGREY_FLEA2_WITHOUT_DISTORTION_FILENAME = - std::string(VISP_VIPER650_DATA_PATH) + - std::string("/include/const_eMc_PTGreyFlea2_without_distortion_Viper650.cnf"); +std::string(VISP_VIPER650_DATA_PATH) + +std::string("/include/const_eMc_PTGreyFlea2_without_distortion_Viper650.cnf"); const std::string vpViper650::CONST_EMC_PTGREY_FLEA2_WITH_DISTORTION_FILENAME = - std::string(VISP_VIPER650_DATA_PATH) + std::string("/include/const_eMc_PTGreyFlea2_with_distortion_Viper650.cnf"); +std::string(VISP_VIPER650_DATA_PATH) + std::string("/include/const_eMc_PTGreyFlea2_with_distortion_Viper650.cnf"); const std::string vpViper650::CONST_EMC_SCHUNK_GRIPPER_WITHOUT_DISTORTION_FILENAME = - std::string(VISP_VIPER650_DATA_PATH) + std::string("/include/" - "const_eMc_schunk_gripper_without_distortion_Viper650." - "cnf"); +std::string(VISP_VIPER650_DATA_PATH) + std::string("/include/" + "const_eMc_schunk_gripper_without_distortion_Viper650." + "cnf"); const std::string vpViper650::CONST_EMC_SCHUNK_GRIPPER_WITH_DISTORTION_FILENAME = - std::string(VISP_VIPER650_DATA_PATH) + - std::string("/include/const_eMc_schunk_gripper_with_distortion_Viper650.cnf"); +std::string(VISP_VIPER650_DATA_PATH) + +std::string("/include/const_eMc_schunk_gripper_with_distortion_Viper650.cnf"); const std::string vpViper650::CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME = - std::string(VISP_VIPER650_DATA_PATH) + std::string("/include/const_eMc_generic_without_distortion_Viper650.cnf"); +std::string(VISP_VIPER650_DATA_PATH) + std::string("/include/const_eMc_generic_without_distortion_Viper650.cnf"); const std::string vpViper650::CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME = - std::string(VISP_VIPER650_DATA_PATH) + std::string("/include/const_eMc_generic_with_distortion_Viper650.cnf"); +std::string(VISP_VIPER650_DATA_PATH) + std::string("/include/const_eMc_generic_with_distortion_Viper650.cnf"); const std::string vpViper650::CONST_CAMERA_FILENAME = - std::string(VISP_VIPER650_DATA_PATH) + std::string("/include/const_camera_Viper650.xml"); +std::string(VISP_VIPER650_DATA_PATH) + std::string("/include/const_camera_Viper650.xml"); #endif // VISP_HAVE_VIPER650_DATA @@ -473,7 +473,8 @@ void vpViper650::parseConfigFile(const std::string &filename) // Compute the eMc matrix from the translations and rotations if (get_etc && get_erc) { this->set_eMc(etc_, erc_); - } else { + } + else { throw vpRobotException(vpRobotException::readingParametersError, "Could not read translation and rotation " "parameters from config file %s", @@ -553,13 +554,13 @@ parameters are not found. void vpViper650::getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height) const { -#if defined(VISP_HAVE_VIPER650_DATA) +#if defined(VISP_HAVE_VIPER650_DATA) && defined(VISP_HAVE_PUGIXML) vpXmlParserCamera parser; switch (getToolType()) { case vpViper650::TOOL_MARLIN_F033C_CAMERA: { std::cout << "Get camera parameters for camera \"" << vpViper650::CONST_MARLIN_F033C_CAMERA_NAME << "\"" - << std::endl - << "from the XML file: \"" << vpViper650::CONST_CAMERA_FILENAME << "\"" << std::endl; + << std::endl + << "from the XML file: \"" << vpViper650::CONST_CAMERA_FILENAME << "\"" << std::endl; if (parser.parse(cam, vpViper650::CONST_CAMERA_FILENAME, vpViper650::CONST_MARLIN_F033C_CAMERA_NAME, projModel, image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) { throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters."); @@ -568,8 +569,8 @@ void vpViper650::getCameraParameters(vpCameraParameters &cam, const unsigned int } case vpViper650::TOOL_PTGREY_FLEA2_CAMERA: { std::cout << "Get camera parameters for camera \"" << vpViper650::CONST_PTGREY_FLEA2_CAMERA_NAME << "\"" - << std::endl - << "from the XML file: \"" << vpViper650::CONST_CAMERA_FILENAME << "\"" << std::endl; + << std::endl + << "from the XML file: \"" << vpViper650::CONST_CAMERA_FILENAME << "\"" << std::endl; if (parser.parse(cam, vpViper650::CONST_CAMERA_FILENAME, vpViper650::CONST_PTGREY_FLEA2_CAMERA_NAME, projModel, image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) { throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters."); @@ -578,8 +579,8 @@ void vpViper650::getCameraParameters(vpCameraParameters &cam, const unsigned int } case vpViper650::TOOL_SCHUNK_GRIPPER_CAMERA: { std::cout << "Get camera parameters for camera \"" << vpViper650::CONST_SCHUNK_GRIPPER_CAMERA_NAME << "\"" - << std::endl - << "from the XML file: \"" << vpViper650::CONST_CAMERA_FILENAME << "\"" << std::endl; + << std::endl + << "from the XML file: \"" << vpViper650::CONST_CAMERA_FILENAME << "\"" << std::endl; if (parser.parse(cam, vpViper650::CONST_CAMERA_FILENAME, vpViper650::CONST_SCHUNK_GRIPPER_CAMERA_NAME, projModel, image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) { throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters."); @@ -588,7 +589,7 @@ void vpViper650::getCameraParameters(vpCameraParameters &cam, const unsigned int } case vpViper650::TOOL_GENERIC_CAMERA: { std::cout << "Get camera parameters for camera \"" << vpViper650::CONST_GENERIC_CAMERA_NAME << "\"" << std::endl - << "from the XML file: \"" << vpViper650::CONST_CAMERA_FILENAME << "\"" << std::endl; + << "from the XML file: \"" << vpViper650::CONST_CAMERA_FILENAME << "\"" << std::endl; if (parser.parse(cam, vpViper650::CONST_CAMERA_FILENAME, vpViper650::CONST_GENERIC_CAMERA_NAME, projModel, image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) { throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters."); @@ -618,7 +619,7 @@ void vpViper650::getCameraParameters(vpCameraParameters &cam, const unsigned int // Set default intrinsic camera parameters for 640x480 images if (image_width == 640 && image_height == 480) { std::cout << "Get default camera parameters for camera \"" << vpViper650::CONST_MARLIN_F033C_CAMERA_NAME << "\"" - << std::endl; + << std::endl; switch (this->projModel) { case vpCameraParameters::perspectiveProjWithoutDistortion: cam.initPersProjWithoutDistortion(1232.0, 1233.0, 317.7, 253.9); @@ -631,7 +632,8 @@ void vpViper650::getCameraParameters(vpCameraParameters &cam, const unsigned int "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet."); break; } - } else { + } + else { vpTRACE("Cannot get default intrinsic camera parameters for this image " "resolution"); throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters."); @@ -643,7 +645,7 @@ void vpViper650::getCameraParameters(vpCameraParameters &cam, const unsigned int // Set default intrinsic camera parameters for 640x480 images if (image_width == 640 && image_height == 480) { std::cout << "Get default camera parameters for camera \"" << vpViper650::CONST_PTGREY_FLEA2_CAMERA_NAME << "\"" - << std::endl; + << std::endl; switch (this->projModel) { case vpCameraParameters::perspectiveProjWithoutDistortion: cam.initPersProjWithoutDistortion(868.0, 869.0, 314.8, 254.1); @@ -656,7 +658,8 @@ void vpViper650::getCameraParameters(vpCameraParameters &cam, const unsigned int "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet."); break; } - } else { + } + else { vpTRACE("Cannot get default intrinsic camera parameters for this image " "resolution"); throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters."); @@ -667,7 +670,7 @@ void vpViper650::getCameraParameters(vpCameraParameters &cam, const unsigned int // Set default intrinsic camera parameters for 640x480 images if (image_width == 640 && image_height == 480) { std::cout << "Get default camera parameters for camera \"" << vpViper650::CONST_GENERIC_CAMERA_NAME << "\"" - << std::endl; + << std::endl; switch (this->projModel) { case vpCameraParameters::perspectiveProjWithoutDistortion: cam.initPersProjWithoutDistortion(868.0, 869.0, 314.8, 254.1); @@ -680,7 +683,8 @@ void vpViper650::getCameraParameters(vpCameraParameters &cam, const unsigned int "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet."); break; } - } else { + } + else { vpTRACE("Cannot get default intrinsic camera parameters for this image " "resolution"); throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters."); diff --git a/modules/robot/src/real-robot/viper/vpViper850.cpp b/modules/robot/src/real-robot/viper/vpViper850.cpp index d5951a1714..bce6df6ac4 100644 --- a/modules/robot/src/real-robot/viper/vpViper850.cpp +++ b/modules/robot/src/real-robot/viper/vpViper850.cpp @@ -46,40 +46,40 @@ #include #include -static const char *opt_viper850[] = {"CAMERA", "eMc_ROT_XYZ", "eMc_TRANS_XYZ", nullptr}; +static const char *opt_viper850[] = { "CAMERA", "eMc_ROT_XYZ", "eMc_TRANS_XYZ", nullptr }; #ifdef VISP_HAVE_VIPER850_DATA const std::string vpViper850::CONST_EMC_MARLIN_F033C_WITHOUT_DISTORTION_FILENAME = - std::string(VISP_VIPER850_DATA_PATH) + - std::string("/include/const_eMc_MarlinF033C_without_distortion_Viper850.cnf"); +std::string(VISP_VIPER850_DATA_PATH) + +std::string("/include/const_eMc_MarlinF033C_without_distortion_Viper850.cnf"); const std::string vpViper850::CONST_EMC_MARLIN_F033C_WITH_DISTORTION_FILENAME = - std::string(VISP_VIPER850_DATA_PATH) + std::string("/include/const_eMc_MarlinF033C_with_distortion_Viper850.cnf"); +std::string(VISP_VIPER850_DATA_PATH) + std::string("/include/const_eMc_MarlinF033C_with_distortion_Viper850.cnf"); const std::string vpViper850::CONST_EMC_PTGREY_FLEA2_WITHOUT_DISTORTION_FILENAME = - std::string(VISP_VIPER850_DATA_PATH) + - std::string("/include/const_eMc_PTGreyFlea2_without_distortion_Viper850.cnf"); +std::string(VISP_VIPER850_DATA_PATH) + +std::string("/include/const_eMc_PTGreyFlea2_without_distortion_Viper850.cnf"); const std::string vpViper850::CONST_EMC_PTGREY_FLEA2_WITH_DISTORTION_FILENAME = - std::string(VISP_VIPER850_DATA_PATH) + std::string("/include/const_eMc_PTGreyFlea2_with_distortion_Viper850.cnf"); +std::string(VISP_VIPER850_DATA_PATH) + std::string("/include/const_eMc_PTGreyFlea2_with_distortion_Viper850.cnf"); const std::string vpViper850::CONST_EMC_SCHUNK_GRIPPER_WITHOUT_DISTORTION_FILENAME = - std::string(VISP_VIPER850_DATA_PATH) + std::string("/include/" - "const_eMc_schunk_gripper_without_distortion_Viper850." - "cnf"); +std::string(VISP_VIPER850_DATA_PATH) + std::string("/include/" + "const_eMc_schunk_gripper_without_distortion_Viper850." + "cnf"); const std::string vpViper850::CONST_EMC_SCHUNK_GRIPPER_WITH_DISTORTION_FILENAME = - std::string(VISP_VIPER850_DATA_PATH) + - std::string("/include/const_eMc_schunk_gripper_with_distortion_Viper850.cnf"); +std::string(VISP_VIPER850_DATA_PATH) + +std::string("/include/const_eMc_schunk_gripper_with_distortion_Viper850.cnf"); const std::string vpViper850::CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME = - std::string(VISP_VIPER850_DATA_PATH) + std::string("/include/const_eMc_generic_without_distortion_Viper850.cnf"); +std::string(VISP_VIPER850_DATA_PATH) + std::string("/include/const_eMc_generic_without_distortion_Viper850.cnf"); const std::string vpViper850::CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME = - std::string(VISP_VIPER850_DATA_PATH) + std::string("/include/const_eMc_generic_with_distortion_Viper850.cnf"); +std::string(VISP_VIPER850_DATA_PATH) + std::string("/include/const_eMc_generic_with_distortion_Viper850.cnf"); const std::string vpViper850::CONST_CAMERA_FILENAME = - std::string(VISP_VIPER850_DATA_PATH) + std::string("/include/const_camera_Viper850.xml"); +std::string(VISP_VIPER850_DATA_PATH) + std::string("/include/const_camera_Viper850.xml"); #endif // VISP_HAVE_VIPER850_DATA @@ -475,7 +475,8 @@ void vpViper850::parseConfigFile(const std::string &filename) // Compute the eMc matrix from the translations and rotations if (get_etc && get_erc) { this->set_eMc(etc_, erc_); - } else { + } + else { throw vpRobotException(vpRobotException::readingParametersError, "Could not read translation and rotation " "parameters from config file %s", @@ -555,13 +556,13 @@ parameters are not found. void vpViper850::getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height) const { -#if defined(VISP_HAVE_VIPER850_DATA) +#if defined(VISP_HAVE_VIPER850_DATA) && defined(VISP_HAVE_PUGIXML) vpXmlParserCamera parser; switch (getToolType()) { case vpViper850::TOOL_MARLIN_F033C_CAMERA: { std::cout << "Get camera parameters for camera \"" << vpViper850::CONST_MARLIN_F033C_CAMERA_NAME << "\"" - << std::endl - << "from the XML file: \"" << vpViper850::CONST_CAMERA_FILENAME << "\"" << std::endl; + << std::endl + << "from the XML file: \"" << vpViper850::CONST_CAMERA_FILENAME << "\"" << std::endl; if (parser.parse(cam, vpViper850::CONST_CAMERA_FILENAME, vpViper850::CONST_MARLIN_F033C_CAMERA_NAME, projModel, image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) { throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters."); @@ -570,8 +571,8 @@ void vpViper850::getCameraParameters(vpCameraParameters &cam, const unsigned int } case vpViper850::TOOL_PTGREY_FLEA2_CAMERA: { std::cout << "Get camera parameters for camera \"" << vpViper850::CONST_PTGREY_FLEA2_CAMERA_NAME << "\"" - << std::endl - << "from the XML file: \"" << vpViper850::CONST_CAMERA_FILENAME << "\"" << std::endl; + << std::endl + << "from the XML file: \"" << vpViper850::CONST_CAMERA_FILENAME << "\"" << std::endl; if (parser.parse(cam, vpViper850::CONST_CAMERA_FILENAME, vpViper850::CONST_PTGREY_FLEA2_CAMERA_NAME, projModel, image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) { throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters."); @@ -580,8 +581,8 @@ void vpViper850::getCameraParameters(vpCameraParameters &cam, const unsigned int } case vpViper850::TOOL_SCHUNK_GRIPPER_CAMERA: { std::cout << "Get camera parameters for camera \"" << vpViper850::CONST_SCHUNK_GRIPPER_CAMERA_NAME << "\"" - << std::endl - << "from the XML file: \"" << vpViper850::CONST_CAMERA_FILENAME << "\"" << std::endl; + << std::endl + << "from the XML file: \"" << vpViper850::CONST_CAMERA_FILENAME << "\"" << std::endl; if (parser.parse(cam, vpViper850::CONST_CAMERA_FILENAME, vpViper850::CONST_SCHUNK_GRIPPER_CAMERA_NAME, projModel, image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) { throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters."); @@ -590,7 +591,7 @@ void vpViper850::getCameraParameters(vpCameraParameters &cam, const unsigned int } case vpViper850::TOOL_GENERIC_CAMERA: { std::cout << "Get camera parameters for camera \"" << vpViper850::CONST_GENERIC_CAMERA_NAME << "\"" << std::endl - << "from the XML file: \"" << vpViper850::CONST_CAMERA_FILENAME << "\"" << std::endl; + << "from the XML file: \"" << vpViper850::CONST_CAMERA_FILENAME << "\"" << std::endl; if (parser.parse(cam, vpViper850::CONST_CAMERA_FILENAME, vpViper850::CONST_GENERIC_CAMERA_NAME, projModel, image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) { throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters."); @@ -620,7 +621,7 @@ void vpViper850::getCameraParameters(vpCameraParameters &cam, const unsigned int // Set default intrinsic camera parameters for 640x480 images if (image_width == 640 && image_height == 480) { std::cout << "Get default camera parameters for camera \"" << vpViper850::CONST_MARLIN_F033C_CAMERA_NAME << "\"" - << std::endl; + << std::endl; switch (this->projModel) { case vpCameraParameters::perspectiveProjWithoutDistortion: cam.initPersProjWithoutDistortion(1232.0, 1233.0, 317.7, 253.9); @@ -633,7 +634,8 @@ void vpViper850::getCameraParameters(vpCameraParameters &cam, const unsigned int "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet."); break; } - } else { + } + else { vpTRACE("Cannot get default intrinsic camera parameters for this image " "resolution"); throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters."); @@ -645,7 +647,7 @@ void vpViper850::getCameraParameters(vpCameraParameters &cam, const unsigned int // Set default intrinsic camera parameters for 640x480 images if (image_width == 640 && image_height == 480) { std::cout << "Get default camera parameters for camera \"" << vpViper850::CONST_PTGREY_FLEA2_CAMERA_NAME << "\"" - << std::endl; + << std::endl; switch (this->projModel) { case vpCameraParameters::perspectiveProjWithoutDistortion: cam.initPersProjWithoutDistortion(868.0, 869.0, 314.8, 254.1); @@ -658,7 +660,8 @@ void vpViper850::getCameraParameters(vpCameraParameters &cam, const unsigned int "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet."); break; } - } else { + } + else { vpTRACE("Cannot get default intrinsic camera parameters for this image " "resolution"); throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters."); @@ -669,7 +672,7 @@ void vpViper850::getCameraParameters(vpCameraParameters &cam, const unsigned int // Set default intrinsic camera parameters for 640x480 images if (image_width == 640 && image_height == 480) { std::cout << "Get default camera parameters for camera \"" << vpViper850::CONST_GENERIC_CAMERA_NAME << "\"" - << std::endl; + << std::endl; switch (this->projModel) { case vpCameraParameters::perspectiveProjWithoutDistortion: cam.initPersProjWithoutDistortion(868.0, 869.0, 314.8, 254.1); @@ -682,7 +685,8 @@ void vpViper850::getCameraParameters(vpCameraParameters &cam, const unsigned int "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet."); break; } - } else { + } + else { vpTRACE("Cannot get default intrinsic camera parameters for this image " "resolution"); throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters."); diff --git a/modules/sensor/src/framegrabber/directshow/vpDirectShowSampleGrabberI.cpp b/modules/sensor/src/framegrabber/directshow/vpDirectShowSampleGrabberI.cpp index 0307a84319..be1b052676 100644 --- a/modules/sensor/src/framegrabber/directshow/vpDirectShowSampleGrabberI.cpp +++ b/modules/sensor/src/framegrabber/directshow/vpDirectShowSampleGrabberI.cpp @@ -94,7 +94,7 @@ STDMETHODIMP vpDirectShowSampleGrabberI::BufferCB(double Time, BYTE *pBuffer, lo BITMAPINFOHEADER bmpInfo = pVih->bmiHeader; // if biHeight > 0 and the source is not special - // then the image needs to be verticaly flipped + // then the image needs to be vertically flipped bool flip; if (!specialMediaType) flip = bmpInfo.biHeight >= 0; diff --git a/modules/sensor/src/framegrabber/ueye/vpUeyeGrabber.cpp b/modules/sensor/src/framegrabber/ueye/vpUeyeGrabber.cpp index b90ac24d7b..e73a1b15fe 100644 --- a/modules/sensor/src/framegrabber/ueye/vpUeyeGrabber.cpp +++ b/modules/sensor/src/framegrabber/ueye/vpUeyeGrabber.cpp @@ -1278,7 +1278,7 @@ bool vpUeyeGrabber::setActiveCamera(unsigned int cam_index) /*! * Updates active camera color mode. * - * \warning Before caling this function the connexion with the active camera should be opened. + * \warning Before calling this function the connexion with the active camera should be opened. * * \param[in] color_mode : Desired color mode. Admissible values are "MONO8", "RGB8", "RGB32" or "BAYER8". * \note - When acquiring gray level images using acquire(vpImage &) we strongly recommend to @@ -1298,7 +1298,7 @@ bool vpUeyeGrabber::setColorMode(const std::string &color_mode) /*! * Updates active camera exposure / shutter either to auto mode, or to specified manual parameters. * - * \warning Before caling this function the connexion with the active camera should be opened. + * \warning Before calling this function the connexion with the active camera should be opened. * * \param[in] auto_exposure : When true enable camera's hardware auto exposure / shutter. * This function returns false if the camera does not support auto exposure mode. When set to false, set manual exposure @@ -1319,7 +1319,7 @@ bool vpUeyeGrabber::setExposure(bool auto_exposure, double exposure_ms) * Enabling auto frame rate mode requires to enable auto shutter mode. * Enabling auto frame rate mode will disable auto gain mode. * - * \warning Before caling this function the connexion with the active camera should be opened. + * \warning Before calling this function the connexion with the active camera should be opened. * * \param[in] auto_frame_rate : Updates camera's hardware auto frame rate mode. When true enable auto frame rate mode. * When set to false, enables manual frame rate mode. @@ -1353,7 +1353,7 @@ bool vpUeyeGrabber::setFrameRate(bool auto_frame_rate, double manual_frame_rate_ * * Auto gain mode is disabled if auto frame rate mode is enabled. * - * \warning Before caling this function the connexion with the active camera should be opened. + * \warning Before calling this function the connexion with the active camera should be opened. * * \param[in] auto_gain : Updates camera's hardware auto gain mode. * - Set to true to enable auto gain. If this mode is not supported, returns false. @@ -1372,7 +1372,7 @@ bool vpUeyeGrabber::setGain(bool auto_gain, int master_gain, bool gain_boost) /*! * Updates active camera image subsampling factor to reduce image size. * - * \warning Before caling this function the connexion with the active camera should be opened. + * \warning Before calling this function the connexion with the active camera should be opened. * * \param[in] factor : Desired subsampling factor. The number of rows and columns * of the resulting image corresponds to the full resolution image size divided by this factor. @@ -1384,7 +1384,7 @@ void vpUeyeGrabber::setSubsampling(int factor) { m_impl->setSubsampling(factor); /*! * Enables or disables the active camera auto white balance mode. * - * \warning Before caling this function the connexion with the active camera should be opened. + * \warning Before calling this function the connexion with the active camera should be opened. * * \param auto_wb : If true enable auto white balance mode. If false, disable auto white balance mode. * diff --git a/modules/sensor/src/rgb-depth/kinect/vpKinect.cpp b/modules/sensor/src/rgb-depth/kinect/vpKinect.cpp index 79af83887b..6666baaa81 100644 --- a/modules/sensor/src/rgb-depth/kinect/vpKinect.cpp +++ b/modules/sensor/src/rgb-depth/kinect/vpKinect.cpp @@ -52,8 +52,8 @@ */ vpKinect::vpKinect(freenect_context *ctx, int index) : Freenect::FreenectDevice(ctx, index), m_rgb_mutex(), m_depth_mutex(), RGBcam(), IRcam(), rgbMir(), irMrgb(), - DMres(DMAP_LOW_RES), hd(240), wd(320), dmap(), IRGB(), m_new_rgb_frame(false), m_new_depth_map(false), - m_new_depth_image(false), height(480), width(640) + DMres(DMAP_LOW_RES), hd(240), wd(320), dmap(), IRGB(), m_new_rgb_frame(false), m_new_depth_map(false), + m_new_depth_image(false), height(480), width(640) { dmap.resize(height, width); IRGB.resize(height, width); @@ -68,7 +68,7 @@ vpKinect::vpKinect(freenect_context *ctx, int index) /*! Destructor. */ -vpKinect::~vpKinect() {} +vpKinect::~vpKinect() { } void vpKinect::start(vpKinect::vpDMResolution res) { @@ -82,7 +82,8 @@ void vpKinect::start(vpKinect::vpDMResolution res) IRcam.initPersProjWithDistortion(303.06, 297.89, 160.75, 117.9, -0.27, 0); hd = 240; wd = 320; - } else { + } + else { std::cout << "vpKinect::start MEDIUM depth map resolution 480x640" << std::endl; IRcam.initPersProjWithDistortion(606.12, 595.78, 321.5, 235.8, -0.27, 0); @@ -91,7 +92,7 @@ void vpKinect::start(vpKinect::vpDMResolution res) wd = 640; } -#if defined(VISP_HAVE_VIPER850_DATA) +#if defined(VISP_HAVE_VIPER850_DATA) && defined(VISP_HAVE_PUGIXML) vpXmlParserCamera cameraParser; std::string cameraXmlFile = std::string(VISP_VIPER850_DATA_PATH) + std::string("/include/const_camera_Viper850.xml"); cameraParser.parse(RGBcam, cameraXmlFile, "Generic-camera", vpCameraParameters::perspectiveProjWithDistortion, width, @@ -150,8 +151,8 @@ void vpKinect::DepthCallback(void *depth, uint32_t /* timestamp */) for (unsigned i = 0; i < height; i++) { for (unsigned j = 0; j < width; j++) { dmap[i][j] = - 0.1236f * tan(depth_[width * i + j] / 2842.5f + 1.1863f); // formula from - // http://openkinect.org/wiki/Imaging_Information + 0.1236f * tan(depth_[width * i + j] / 2842.5f + 1.1863f); // formula from + // http://openkinect.org/wiki/Imaging_Information if (depth_[width * i + j] > 1023) { // Depth cannot be computed dmap[i][j] = -1; } @@ -203,7 +204,8 @@ bool vpKinect::getDepthMap(vpImage &map, vpImage &Imap) else Imap[i][j] = 255; } - } else { + } + else { for (unsigned i = 0; i < height; i++) for (unsigned j = 0; j < width; j++) { map[i][j] = tempMap[i][j]; @@ -239,7 +241,8 @@ void vpKinect::warpRGBFrame(const vpImage &Irgb, const vpImage &I { if ((Idepth.getHeight() != hd) || (Idepth.getWidth() != wd)) { vpERROR_TRACE(1, "Idepth image size does not match vpKinect DM resolution"); - } else { + } + else { if ((IrgbWarped.getHeight() != hd) || (IrgbWarped.getWidth() != wd)) IrgbWarped.resize(hd, wd); IrgbWarped = 0; @@ -267,7 +270,8 @@ void vpKinect::warpRGBFrame(const vpImage &Irgb, const vpImage &I if (std::fabs(Z2) > std::numeric_limits::epsilon()) { x2 = P2[0] / P2[2]; y2 = P2[1] / P2[2]; - } else + } + else std::cout << "Z2 = 0 !!" << std::endl; //! compute pixel coordinates of the corresponding point in the @@ -279,7 +283,8 @@ void vpKinect::warpRGBFrame(const vpImage &Irgb, const vpImage &I //! Fill warped image value if ((u_ < width) && (v_ < height)) { IrgbWarped[i][j] = Irgb[v_][u_]; - } else + } + else IrgbWarped[i][j] = 0; } } @@ -289,5 +294,5 @@ void vpKinect::warpRGBFrame(const vpImage &Irgb, const vpImage &I #elif !defined(VISP_BUILD_SHARED_LIBS) // Work around to avoid warning: libvisp_sensor.a(vpKinect.cpp.o) has no // symbols -void dummy_vpKinect(){}; +void dummy_vpKinect() { }; #endif // VISP_HAVE_LIBFREENECT diff --git a/modules/tracker/mbt/CMakeLists.txt b/modules/tracker/mbt/CMakeLists.txt index c5e5e7cb72..9b87e7c378 100644 --- a/modules/tracker/mbt/CMakeLists.txt +++ b/modules/tracker/mbt/CMakeLists.txt @@ -36,6 +36,7 @@ # Add optional 3rd parties set(opt_incs "") set(opt_libs "") +set(opt_libs_private "") if(USE_COIN3D AND NOT HAVE_visp_ar) if(WIN32) @@ -122,22 +123,26 @@ endif() if(WITH_CLIPPER) # clipper is private include_directories(${CLIPPER_INCLUDE_DIRS}) + list(APPEND opt_libs_private ${CLIPPER_LIBRARIES}) endif() # pugixml is always enabled to provide default XML I/O capabilities # pugixml is private include_directories(${PUGIXML_INCLUDE_DIRS}) +list(APPEND opt_libs_private ${PUGIXML_LIBRARIES}) if(WITH_CATCH2) # catch2 is private include_directories(${CATCH2_INCLUDE_DIRS}) endif() -# simdlib is always enabled since it contains fallback code to plain C++ code -# Simd lib is private -include_directories(${SIMDLIB_INCLUDE_DIRS}) +if(WITH_SIMDLIB) + # Simd lib is private + include_directories(${SIMDLIB_INCLUDE_DIRS}) + list(APPEND opt_libs_private ${SIMDLIB_LIBRARIES}) +endif() -vp_add_module(mbt visp_vision visp_core visp_me visp_visual_features OPTIONAL visp_ar visp_klt visp_gui PRIVATE_OPTIONAL ${CLIPPER_LIBRARIES} ${PUGIXML_LIBRARIES} ${SIMDLIB_LIBRARIES} WRAP java) +vp_add_module(mbt visp_vision visp_core visp_me visp_visual_features OPTIONAL visp_ar visp_klt visp_gui PRIVATE_OPTIONAL ${opt_libs_private} WRAP java) vp_glob_module_sources() if(USE_OGRE) diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbtTukeyEstimator.h b/modules/tracker/mbt/include/visp3/mbt/vpMbtTukeyEstimator.h index ed99ef4a17..763a4ec1d8 100755 --- a/modules/tracker/mbt/include/visp3/mbt/vpMbtTukeyEstimator.h +++ b/modules/tracker/mbt/include/visp3/mbt/vpMbtTukeyEstimator.h @@ -312,7 +312,11 @@ template <> inline void vpMbtTukeyEstimator::MEstimator(const std::vector &residues, std::vector &weights, float NoiseThreshold) { +#if defined(VISP_HAVE_SIMDLIB) bool checkSimd = vpCPUFeatures::checkSSSE3() || vpCPUFeatures::checkNeon(); +#else + bool checkSimd = vpCPUFeatures::checkSSSE3(); +#endif #if !VISP_HAVE_SSSE3 && !VISP_HAVE_NEON checkSimd = false; #endif @@ -330,7 +334,11 @@ template <> inline void vpMbtTukeyEstimator::MEstimator(const std::vector &residues, std::vector &weights, double NoiseThreshold) { +#if defined(VISP_HAVE_SIMDLIB) bool checkSimd = vpCPUFeatures::checkSSSE3() || vpCPUFeatures::checkNeon(); +#else + bool checkSimd = vpCPUFeatures::checkSSSE3(); +#endif #if !VISP_HAVE_SSSE3 && !VISP_HAVE_NEON checkSimd = false; #endif diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbtXmlGenericParser.h b/modules/tracker/mbt/include/visp3/mbt/vpMbtXmlGenericParser.h index fff258040e..e9de44f18e 100644 --- a/modules/tracker/mbt/include/visp3/mbt/vpMbtXmlGenericParser.h +++ b/modules/tracker/mbt/include/visp3/mbt/vpMbtXmlGenericParser.h @@ -44,6 +44,7 @@ #include +#if defined(VISP_HAVE_PUGIXML) #include #include #include @@ -60,7 +61,8 @@ class VISP_EXPORT vpMbtXmlGenericParser { public: - enum vpParserType { + enum vpParserType + { EDGE_PARSER = 1 << 0, /*!< Parser for model-based tracking using moving edges features. */ KLT_PARSER = 1 << 1, /*!< Parser for model-based tracking using KLT features. */ @@ -164,3 +166,4 @@ class VISP_EXPORT vpMbtXmlGenericParser }; #endif +#endif diff --git a/modules/tracker/mbt/src/depth/vpMbDepthDenseTracker.cpp b/modules/tracker/mbt/src/depth/vpMbDepthDenseTracker.cpp index db38947bd9..bd679514f8 100644 --- a/modules/tracker/mbt/src/depth/vpMbDepthDenseTracker.cpp +++ b/modules/tracker/mbt/src/depth/vpMbDepthDenseTracker.cpp @@ -395,6 +395,7 @@ void vpMbDepthDenseTracker::init(const vpImage &I) void vpMbDepthDenseTracker::loadConfigFile(const std::string &configFile, bool verbose) { +#if defined(VISP_HAVE_PUGIXML) vpMbtXmlGenericParser xmlp(vpMbtXmlGenericParser::DEPTH_DENSE_PARSER); xmlp.setVerbose(verbose); xmlp.setCameraParameters(m_cam); @@ -432,6 +433,11 @@ void vpMbDepthDenseTracker::loadConfigFile(const std::string &configFile, bool v setClipping(clippingFlag | vpPolygon3D::FOV_CLIPPING); setDepthDenseSamplingStep(xmlp.getDepthDenseSamplingStepX(), xmlp.getDepthDenseSamplingStepY()); +#else + (void)configFile; + (void)verbose; + throw(vpException(vpException::ioError, "vpMbDepthDenseTracker::loadConfigFile() needs pugixml built-in 3rdparty")); +#endif } void vpMbDepthDenseTracker::reInitModel(const vpImage &I, const std::string &cad_name, diff --git a/modules/tracker/mbt/src/depth/vpMbDepthNormalTracker.cpp b/modules/tracker/mbt/src/depth/vpMbDepthNormalTracker.cpp index 2b349c8c12..80948122cc 100644 --- a/modules/tracker/mbt/src/depth/vpMbDepthNormalTracker.cpp +++ b/modules/tracker/mbt/src/depth/vpMbDepthNormalTracker.cpp @@ -424,6 +424,7 @@ void vpMbDepthNormalTracker::init(const vpImage &I) void vpMbDepthNormalTracker::loadConfigFile(const std::string &configFile, bool verbose) { +#if defined(VISP_HAVE_PUGIXML) vpMbtXmlGenericParser xmlp(vpMbtXmlGenericParser::DEPTH_NORMAL_PARSER); xmlp.setVerbose(verbose); xmlp.setCameraParameters(m_cam); @@ -469,6 +470,11 @@ void vpMbDepthNormalTracker::loadConfigFile(const std::string &configFile, bool setDepthNormalPclPlaneEstimationRansacMaxIter(xmlp.getDepthNormalPclPlaneEstimationRansacMaxIter()); setDepthNormalPclPlaneEstimationRansacThreshold(xmlp.getDepthNormalPclPlaneEstimationRansacThreshold()); setDepthNormalSamplingStep(xmlp.getDepthNormalSamplingStepX(), xmlp.getDepthNormalSamplingStepY()); +#else + (void)configFile; + (void)verbose; + throw(vpException(vpException::ioError, "vpMbDepthDenseTracker::loadConfigFile() needs pugixml built-in 3rdparty")); +#endif } void vpMbDepthNormalTracker::reInitModel(const vpImage &I, const std::string &cad_name, diff --git a/modules/tracker/mbt/src/depth/vpMbtFaceDepthDense.cpp b/modules/tracker/mbt/src/depth/vpMbtFaceDepthDense.cpp index 7d0249cdc2..c602e14d0d 100644 --- a/modules/tracker/mbt/src/depth/vpMbtFaceDepthDense.cpp +++ b/modules/tracker/mbt/src/depth/vpMbtFaceDepthDense.cpp @@ -564,7 +564,11 @@ void vpMbtFaceDepthDense::computeInteractionMatrixAndResidu(const vpHomogeneousM double nz = m_planeCamera.getC(); double D = m_planeCamera.getD(); +#if defined(VISP_HAVE_SIMDLIB) bool useSIMD = vpCPUFeatures::checkSSE2() || vpCPUFeatures::checkNeon(); +#else + bool useSIMD = vpCPUFeatures::checkSSE2(); +#endif #if USE_OPENCV_HAL useSIMD = true; #endif @@ -573,7 +577,7 @@ void vpMbtFaceDepthDense::computeInteractionMatrixAndResidu(const vpHomogeneousM #endif if (useSIMD) { -#if USE_SSE || USE_NEON|| USE_OPENCV_HAL +#if USE_SSE || USE_NEON || USE_OPENCV_HAL size_t cpt = 0; if (getNbFeatures() >= 2) { double *ptr_point_cloud = &m_pointCloudFace[0]; diff --git a/modules/tracker/mbt/src/edge/vpMbEdgeTracker.cpp b/modules/tracker/mbt/src/edge/vpMbEdgeTracker.cpp index 663fa223f0..f55f12ab1b 100644 --- a/modules/tracker/mbt/src/edge/vpMbEdgeTracker.cpp +++ b/modules/tracker/mbt/src/edge/vpMbEdgeTracker.cpp @@ -63,10 +63,10 @@ */ vpMbEdgeTracker::vpMbEdgeTracker() : me(), lines(1), circles(1), cylinders(1), nline(0), ncircle(0), ncylinder(0), nbvisiblepolygone(0), - percentageGdPt(0.4), scales(1), Ipyramid(0), scaleLevel(0), nbFeaturesForProjErrorComputation(0), m_factor(), - m_robustLines(), m_robustCylinders(), m_robustCircles(), m_wLines(), m_wCylinders(), m_wCircles(), m_errorLines(), - m_errorCylinders(), m_errorCircles(), m_L_edge(), m_error_edge(), m_w_edge(), m_weightedError_edge(), - m_robust_edge(), m_featuresToBeDisplayedEdge() + percentageGdPt(0.4), scales(1), Ipyramid(0), scaleLevel(0), nbFeaturesForProjErrorComputation(0), m_factor(), + m_robustLines(), m_robustCylinders(), m_robustCircles(), m_wLines(), m_wCylinders(), m_wCircles(), m_errorLines(), + m_errorCylinders(), m_errorCircles(), m_L_edge(), m_error_edge(), m_w_edge(), m_weightedError_edge(), + m_robust_edge(), m_featuresToBeDisplayedEdge() { scales[0] = true; @@ -251,7 +251,8 @@ void vpMbEdgeTracker::computeVVS(const vpImage &_I, unsigned int m_L_edge[i][j] = wi * m_L_edge[i][j]; } } - } else { + } + else { for (unsigned int i = 0; i < nbrow; i++) { wi = m_w_edge[i] * m_factor[i]; W_true[i] = wi; @@ -354,7 +355,8 @@ void vpMbEdgeTracker::computeVVSFirstPhase(const vpImage &_I, uns m_w_edge[n + i] = 1 /*0.5*/; } e_prev = e_cur; - } else + } + else m_w_edge[n + i] = 1; } @@ -415,7 +417,8 @@ void vpMbEdgeTracker::computeVVSFirstPhase(const vpImage &_I, uns if (i < cy->nbFeaturel1) { site = *itCyl1; ++itCyl1; - } else { + } + else { site = *itCyl2; ++itCyl2; } @@ -432,7 +435,8 @@ void vpMbEdgeTracker::computeVVSFirstPhase(const vpImage &_I, uns m_w_edge[n + i] = 1 /*0.5*/; } e_prev = e_cur; - } else + } + else m_w_edge[n + i] = 1; } if (i == cy->nbFeaturel1) { @@ -443,7 +447,8 @@ void vpMbEdgeTracker::computeVVSFirstPhase(const vpImage &_I, uns m_w_edge[n + i] = 1 /*0.5*/; } e_prev = e_cur; - } else + } + else m_w_edge[n + i] = 1; } @@ -518,7 +523,8 @@ void vpMbEdgeTracker::computeVVSFirstPhase(const vpImage &_I, uns m_w_edge[n + i] = 1 /*0.5*/; } e_prev = e_cur; - } else + } + else m_w_edge[n + i] = 1; } @@ -611,7 +617,8 @@ void vpMbEdgeTracker::computeVVSFirstPhaseFactor(const vpImage &I if (i < cy->nbFeaturel1) { site = *itCyl1; ++itCyl1; - } else { + } + else { site = *itCyl2; ++itCyl2; } @@ -662,7 +669,8 @@ void vpMbEdgeTracker::computeVVSFirstPhasePoseEstimation(unsigned int iter, bool m_L_edge[i][j] = wi * m_L_edge[i][j]; } } - } else { + } + else { for (unsigned int i = 0; i < nerror; i++) { wi = m_w_edge[i] * m_factor[i]; eri = m_error_edge[i]; @@ -702,7 +710,8 @@ void vpMbEdgeTracker::computeVVSFirstPhasePoseEstimation(unsigned int iter, bool LTL = m_L_edge.AtA(); computeJTR(m_L_edge, m_weightedError_edge, LTR); v = -0.7 * LTL.pseudoInverse(LTL.getRows() * std::numeric_limits::epsilon()) * LTR; - } else { + } + else { cVo.buildFrom(m_cMo); vpMatrix LVJ = (m_L_edge * cVo * oJo); vpMatrix LVJTLVJ = (LVJ).AtA(); @@ -830,7 +839,7 @@ void vpMbEdgeTracker::computeVVSInteractionMatrixAndResidu(const vpImage 0) m_robustLines.MEstimator(vpRobust::TUKEY, m_errorLines, m_wLines); @@ -916,7 +925,8 @@ void vpMbEdgeTracker::computeProjectionError(const vpImage &_I) if (nbFeatures > 0) { projectionError = vpMath::deg(projectionError / (double)nbFeatures); - } else { + } + else { projectionError = 90.0; } @@ -1003,8 +1013,8 @@ void vpMbEdgeTracker::testTracking() if (nbGoodPoint < nb_min || nbExpectedPoint < 2) { std::ostringstream oss; oss << "Not enough moving edges (" << nbGoodPoint << ") to track the object: expected " << nb_min - << ". Try to reduce the threshold=" << percentageGdPt - << " using vpMbTracker::setGoodMovingEdgesRatioThreshold()"; + << ". Try to reduce the threshold=" << percentageGdPt + << " using vpMbTracker::setGoodMovingEdgesRatioThreshold()"; throw vpTrackingException(vpTrackingException::fatalError, oss.str()); } } @@ -1033,7 +1043,8 @@ void vpMbEdgeTracker::track(const vpImage &I) try { trackMovingEdge(*Ipyramid[lvl]); - } catch (...) { + } + catch (...) { vpTRACE("Error in moving edge tracking"); throw; } @@ -1064,7 +1075,8 @@ void vpMbEdgeTracker::track(const vpImage &I) try { computeVVS(*Ipyramid[lvl], lvl); - } catch (...) { + } + catch (...) { covarianceMatrix = -1; throw; // throw the original exception } @@ -1095,12 +1107,14 @@ void vpMbEdgeTracker::track(const vpImage &I) computeProjectionError(I); upScale(lvl); - } catch (const vpException &e) { + } + catch (const vpException &e) { if (lvl != 0) { m_cMo = cMo_1; reInitLevel(lvl); upScale(lvl); - } else { + } + else { upScale(lvl); throw(e); } @@ -1214,7 +1228,8 @@ void vpMbEdgeTracker::setPose(const vpImage &I_color, const vpHomogeneou */ void vpMbEdgeTracker::loadConfigFile(const std::string &configFile, bool verbose) { - // Load projection error config +#if defined(VISP_HAVE_PUGIXML) +// Load projection error config vpMbTracker::loadConfigFile(configFile, verbose); vpMbtXmlGenericParser xmlp(vpMbtXmlGenericParser::EDGE_PARSER); @@ -1229,7 +1244,8 @@ void vpMbEdgeTracker::loadConfigFile(const std::string &configFile, bool verbose std::cout << " *********** Parsing XML for Mb Edge Tracker ************ " << std::endl; } xmlp.parse(configFile); - } catch (...) { + } + catch (...) { throw vpException(vpException::ioError, "Cannot open XML file \"%s\"", configFile.c_str()); } @@ -1263,6 +1279,11 @@ void vpMbEdgeTracker::loadConfigFile(const std::string &configFile, bool verbose setMinLineLengthThresh(minLineLengthThresholdGeneral); setMinPolygonAreaThresh(minPolygonAreaThresholdGeneral); } +#else + (void)configFile; + (void)verbose; + throw(vpException(vpException::ioError, "vpMbEdgeTracker::loadConfigFile() needs pugixml built-in 3rdparty")); +#endif } /*! @@ -1286,14 +1307,15 @@ void vpMbEdgeTracker::display(const vpImage &I, const vpHomogeneo } std::vector > models = - vpMbEdgeTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel); + vpMbEdgeTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel); for (size_t i = 0; i < models.size(); i++) { if (vpMath::equal(models[i][0], 0)) { vpImagePoint ip1(models[i][1], models[i][2]); vpImagePoint ip2(models[i][3], models[i][4]); vpDisplay::displayLine(I, ip1, ip2, col, thickness); - } else if (vpMath::equal(models[i][0], 1)) { + } + else if (vpMath::equal(models[i][0], 1)) { vpImagePoint center(models[i][1], models[i][2]); double n20 = models[i][3]; double n11 = models[i][4]; @@ -1328,14 +1350,15 @@ void vpMbEdgeTracker::display(const vpImage &I, const vpHomogeneousMatri } std::vector > models = - vpMbEdgeTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel); + vpMbEdgeTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel); for (size_t i = 0; i < models.size(); i++) { if (vpMath::equal(models[i][0], 0)) { vpImagePoint ip1(models[i][1], models[i][2]); vpImagePoint ip2(models[i][3], models[i][4]); vpDisplay::displayLine(I, ip1, ip2, col, thickness); - } else if (vpMath::equal(models[i][0], 1)) { + } + else if (vpMath::equal(models[i][0], 1)) { vpImagePoint center(models[i][1], models[i][2]); double n20 = models[i][3]; double n11 = models[i][4]; @@ -1410,14 +1433,14 @@ std::vector > vpMbEdgeTracker::getModelForDisplay(unsigned i for (std::list::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end(); ++it) { std::vector > currentModel = - (*it)->getModelForDisplay(width, height, cMo, cam, displayFullModel); + (*it)->getModelForDisplay(width, height, cMo, cam, displayFullModel); models.insert(models.end(), currentModel.begin(), currentModel.end()); } for (std::list::const_iterator it = cylinders[scaleLevel].begin(); it != cylinders[scaleLevel].end(); ++it) { std::vector > currentModel = - (*it)->getModelForDisplay(width, height, cMo, cam, displayFullModel); + (*it)->getModelForDisplay(width, height, cMo, cam, displayFullModel); models.insert(models.end(), currentModel.begin(), currentModel.end()); } @@ -1542,7 +1565,8 @@ void vpMbEdgeTracker::initMovingEdge(const vpImage &I, const vpHo l->updateTracked(); if (l->meline.empty() && l->isTracked()) l->initMovingEdge(I, _cMo, doNotTrack, m_mask); - } else { + } + else { l->setVisible(false); for (size_t a = 0; a < l->meline.size(); a++) { if (l->meline[a] != nullptr) @@ -1578,7 +1602,8 @@ void vpMbEdgeTracker::initMovingEdge(const vpImage &I, const vpHo if (cy->isTracked()) cy->initMovingEdge(I, _cMo, doNotTrack, m_mask); } - } else { + } + else { cy->setVisible(false); if (cy->meline1 != nullptr) delete cy->meline1; @@ -1611,7 +1636,8 @@ void vpMbEdgeTracker::initMovingEdge(const vpImage &I, const vpHo if (ci->isTracked()) ci->initMovingEdge(I, _cMo, doNotTrack, m_mask); } - } else { + } + else { ci->setVisible(false); if (ci->meEllipse != nullptr) delete ci->meEllipse; @@ -2059,7 +2085,7 @@ void vpMbEdgeTracker::addCircle(const vpPoint &P1, const vpPoint &P2, const vpPo if ((samePoint(*(ci->p1), P1) && samePoint(*(ci->p2), P2) && samePoint(*(ci->p3), P3)) || (samePoint(*(ci->p1), P1) && samePoint(*(ci->p2), P3) && samePoint(*(ci->p3), P2))) { already_here = - (std::fabs(ci->radius - r) < std::numeric_limits::epsilon() * vpMath::maximum(ci->radius, r)); + (std::fabs(ci->radius - r) < std::numeric_limits::epsilon() * vpMath::maximum(ci->radius, r)); } } @@ -2118,7 +2144,7 @@ void vpMbEdgeTracker::addCylinder(const vpPoint &P1, const vpPoint &P2, double r if ((samePoint(*(cy->p1), P1) && samePoint(*(cy->p2), P2)) || (samePoint(*(cy->p1), P2) && samePoint(*(cy->p2), P1))) { already_here = - (std::fabs(cy->radius - r) < std::numeric_limits::epsilon() * vpMath::maximum(cy->radius, r)); + (std::fabs(cy->radius - r) < std::numeric_limits::epsilon() * vpMath::maximum(cy->radius, r)); } } @@ -2220,7 +2246,8 @@ void vpMbEdgeTracker::visibleFace(const vpImage &I, const vpHomog // n = faces.setVisible(_I.getWidth(), I.getHeight(), m_cam, cMo, vpMath::rad(89), vpMath::rad(89), // changed); n = faces.setVisible(I.getWidth(), I.getHeight(), m_cam, cMo, angleAppears, angleDisappears, changed); - } else { + } + else { #ifdef VISP_HAVE_OGRE n = faces.setVisibleOgre(I.getWidth(), I.getHeight(), m_cam, cMo, angleAppears, angleDisappears, changed); #else @@ -2231,7 +2258,8 @@ void vpMbEdgeTracker::visibleFace(const vpImage &I, const vpHomog if (n > nbvisiblepolygone) { // cout << "une nouvelle face est visible " << endl; newvisibleline = true; - } else + } + else newvisibleline = false; nbvisiblepolygone = n; @@ -2518,9 +2546,9 @@ unsigned int vpMbEdgeTracker::getNbPoints(unsigned int level) const if (l->nbFeature[a] != 0) for (std::list::const_iterator itme = l->meline[a]->getMeList().begin(); itme != l->meline[a]->getMeList().end(); ++itme) { - if (itme->getState() == vpMeSite::NO_SUPPRESSION) - nbGoodPoints++; - } + if (itme->getState() == vpMeSite::NO_SUPPRESSION) + nbGoodPoints++; + } } } } @@ -2602,7 +2630,8 @@ void vpMbEdgeTracker::setScales(const std::vector &scale) circles.resize(1); circles[0].clear(); - } else { + } + else { this->scales = scale; lines.resize(scale.size()); @@ -2626,12 +2655,12 @@ void vpMbEdgeTracker::setFarClippingDistance(const double &dist) { if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING && dist <= distNearClip) std::cerr << "Far clipping value cannot be inferior than near clipping " - "value. Far clipping won't be considered." - << std::endl; + "value. Far clipping won't be considered." + << std::endl; else if (dist < 0) std::cerr << "Far clipping value cannot be inferior than 0. Far clipping " - "won't be considered." - << std::endl; + "won't be considered." + << std::endl; else { vpMbTracker::setFarClippingDistance(dist); vpMbtDistanceLine *l; @@ -2656,12 +2685,12 @@ void vpMbEdgeTracker::setNearClippingDistance(const double &dist) { if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING && dist >= distFarClip) std::cerr << "Near clipping value cannot be superior than far clipping " - "value. Near clipping won't be considered." - << std::endl; + "value. Near clipping won't be considered." + << std::endl; else if (dist < 0) std::cerr << "Near clipping value cannot be inferior than 0. Near " - "clipping won't be considered." - << std::endl; + "clipping won't be considered." + << std::endl; else { vpMbTracker::setNearClippingDistance(dist); vpMbtDistanceLine *l; @@ -2722,7 +2751,8 @@ void vpMbEdgeTracker::initPyramid(const vpImage &_I, if (scales[0]) { _pyramid[0] = &_I; - } else { + } + else { _pyramid[0] = nullptr; } @@ -2736,7 +2766,8 @@ void vpMbEdgeTracker::initPyramid(const vpImage &_I, } } _pyramid[i] = I; - } else { + } + else { _pyramid[i] = nullptr; } } diff --git a/modules/tracker/mbt/src/hybrid/vpMbEdgeKltTracker.cpp b/modules/tracker/mbt/src/hybrid/vpMbEdgeKltTracker.cpp index 911fc9692c..712c2c5703 100644 --- a/modules/tracker/mbt/src/hybrid/vpMbEdgeKltTracker.cpp +++ b/modules/tracker/mbt/src/hybrid/vpMbEdgeKltTracker.cpp @@ -273,7 +273,8 @@ unsigned int vpMbEdgeKltTracker::initMbtTracking(unsigned int lvl) */ void vpMbEdgeKltTracker::loadConfigFile(const std::string &configFile, bool verbose) { - // Load projection error config +#if defined(VISP_HAVE_PUGIXML) +// Load projection error config vpMbTracker::loadConfigFile(configFile, verbose); vpMbtXmlGenericParser xmlp(vpMbtXmlGenericParser::EDGE_PARSER | vpMbtXmlGenericParser::KLT_PARSER); @@ -348,6 +349,11 @@ void vpMbEdgeKltTracker::loadConfigFile(const std::string &configFile, bool verb // if(useScanLine) faces.getMbScanLineRenderer().setMaskBorder(maskBorder); +#else + (void)configFile; + (void)verbose; + throw(vpException(vpException::ioError, "vpMbEdgeKltTracker::loadConfigFile() needs pugixml built-in 3rdparty")); +#endif } /*! diff --git a/modules/tracker/mbt/src/klt/vpMbKltTracker.cpp b/modules/tracker/mbt/src/klt/vpMbKltTracker.cpp index 944a4c1ee8..eb5b1b9a2c 100644 --- a/modules/tracker/mbt/src/klt/vpMbKltTracker.cpp +++ b/modules/tracker/mbt/src/klt/vpMbKltTracker.cpp @@ -107,9 +107,9 @@ vpMatrix homography2collineation(const vpMatrix &H, const vpCameraParameters &ca vpMbKltTracker::vpMbKltTracker() : - cur(), c0Mo(), firstInitialisation(true), maskBorder(5), threshold_outlier(0.5), percentGood(0.6), ctTc0(), tracker(), - kltPolygons(), kltCylinders(), circles_disp(), m_nbInfos(0), m_nbFaceUsed(0), m_L_klt(), m_error_klt(), m_w_klt(), - m_weightedError_klt(), m_robust_klt(), m_featuresToBeDisplayedKlt() + cur(), c0Mo(), firstInitialisation(true), maskBorder(5), threshold_outlier(0.5), percentGood(0.6), ctTc0(), tracker(), + kltPolygons(), kltCylinders(), circles_disp(), m_nbInfos(0), m_nbFaceUsed(0), m_L_klt(), m_error_klt(), m_w_klt(), + m_weightedError_klt(), m_robust_klt(), m_featuresToBeDisplayedKlt() { tracker.setTrackerId(1); tracker.setUseHarris(1); @@ -218,7 +218,8 @@ void vpMbKltTracker::reinit(const vpImage &I) vpMbtDistanceKltCylinder *kltPolyCylinder; if (useScanLine) { vpImageConvert::convert(faces.getMbScanLineRenderer().getMask(), mask); - } else { + } + else { unsigned char val = 255 /* - i*15*/; for (std::list::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) { kltpoly = *it; @@ -443,16 +444,18 @@ void vpMbKltTracker::setPose(const vpImage *const I, const vpImag if (!kltCylinders.empty()) { std::cout << "WARNING: Cannot set pose when model contains cylinder(s). " - "This feature is not implemented yet." - << std::endl; + "This feature is not implemented yet." + << std::endl; std::cout << "Tracker will be reinitialized with the given pose." << std::endl; m_cMo = cdMo; if (I) { init(*I); - } else { + } + else { init(m_I); } - } else { + } + else { vpMbtDistanceKltPoints *kltpoly; std::vector init_pts; @@ -543,7 +546,8 @@ void vpMbKltTracker::setPose(const vpImage *const I, const vpImag if (I) { vpImageConvert::convert(*I, cur); - } else { + } + else { vpImageConvert::convert(m_I, cur); } @@ -553,22 +557,26 @@ void vpMbKltTracker::setPose(const vpImage *const I, const vpImag if (!useOgre) { if (I) { faces.setVisible(I->getWidth(), I->getHeight(), m_cam, cdMo, angleAppears, angleDisappears, reInitialisation); - } else { + } + else { faces.setVisible(m_I.getWidth(), m_I.getHeight(), m_cam, cdMo, angleAppears, angleDisappears, reInitialisation); } - } else { + } + else { #ifdef VISP_HAVE_OGRE if (I) { faces.setVisibleOgre(I->getWidth(), I->getHeight(), m_cam, cdMo, angleAppears, angleDisappears, reInitialisation); - } else { + } + else { faces.setVisibleOgre(m_I.getWidth(), m_I.getHeight(), m_cam, cdMo, angleAppears, angleDisappears, reInitialisation); } #else if (I) { faces.setVisible(I->getWidth(), I->getHeight(), m_cam, cdMo, angleAppears, angleDisappears, reInitialisation); - } else { + } + else { faces.setVisible(m_I.getWidth(), m_I.getHeight(), m_cam, cdMo, angleAppears, angleDisappears, reInitialisation); } #endif @@ -748,7 +756,8 @@ bool vpMbKltTracker::postTracking(const vpImage &I, vpColVector & // std::cout << "Too many point disappear : " << initialNumber << "/" // << currentNumber << std::endl; reInitialisation = true; - } else { + } + else { if (!useOgre) faces.setVisible(I.getWidth(), I.getHeight(), m_cam, m_cMo, angleAppears, angleDisappears, reInitialisation); else { @@ -873,7 +882,8 @@ void vpMbKltTracker::computeVVSInteractionMatrixAndResidu() try { kltpoly->computeHomography(ctTc0, H); kltpoly->computeInteractionMatrixAndResidu(subR, subL); - } catch (...) { + } + catch (...) { throw vpTrackingException(vpTrackingException::fatalError, "Cannot compute interaction matrix"); } @@ -891,7 +901,8 @@ void vpMbKltTracker::computeVVSInteractionMatrixAndResidu() try { kltPolyCylinder->computeInteractionMatrixAndResidu(ctTc0, subR, subL); - } catch (...) { + } + catch (...) { throw vpTrackingException(vpTrackingException::fatalError, "Cannot compute interaction matrix"); } @@ -992,7 +1003,8 @@ void vpMbKltTracker::track(const vpImage &I_color) */ void vpMbKltTracker::loadConfigFile(const std::string &configFile, bool verbose) { - // Load projection error config +#if defined(VISP_HAVE_PUGIXML) +// Load projection error config vpMbTracker::loadConfigFile(configFile, verbose); vpMbtXmlGenericParser xmlp(vpMbtXmlGenericParser::KLT_PARSER); @@ -1013,7 +1025,8 @@ void vpMbKltTracker::loadConfigFile(const std::string &configFile, bool verbose) std::cout << " *********** Parsing XML for MBT KLT Tracker ************ " << std::endl; } xmlp.parse(configFile.c_str()); - } catch (...) { + } + catch (...) { vpERROR_TRACE("Can't open XML file \"%s\"\n ", configFile.c_str()); throw vpException(vpException::ioError, "problem to parse configuration file."); } @@ -1056,6 +1069,11 @@ void vpMbKltTracker::loadConfigFile(const std::string &configFile, bool verbose) setMinLineLengthThresh(minLineLengthThresholdGeneral); setMinPolygonAreaThresh(minPolygonAreaThresholdGeneral); } +#else + (void)configFile; + (void)verbose; + throw(vpException(vpException::ioError, "vpMbKltTracker::loadConfigFile() needs pugixml built-in 3rdparty")); +#endif } /*! @@ -1074,14 +1092,15 @@ void vpMbKltTracker::display(const vpImage &I, const vpHomogeneou bool displayFullModel) { std::vector > models = - vpMbKltTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel); + vpMbKltTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel); for (size_t i = 0; i < models.size(); i++) { if (vpMath::equal(models[i][0], 0)) { vpImagePoint ip1(models[i][1], models[i][2]); vpImagePoint ip2(models[i][3], models[i][4]); vpDisplay::displayLine(I, ip1, ip2, col, thickness); - } else if (vpMath::equal(models[i][0], 1)) { + } + else if (vpMath::equal(models[i][0], 1)) { vpImagePoint center(models[i][1], models[i][2]); double n20 = models[i][3]; double n11 = models[i][4]; @@ -1126,14 +1145,15 @@ void vpMbKltTracker::display(const vpImage &I, const vpHomogeneousMatrix const vpColor &col, unsigned int thickness, bool displayFullModel) { std::vector > models = - vpMbKltTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel); + vpMbKltTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel); for (size_t i = 0; i < models.size(); i++) { if (vpMath::equal(models[i][0], 0)) { vpImagePoint ip1(models[i][1], models[i][2]); vpImagePoint ip2(models[i][3], models[i][4]); vpDisplay::displayLine(I, ip1, ip2, col, thickness); - } else if (vpMath::equal(models[i][0], 1)) { + } + else if (vpMath::equal(models[i][0], 1)) { vpImagePoint center(models[i][1], models[i][2]); double n20 = models[i][3]; double n11 = models[i][4]; @@ -1443,5 +1463,5 @@ void vpMbKltTracker::setUseKltTracking(const std::string &name, const bool &useK #elif !defined(VISP_BUILD_SHARED_LIBS) // Work around to avoid warning: libvisp_mbt.a(vpMbKltTracker.cpp.o) has no // symbols -void dummy_vpMbKltTracker(){}; +void dummy_vpMbKltTracker() { }; #endif // VISP_HAVE_OPENCV diff --git a/modules/tracker/mbt/src/vpMbGenericTracker.cpp b/modules/tracker/mbt/src/vpMbGenericTracker.cpp index f15d2e2b03..230e39ed0e 100644 --- a/modules/tracker/mbt/src/vpMbGenericTracker.cpp +++ b/modules/tracker/mbt/src/vpMbGenericTracker.cpp @@ -6687,6 +6687,7 @@ void vpMbGenericTracker::TrackerWrapper::initMbtTracking(const vpImage #include -#include +#include +#if defined(VISP_HAVE_SIMDLIB) +#include +#endif #include #include @@ -2865,8 +2868,19 @@ void vpMbTracker::computeJTR(const vpMatrix &interaction, const vpColVector &err } JTR.resize(6, false); - +#if defined(VISP_HAVE_SIMDLIB) SimdComputeJtR(interaction.data, interaction.getRows(), error.data, JTR.data); +#else + const unsigned int N = interaction.getRows(); + + for (unsigned int i = 0; i < 6; i += 1) { + double ssum = 0; + for (unsigned int j = 0; j < N; j += 1) { + ssum += interaction[j][i] * error[j]; + } + JTR[i] = ssum; + } +#endif } void vpMbTracker::computeVVSCheckLevenbergMarquardt(unsigned int iter, vpColVector &error, @@ -3765,6 +3779,7 @@ void vpMbTracker::projectionErrorInitMovingEdge(const vpImage &I, void vpMbTracker::loadConfigFile(const std::string &configFile, bool verbose) { +#if defined(VISP_HAVE_PUGIXML) vpMbtXmlGenericParser xmlp(vpMbtXmlGenericParser::PROJECTION_ERROR_PARSER); xmlp.setVerbose(verbose); xmlp.setProjectionErrorMe(m_projectionErrorMe); @@ -3785,6 +3800,11 @@ void vpMbTracker::loadConfigFile(const std::string &configFile, bool verbose) setProjectionErrorMovingEdge(meParser); setProjectionErrorKernelSize(xmlp.getProjectionErrorKernelSize()); +#else + (void)configFile; + (void)verbose; + throw(vpException(vpException::ioError, "vpMbTracker::loadConfigFile() needs pugixml built-in 3rdparty")); +#endif } /*! diff --git a/modules/tracker/mbt/src/vpMbtXmlGenericParser.cpp b/modules/tracker/mbt/src/vpMbtXmlGenericParser.cpp index c3707d7735..faf8cee7a3 100644 --- a/modules/tracker/mbt/src/vpMbtXmlGenericParser.cpp +++ b/modules/tracker/mbt/src/vpMbtXmlGenericParser.cpp @@ -40,6 +40,7 @@ #include +#if defined(VISP_HAVE_PUGIXML) #include #ifndef DOXYGEN_SHOULD_SKIP_THIS @@ -1915,3 +1916,9 @@ void vpMbtXmlGenericParser::setProjectionErrorKernelSize(const unsigned int &siz \param verbose : verbose flag */ void vpMbtXmlGenericParser::setVerbose(bool verbose) { m_impl->setVerbose(verbose); } + +#elif !defined(VISP_BUILD_SHARED_LIBS) +// Work around to avoid warning: libvisp_core.a(vpMbtXmlGenericParser.cpp.o) has no symbols +void dummy_vpMbtXmlGenericParser() { }; + +#endif diff --git a/modules/tracker/mbt/test/generic-with-dataset/perfGenericTracker.cpp b/modules/tracker/mbt/test/generic-with-dataset/perfGenericTracker.cpp index da5bb48cb8..94c13ca1a5 100644 --- a/modules/tracker/mbt/test/generic-with-dataset/perfGenericTracker.cpp +++ b/modules/tracker/mbt/test/generic-with-dataset/perfGenericTracker.cpp @@ -129,12 +129,65 @@ TEST_CASE("Benchmark generic tracker", "[benchmark]") const std::string input_directory = vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "mbt-depth/Castle-simu"); + + const bool verbose = false; +#if defined(VISP_HAVE_PUGIXML) const std::string configFileCam1 = input_directory + std::string("/Config/chateau.xml"); const std::string configFileCam2 = input_directory + std::string("/Config/chateau_depth.xml"); REQUIRE(vpIoTools::checkFilename(configFileCam1)); REQUIRE(vpIoTools::checkFilename(configFileCam2)); - const bool verbose = false; tracker.loadConfigFile(configFileCam1, configFileCam2, verbose); +#else + { + vpCameraParameters cam_color, cam_depth; + cam_color.initPersProjWithoutDistortion(700.0, 700.0, 320.0, 240.0); + cam_depth.initPersProjWithoutDistortion(700.0, 700.0, 320.0, 240.0); + tracker.setCameraParameters(cam_color, cam_depth); + } + + // Edge + vpMe me; + me.setMaskSize(5); + me.setMaskNumber(180); + me.setRange(8); + me.setLikelihoodThresholdType(vpMe::NORMALIZED_THRESHOLD); + me.setThreshold(5); + me.setMu1(0.5); + me.setMu2(0.5); + me.setSampleStep(5); + tracker.setMovingEdge(me); + + // Klt +#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO) + vpKltOpencv klt; + tracker.setKltMaskBorder(5); + klt.setMaxFeatures(10000); + klt.setWindowSize(5); + klt.setQuality(0.01); + klt.setMinDistance(5); + klt.setHarrisFreeParameter(0.02); + klt.setBlockSize(3); + klt.setPyramidLevels(3); + + tracker.setKltOpencv(klt); +#endif + + // Depth + tracker.setDepthNormalFeatureEstimationMethod(vpMbtFaceDepthNormal::ROBUST_FEATURE_ESTIMATION); + tracker.setDepthNormalPclPlaneEstimationMethod(2); + tracker.setDepthNormalPclPlaneEstimationRansacMaxIter(200); + tracker.setDepthNormalPclPlaneEstimationRansacThreshold(0.001); + tracker.setDepthNormalSamplingStep(2, 2); + + tracker.setDepthDenseSamplingStep(4, 4); + + tracker.setAngleAppear(vpMath::rad(85.0)); + tracker.setAngleDisappear(vpMath::rad(89.0)); + tracker.setNearClippingDistance(0.01); + tracker.setFarClippingDistance(2.0); + tracker.setClipping(tracker.getClipping() | vpMbtPolygon::FOV_CLIPPING); +#endif + REQUIRE(vpIoTools::checkFilename(input_directory + "/Models/chateau.cao")); tracker.loadModel(input_directory + "/Models/chateau.cao", input_directory + "/Models/chateau.cao", verbose); @@ -226,7 +279,58 @@ TEST_CASE("Benchmark generic tracker", "[benchmark]") tracker.setTrackerType(mapOfTrackerTypes[idx]); const bool verbose = false; +#if defined(VISP_HAVE_PUGIXML) tracker.loadConfigFile(configFileCam1, configFileCam2, verbose); +#else + { + vpCameraParameters cam_color, cam_depth; + cam_color.initPersProjWithoutDistortion(700.0, 700.0, 320.0, 240.0); + cam_depth.initPersProjWithoutDistortion(700.0, 700.0, 320.0, 240.0); + tracker.setCameraParameters(cam_color, cam_depth); + } + + // Edge + vpMe me; + me.setMaskSize(5); + me.setMaskNumber(180); + me.setRange(8); + me.setLikelihoodThresholdType(vpMe::NORMALIZED_THRESHOLD); + me.setThreshold(5); + me.setMu1(0.5); + me.setMu2(0.5); + me.setSampleStep(5); + tracker.setMovingEdge(me); + + // Klt +#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO) + vpKltOpencv klt; + tracker.setKltMaskBorder(5); + klt.setMaxFeatures(10000); + klt.setWindowSize(5); + klt.setQuality(0.01); + klt.setMinDistance(5); + klt.setHarrisFreeParameter(0.02); + klt.setBlockSize(3); + klt.setPyramidLevels(3); + + tracker.setKltOpencv(klt); +#endif + + // Depth + tracker.setDepthNormalFeatureEstimationMethod(vpMbtFaceDepthNormal::ROBUST_FEATURE_ESTIMATION); + tracker.setDepthNormalPclPlaneEstimationMethod(2); + tracker.setDepthNormalPclPlaneEstimationRansacMaxIter(200); + tracker.setDepthNormalPclPlaneEstimationRansacThreshold(0.001); + tracker.setDepthNormalSamplingStep(2, 2); + + tracker.setDepthDenseSamplingStep(4, 4); + + tracker.setAngleAppear(vpMath::rad(85.0)); + tracker.setAngleDisappear(vpMath::rad(89.0)); + tracker.setNearClippingDistance(0.01); + tracker.setFarClippingDistance(2.0); + tracker.setClipping(tracker.getClipping() | vpMbtPolygon::FOV_CLIPPING); +#endif tracker.loadModel(input_directory + "/Models/chateau.cao", input_directory + "/Models/chateau.cao", verbose); tracker.loadModel(input_directory + "/Models/cube.cao", verbose, T); tracker.initFromPose(images.front(), cMo_truth_all.front()); diff --git a/modules/tracker/mbt/test/generic-with-dataset/testGenericTracker.cpp b/modules/tracker/mbt/test/generic-with-dataset/testGenericTracker.cpp index 03273fbce1..03087c034b 100644 --- a/modules/tracker/mbt/test/generic-with-dataset/testGenericTracker.cpp +++ b/modules/tracker/mbt/test/generic-with-dataset/testGenericTracker.cpp @@ -275,12 +275,14 @@ bool run(const std::string &input_directory, bool opt_click_allowed, bool opt_di tracker_type[0] = trackerType_image; tracker_type[1] = vpMbGenericTracker::DEPTH_DENSE_TRACKER; vpMbGenericTracker tracker(tracker_type); + +#if defined(VISP_HAVE_PUGIXML) std::string configFileCam1 = input_directory + std::string("/Config/chateau.xml"); std::string configFileCam2 = input_directory + std::string("/Config/chateau_depth.xml"); std::cout << "Load config file for camera 1: " << configFileCam1 << std::endl; std::cout << "Load config file for camera 2: " << configFileCam2 << std::endl; tracker.loadConfigFile(configFileCam1, configFileCam2); -#if 0 +#else // Corresponding parameters manually set to have an example code { vpCameraParameters cam_color, cam_depth; diff --git a/modules/tracker/mbt/test/generic-with-dataset/testGenericTrackerDepth.cpp b/modules/tracker/mbt/test/generic-with-dataset/testGenericTrackerDepth.cpp index 23af518084..234eeebc29 100644 --- a/modules/tracker/mbt/test/generic-with-dataset/testGenericTrackerDepth.cpp +++ b/modules/tracker/mbt/test/generic-with-dataset/testGenericTrackerDepth.cpp @@ -248,8 +248,10 @@ bool run(vpImage &I, const std::string &input_directory, bool opt_click_al std::vector tracker_type; tracker_type.push_back(vpMbGenericTracker::DEPTH_DENSE_TRACKER); vpMbGenericTracker tracker(tracker_type); + +#if defined(VISP_HAVE_PUGIXML) tracker.loadConfigFile(input_directory + "/Config/chateau_depth.xml"); -#if 0 +#else // Corresponding parameters manually set to have an example code { vpCameraParameters cam_depth; diff --git a/modules/tracker/mbt/test/generic-with-dataset/testGenericTrackerDeterminist.cpp b/modules/tracker/mbt/test/generic-with-dataset/testGenericTrackerDeterminist.cpp index a6f303aaf8..3e23e77981 100644 --- a/modules/tracker/mbt/test/generic-with-dataset/testGenericTrackerDeterminist.cpp +++ b/modules/tracker/mbt/test/generic-with-dataset/testGenericTrackerDeterminist.cpp @@ -89,8 +89,48 @@ void configureTracker(vpMbGenericTracker &tracker, vpCameraParameters &cam) const std::string env_ipath = vpIoTools::getViSPImagesDataPath(); const std::string configFile = vpIoTools::createFilePath(env_ipath, "mbt/cube.xml"); const std::string modelFile = vpIoTools::createFilePath(env_ipath, "mbt/cube_and_cylinder.cao"); +#if defined(VISP_HAVE_PUGIXML) const bool verbose = false; tracker.loadConfigFile(configFile, verbose); +#else + // Corresponding parameters manually set to have an example code + // By setting the parameters: + cam.initPersProjWithoutDistortion(547, 542, 338, 234); + + vpMe me; + me.setMaskSize(5); + me.setMaskNumber(180); + me.setRange(7); + me.setLikelihoodThresholdType(vpMe::NORMALIZED_THRESHOLD); + me.setThreshold(5); + me.setMu1(0.5); + me.setMu2(0.5); + me.setSampleStep(4); + + vpKltOpencv klt; + klt.setMaxFeatures(300); + klt.setWindowSize(5); + klt.setQuality(0.01); + klt.setMinDistance(5); + klt.setHarrisFreeParameter(0.01); + klt.setBlockSize(3); + klt.setPyramidLevels(3); + + tracker.setCameraParameters(cam); + tracker.setMovingEdge(me); + tracker.setKltOpencv(klt); + tracker.setKltMaskBorder(5); + tracker.setAngleAppear(vpMath::rad(65)); + tracker.setAngleDisappear(vpMath::rad(75)); + + // Specify the clipping to + tracker.setNearClippingDistance(0.01); + tracker.setFarClippingDistance(0.90); + tracker.setClipping(tracker.getClipping() | vpMbtPolygon::FOV_CLIPPING); + // tracker.setClipping(tracker.getClipping() | vpMbtPolygon::LEFT_CLIPPING | + // vpMbtPolygon::RIGHT_CLIPPING | vpMbtPolygon::UP_CLIPPING | + // vpMbtPolygon::DOWN_CLIPPING); // Equivalent to FOV_CLIPPING +#endif tracker.getCameraParameters(cam); tracker.loadModel(modelFile); tracker.setDisplayFeatures(true); diff --git a/modules/tracker/mbt/test/generic-with-dataset/testMbtXmlGenericParser.cpp b/modules/tracker/mbt/test/generic-with-dataset/testMbtXmlGenericParser.cpp index d541fbace8..c78650c1bd 100644 --- a/modules/tracker/mbt/test/generic-with-dataset/testMbtXmlGenericParser.cpp +++ b/modules/tracker/mbt/test/generic-with-dataset/testMbtXmlGenericParser.cpp @@ -44,7 +44,7 @@ int main() { -#if defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV) +#if defined(VISP_HAVE_PUGIXML) && (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV)) std::string visp_images_dir = vpIoTools::getViSPImagesDataPath(); if (vpIoTools::checkDirectory(visp_images_dir + "/xml")) { double eps = std::numeric_limits::epsilon(); @@ -159,6 +159,8 @@ int main() } #elif !(defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV)) std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl; +#elif !(defined(VISP_HAVE_PUGIXML)) + std::cout << "Cannot run this example: enable pugixml built-in" << std::endl; #endif std::cout << "Test succeed" << std::endl; diff --git a/modules/vision/include/visp3/vision/vpHomography.h b/modules/vision/include/visp3/vision/vpHomography.h index f7285cf638..2f23e92eba 100644 --- a/modules/vision/include/visp3/vision/vpHomography.h +++ b/modules/vision/include/visp3/vision/vpHomography.h @@ -635,11 +635,11 @@ class VISP_EXPORT vpHomography : public vpArray2D static bool degenerateConfiguration(const vpColVector &x, unsigned int *ind, double threshold_area); static bool degenerateConfiguration(const std::vector &xb, const std::vector &yb, const std::vector &xa, const std::vector &ya); - static void HartleyNormalization(unsigned int n, const double *x, const double *y, double *xn, double *yn, double &xg, + static void hartleyNormalization(unsigned int n, const double *x, const double *y, double *xn, double *yn, double &xg, double &yg, double &coef); - static void HartleyNormalization(const std::vector &x, const std::vector &y, std::vector &xn, + static void hartleyNormalization(const std::vector &x, const std::vector &y, std::vector &xn, std::vector &yn, double &xg, double &yg, double &coef); - static void HartleyDenormalization(vpHomography &aHbn, vpHomography &aHb, double xg1, double yg1, double coef1, + static void hartleyDenormalization(vpHomography &aHbn, vpHomography &aHb, double xg1, double yg1, double coef1, double xg2, double yg2, double coef2); #endif // DOXYGEN_SHOULD_SKIP_THIS diff --git a/modules/vision/include/visp3/vision/vpXmlConfigParserKeyPoint.h b/modules/vision/include/visp3/vision/vpXmlConfigParserKeyPoint.h index e039258dbb..eaf7e67cfb 100644 --- a/modules/vision/include/visp3/vision/vpXmlConfigParserKeyPoint.h +++ b/modules/vision/include/visp3/vision/vpXmlConfigParserKeyPoint.h @@ -44,6 +44,7 @@ #include +#if defined(VISP_HAVE_PUGIXML) #include /*! @@ -198,3 +199,4 @@ class VISP_EXPORT vpXmlConfigParserKeyPoint Impl *m_impl; }; #endif +#endif diff --git a/modules/vision/src/homography-estimation/vpHomography.cpp b/modules/vision/src/homography-estimation/vpHomography.cpp index ea60522fea..493db75ef9 100644 --- a/modules/vision/src/homography-estimation/vpHomography.cpp +++ b/modules/vision/src/homography-estimation/vpHomography.cpp @@ -99,7 +99,8 @@ void vpHomography::buildFrom(const vpRotationMatrix &aRb, const vpTranslationVec void vpHomography::buildFrom(const vpPoseVector &arb, const vpPlane &p) { - m_aMb.buildFrom(arb[0], arb[1], arb[2], arb[3], arb[4], arb[5]); + double tx = arb[0], ty = arb[1], tz = arb[2], tux = arb[3], tuy = arb[4], tuz = arb[5]; + m_aMb.buildFrom(tx, ty, tz, tux, tuy, tuz); insert(p); build(); } @@ -130,11 +131,13 @@ vpHomography vpHomography::inverse(double sv_threshold, unsigned int *rank) cons } vpHomography H; - - for (unsigned int i = 0; i < 3; i++) - for (unsigned int j = 0; j < 3; j++) + unsigned int nbRows = H.getRows(); + unsigned int nbCols = H.getCols(); + for (unsigned int i = 0; i < nbRows; ++i) { + for (unsigned int j = 0; j < nbCols; ++j) { H[i][j] = Minv[i][j]; - + } + } return H; } @@ -153,10 +156,11 @@ void vpHomography::save(std::ofstream &f) const vpHomography vpHomography::operator*(const vpHomography &H) const { vpHomography Hp; - for (unsigned int i = 0; i < 3; i++) { - for (unsigned int j = 0; j < 3; j++) { + const unsigned int nbCols = 3, nbRows = 3; + for (unsigned int i = 0; i < nbRows; ++i) { + for (unsigned int j = 0; j < nbCols; ++j) { double s = 0.; - for (unsigned int k = 0; k < 3; k++) { + for (unsigned int k = 0; k < nbCols; ++k) { s += (*this)[i][k] * H[k][j]; } Hp[i][j] = s; @@ -167,15 +171,18 @@ vpHomography vpHomography::operator*(const vpHomography &H) const vpColVector vpHomography::operator*(const vpColVector &b) const { - if (b.size() != 3) + const unsigned int requiredSize = 3; + if (b.size() != requiredSize) { throw(vpException(vpException::dimensionError, "Cannot multiply an homography by a vector of dimension %d", b.size())); + } - vpColVector a(3); - for (unsigned int i = 0; i < 3; i++) { + vpColVector a(requiredSize); + for (unsigned int i = 0; i < requiredSize; ++i) { a[i] = 0.; - for (unsigned int j = 0; j < 3; j++) + for (unsigned int j = 0; j < requiredSize; ++j) { a[i] += (*this)[i][j] * b[j]; + } } return a; @@ -183,9 +190,10 @@ vpColVector vpHomography::operator*(const vpColVector &b) const vpHomography vpHomography::operator*(const double &v) const { + const unsigned int nbData = 9; // cols x rows = 3 x 3 vpHomography H; - for (unsigned int i = 0; i < 9; i++) { + for (unsigned int i = 0; i < nbData; ++i) { H.data[i] = this->data[i] * v; } @@ -201,11 +209,11 @@ vpPoint vpHomography::operator*(const vpPoint &b_P) const v[1] = b_P.get_y(); v[2] = b_P.get_w(); - v1[0] = (*this)[0][0] * v[0] + (*this)[0][1] * v[1] + (*this)[0][2] * v[2]; - v1[1] = (*this)[1][0] * v[0] + (*this)[1][1] * v[1] + (*this)[1][2] * v[2]; - v1[2] = (*this)[2][0] * v[0] + (*this)[2][1] * v[1] + (*this)[2][2] * v[2]; + v1[0] = ((*this)[0][0] * v[0]) + ((*this)[0][1] * v[1]) + ((*this)[0][2] * v[2]); + v1[1] = ((*this)[1][0] * v[0]) + ((*this)[1][1] * v[1]) + ((*this)[1][2] * v[2]); + v1[2] = ((*this)[2][0] * v[0]) + ((*this)[2][1] * v[1]) + ((*this)[2][2] * v[2]); - // v1 = M*v ; + // v1 is equal to M v ; a_P.set_x(v1[0]); a_P.set_y(v1[1]); a_P.set_w(v1[2]); @@ -216,12 +224,14 @@ vpPoint vpHomography::operator*(const vpPoint &b_P) const vpHomography vpHomography::operator/(const double &v) const { vpHomography H; - if (std::fabs(v) <= std::numeric_limits::epsilon()) + if (std::fabs(v) <= std::numeric_limits::epsilon()) { throw vpMatrixException(vpMatrixException::divideByZeroError, "Divide by zero in method /=(double v)"); + } double vinv = 1 / v; - for (unsigned int i = 0; i < 9; i++) { + const unsigned int nbData = 9; // cols x rows = 3 x 3 + for (unsigned int i = 0; i < nbData; ++i) { H.data[i] = this->data[i] * vinv; } @@ -230,23 +240,28 @@ vpHomography vpHomography::operator/(const double &v) const vpHomography &vpHomography::operator/=(double v) { - // if (x == 0) - if (std::fabs(v) <= std::numeric_limits::epsilon()) + if (std::fabs(v) <= std::numeric_limits::epsilon()) { throw vpMatrixException(vpMatrixException::divideByZeroError, "Divide by zero in method /=(double v)"); + } double vinv = 1 / v; - for (unsigned int i = 0; i < 9; i++) + const unsigned int nbData = 9; // cols x rows = 3 x 3 + for (unsigned int i = 0; i < nbData; ++i) { data[i] *= vinv; + } return *this; } vpHomography &vpHomography::operator=(const vpHomography &H) { - for (unsigned int i = 0; i < 3; i++) - for (unsigned int j = 0; j < 3; j++) + const unsigned int nbCols = 3, nbRows = 3; + for (unsigned int i = 0; i < nbRows; ++i){ + for (unsigned int j = 0; j < nbCols; ++j) { (*this)[i][j] = H[i][j]; + } + } m_aMb = H.m_aMb; m_bP = H.m_bP; @@ -255,23 +270,29 @@ vpHomography &vpHomography::operator=(const vpHomography &H) vpHomography &vpHomography::operator=(const vpMatrix &H) { - if (H.getRows() != 3 || H.getCols() != 3) + const unsigned int nbCols = 3, nbRows = 3; + if ((H.getRows() != nbRows) || (H.getCols() != nbCols)) { throw(vpException(vpException::dimensionError, "The matrix is not an homography")); + } - for (unsigned int i = 0; i < 3; i++) - for (unsigned int j = 0; j < 3; j++) + for (unsigned int i = 0; i < nbRows; ++i) { + for (unsigned int j = 0; j < nbCols; ++j) { (*this)[i][j] = H[i][j]; + } + } return *this; } void vpHomography::load(std::ifstream &f) { + const unsigned int nbCols = 3, nbRows = 3; if (!f.fail()) { - for (unsigned int i = 0; i < 3; i++) - for (unsigned int j = 0; j < 3; j++) { + for (unsigned int i = 0; i < nbRows; ++i) { + for (unsigned int j = 0; j < nbCols; ++j) { f >> (*this)[i][j]; } + } } else { throw(vpException(vpException::ioError, "Cannot read the homography from the input stream")); @@ -287,25 +308,29 @@ void vpHomography::load(std::ifstream &f) */ void vpHomography::build() { - vpColVector n(3); - vpColVector atb(3); - vpMatrix aRb(3, 3); - for (unsigned int i = 0; i < 3; i++) { + const unsigned int nbCols = 3, nbRows = 3; + vpColVector n(nbRows); + vpColVector atb(nbRows); + vpMatrix aRb(nbRows, nbCols); + for (unsigned int i = 0; i < nbRows; ++i) { atb[i] = m_aMb[i][3]; - for (unsigned int j = 0; j < 3; j++) + for (unsigned int j = 0; j < nbCols; ++j) { aRb[i][j] = m_aMb[i][j]; + } } m_bP.getNormal(n); double d = m_bP.getD(); - vpMatrix aHb = aRb - atb * n.t() / d; // the d used in the equation is such as nX=d is the + vpMatrix aHb = aRb - ((atb * n.t()) / d); // the d used in the equation is such as nX=d is the // plane equation. So if the plane is described by // Ax+By+Cz+D=0, d=-D - for (unsigned int i = 0; i < 3; i++) - for (unsigned int j = 0; j < 3; j++) + for (unsigned int i = 0; i < nbRows; ++i) { + for (unsigned int j = 0; j < nbCols; ++j) { (*this)[i][j] = aHb[i][j]; + } + } } /*! @@ -318,42 +343,51 @@ void vpHomography::build() */ void vpHomography::build(vpHomography &aHb, const vpHomogeneousMatrix &aMb, const vpPlane &bP) { - vpColVector n(3); - vpColVector atb(3); - vpMatrix aRb(3, 3); - for (unsigned int i = 0; i < 3; i++) { + const unsigned int nbCols = 3, nbRows = 3; + vpColVector n(nbRows); + vpColVector atb(nbRows); + vpMatrix aRb(nbRows, nbCols); + for (unsigned int i = 0; i < nbRows; ++i) { atb[i] = aMb[i][3]; - for (unsigned int j = 0; j < 3; j++) + for (unsigned int j = 0; j < nbCols; ++j) { aRb[i][j] = aMb[i][j]; + } } bP.getNormal(n); double d = bP.getD(); - vpMatrix aHb_ = aRb - atb * n.t() / d; // the d used in the equation is such as nX=d is the + vpMatrix aHb_ = aRb - ((atb * n.t()) / d); // the d used in the equation is such as nX=d is the // plane equation. So if the plane is described by // Ax+By+Cz+D=0, d=-D - for (unsigned int i = 0; i < 3; i++) - for (unsigned int j = 0; j < 3; j++) + for (unsigned int i = 0; i < nbRows; ++i) { + for (unsigned int j = 0; j < nbCols; ++j) { aHb[i][j] = aHb_[i][j]; + } + } } double vpHomography::det() const { - return ((*this)[0][0] * ((*this)[1][1] * (*this)[2][2] - (*this)[1][2] * (*this)[2][1]) - - (*this)[0][1] * ((*this)[1][0] * (*this)[2][2] - (*this)[1][2] * (*this)[2][0]) + - (*this)[0][2] * ((*this)[1][0] * (*this)[2][1] - (*this)[1][1] * (*this)[2][0])); + return ((*this)[0][0] * (((*this)[1][1] * (*this)[2][2]) - ((*this)[1][2] * (*this)[2][1])) - + (*this)[0][1] * (((*this)[1][0] * (*this)[2][2]) - ((*this)[1][2] * (*this)[2][0])) + + (*this)[0][2] * (((*this)[1][0] * (*this)[2][1]) - ((*this)[1][1] * (*this)[2][0]))); } void vpHomography::eye() { - for (unsigned int i = 0; i < 3; i++) - for (unsigned int j = 0; j < 3; j++) - if (i == j) + const unsigned int nbCols = 3, nbRows = 3; + for (unsigned int i = 0; i < nbRows; ++i) { + for (unsigned int j = 0; j < nbCols; ++j) { + if (i == j) { (*this)[i][j] = 1.0; - else + } + else { (*this)[i][j] = 0.0; + } + } + } } vpImagePoint vpHomography::project(const vpCameraParameters &cam, const vpHomography &bHa, const vpImagePoint &iPa) @@ -361,9 +395,9 @@ vpImagePoint vpHomography::project(const vpCameraParameters &cam, const vpHomogr double xa = iPa.get_u(); double ya = iPa.get_v(); vpHomography bGa = bHa.homography2collineation(cam); - double z = xa * bGa[2][0] + ya * bGa[2][1] + bGa[2][2]; - double xb = (xa * bGa[0][0] + ya * bGa[0][1] + bGa[0][2]) / z; - double yb = (xa * bGa[1][0] + ya * bGa[1][1] + bGa[1][2]) / z; + double z = (xa * bGa[2][0]) + (ya * bGa[2][1]) + bGa[2][2]; + double xb = ((xa * bGa[0][0]) + (ya * bGa[0][1]) + bGa[0][2]) / z; + double yb = ((xa * bGa[1][0]) + (ya * bGa[1][1]) + bGa[1][2]) / z; vpImagePoint iPb(yb, xb); @@ -374,9 +408,9 @@ vpPoint vpHomography::project(const vpHomography &bHa, const vpPoint &Pa) { double xa = Pa.get_x(); double ya = Pa.get_y(); - double z = xa * bHa[2][0] + ya * bHa[2][1] + bHa[2][2]; - double xb = (xa * bHa[0][0] + ya * bHa[0][1] + bHa[0][2]) / z; - double yb = (xa * bHa[1][0] + ya * bHa[1][1] + bHa[1][2]) / z; + double z = (xa * bHa[2][0]) + (ya * bHa[2][1]) + bHa[2][2]; + double xb = ((xa * bHa[0][0]) + (ya * bHa[0][1]) + bHa[0][2]) / z; + double yb = ((xa * bHa[1][0]) + (ya * bHa[1][1]) + bHa[1][2]) / z; vpPoint Pb; Pb.set_x(xb); @@ -390,13 +424,16 @@ void vpHomography::robust(const std::vector &xb, const std::vector &ya, vpHomography &aHb, std::vector &inliers, double &residual, double weights_threshold, unsigned int niter, bool normalization) { - unsigned int n = (unsigned int)xb.size(); - if (yb.size() != n || xa.size() != n || ya.size() != n) + unsigned int n = static_cast(xb.size()); + if ((yb.size() != n) || (xa.size() != n) || (ya.size() != n)) { throw(vpException(vpException::dimensionError, "Bad dimension for robust homography estimation")); + } // 4 point are required - if (n < 4) + const unsigned int nbRequiredPoints = 4; + if (n < nbRequiredPoints) { throw(vpException(vpException::fatalError, "There must be at least 4 matched points")); + } try { std::vector xan, yan, xbn, ybn; @@ -406,8 +443,8 @@ void vpHomography::robust(const std::vector &xb, const std::vector &xb, const std::vector &xb, const std::vector &xb, const std::vector &xb, const std::vector std::numeric_limits::epsilon()) { ipa.set_u(u_a / w_a); @@ -545,10 +584,13 @@ vpImagePoint vpHomography::projection(const vpImagePoint &ipb) vpMatrix vpHomography::convert() const { - vpMatrix M(3, 3); - for (unsigned int i = 0; i < 3; i++) - for (unsigned int j = 0; j < 3; j++) + const unsigned int nbRows = 3, nbCols = 3; + vpMatrix M(nbRows, nbCols); + for (unsigned int i = 0; i < nbRows; ++i) { + for (unsigned int j = 0; j < nbCols; ++j) { M[i][j] = (*this)[i][j]; + } + } return M; } @@ -569,12 +611,12 @@ vpHomography vpHomography::collineation2homography(const vpCameraParameters &cam double u0_one_over_px = u0 * one_over_px; double v0_one_over_py = v0 * one_over_py; - double A = h00 * one_over_px - h20 * u0_one_over_px; - double B = h01 * one_over_px - h21 * u0_one_over_px; - double C = h02 * one_over_px - h22 * u0_one_over_px; - double D = h10 * one_over_py - h20 * v0_one_over_py; - double E = h11 * one_over_py - h21 * v0_one_over_py; - double F = h12 * one_over_py - h22 * v0_one_over_py; + double A = (h00 * one_over_px) - (h20 * u0_one_over_px); + double B = (h01 * one_over_px) - (h21 * u0_one_over_px); + double C = (h02 * one_over_px) - (h22 * u0_one_over_px); + double D = (h10 * one_over_py) - (h20 * v0_one_over_py); + double E = (h11 * one_over_py) - (h21 * v0_one_over_py); + double F = (h12 * one_over_py) - (h22 * v0_one_over_py); H[0][0] = A * px; H[1][0] = D * px; @@ -584,9 +626,9 @@ vpHomography vpHomography::collineation2homography(const vpCameraParameters &cam H[1][1] = E * py; H[2][1] = h21 * py; - H[0][2] = A * u0 + B * v0 + C; - H[1][2] = D * u0 + E * v0 + F; - H[2][2] = h20 * u0 + h21 * v0 + h22; + H[0][2] = (A * u0) + (B * v0) + C; + H[1][2] = (D * u0) + (E * v0) + F; + H[2][2] = (h20 * u0) + (h21 * v0) + h22; return H; } @@ -604,12 +646,12 @@ vpHomography vpHomography::homography2collineation(const vpCameraParameters &cam double h10 = (*this)[1][0], h11 = (*this)[1][1], h12 = (*this)[1][2]; double h20 = (*this)[2][0], h21 = (*this)[2][1], h22 = (*this)[2][2]; - double A = h00 * px + u0 * h20; - double B = h01 * px + u0 * h21; - double C = h02 * px + u0 * h22; - double D = h10 * py + v0 * h20; - double E = h11 * py + v0 * h21; - double F = h12 * py + v0 * h22; + double A = (h00 * px) + (u0 * h20); + double B = (h01 * px) + (u0 * h21); + double C = (h02 * px) + (u0 * h22); + double D = (h10 * py) + (v0 * h20); + double E = (h11 * py) + (v0 * h21); + double F = (h12 * py) + (v0 * h22); H[0][0] = A * one_over_px; H[1][0] = D * one_over_px; @@ -622,9 +664,9 @@ vpHomography vpHomography::homography2collineation(const vpCameraParameters &cam double u0_one_over_px = u0 * one_over_px; double v0_one_over_py = v0 * one_over_py; - H[0][2] = -A * u0_one_over_px - B * v0_one_over_py + C; - H[1][2] = -D * u0_one_over_px - E * v0_one_over_py + F; - H[2][2] = -h20 * u0_one_over_px - h21 * v0_one_over_py + h22; + H[0][2] = ((-A * u0_one_over_px) - (B * v0_one_over_py)) + C; + H[1][2] = ((-D * u0_one_over_px) - (E * v0_one_over_py)) + F; + H[2][2] = ((-h20 * u0_one_over_px) - (h21 * v0_one_over_py)) + h22; return H; } diff --git a/modules/vision/src/homography-estimation/vpHomographyDLT.cpp b/modules/vision/src/homography-estimation/vpHomographyDLT.cpp index ca551c3fbc..7025c9f5c5 100644 --- a/modules/vision/src/homography-estimation/vpHomographyDLT.cpp +++ b/modules/vision/src/homography-estimation/vpHomographyDLT.cpp @@ -31,12 +31,12 @@ * Homography estimation. */ -/*! - \file vpHomographyDLT.cpp + /*! + \file vpHomographyDLT.cpp - This file implements the functions related with the homography - estimation using the DLT algorithm -*/ + This file implements the functions related with the homography + estimation using the DLT algorithm + */ #include #include @@ -47,24 +47,27 @@ #ifndef DOXYGEN_SHOULD_SKIP_THIS -void vpHomography::HartleyNormalization(const std::vector &x, const std::vector &y, +void vpHomography::hartleyNormalization(const std::vector &x, const std::vector &y, std::vector &xn, std::vector &yn, double &xg, double &yg, double &coef) { - if (x.size() != y.size()) + if (x.size() != y.size()) { throw(vpException(vpException::dimensionError, "Hartley normalization require that x and y vector " - "have the same dimension")); + "have the same dimension")); + } - unsigned int n = (unsigned int)x.size(); - if (xn.size() != n) + unsigned int n = static_cast(x.size()); + if (xn.size() != n) { xn.resize(n); - if (yn.size() != n) + } + if (yn.size() != n) { yn.resize(n); + } xg = 0; yg = 0; - for (unsigned int i = 0; i < n; i++) { + for (unsigned int i = 0; i < n; ++i) { xg += x[i]; yg += y[i]; } @@ -74,7 +77,7 @@ void vpHomography::HartleyNormalization(const std::vector &x, const std: // Changement d'origine : le centre de gravite doit correspondre // a l'origine des coordonnees double distance = 0; - for (unsigned int i = 0; i < n; i++) { + for (unsigned int i = 0; i < n; ++i) { double xni = x[i] - xg; double yni = y[i] - yg; xn[i] = xni; @@ -86,25 +89,27 @@ void vpHomography::HartleyNormalization(const std::vector &x, const std: distance /= n; // calcul du coef de changement d'echelle // if(distance ==0) - if (std::fabs(distance) <= std::numeric_limits::epsilon()) + if (std::fabs(distance) <= std::numeric_limits::epsilon()) { coef = 1; - else + } + else { coef = sqrt(2.0) / distance; + } - for (unsigned int i = 0; i < n; i++) { + for (unsigned int i = 0; i < n; ++i) { xn[i] *= coef; yn[i] *= coef; } } -void vpHomography::HartleyNormalization(unsigned int n, const double *x, const double *y, double *xn, double *yn, +void vpHomography::hartleyNormalization(unsigned int n, const double *x, const double *y, double *xn, double *yn, double &xg, double &yg, double &coef) { unsigned int i; xg = 0; yg = 0; - for (i = 0; i < n; i++) { + for (i = 0; i < n; ++i) { xg += x[i]; yg += y[i]; } @@ -114,7 +119,7 @@ void vpHomography::HartleyNormalization(unsigned int n, const double *x, const d // Changement d'origine : le centre de gravite doit correspondre // a l'origine des coordonnees double distance = 0; - for (i = 0; i < n; i++) { + for (i = 0; i < n; ++i) { double xni = x[i] - xg; double yni = y[i] - yg; xn[i] = xni; @@ -126,12 +131,14 @@ void vpHomography::HartleyNormalization(unsigned int n, const double *x, const d distance /= n; // calcul du coef de changement d'echelle // if(distance ==0) - if (std::fabs(distance) <= std::numeric_limits::epsilon()) + if (std::fabs(distance) <= std::numeric_limits::epsilon()) { coef = 1; - else + } + else { coef = sqrt(2.0) / distance; + } - for (i = 0; i < n; i++) { + for (i = 0; i < n; ++i) { xn[i] *= coef; yn[i] *= coef; } @@ -139,7 +146,7 @@ void vpHomography::HartleyNormalization(unsigned int n, const double *x, const d //--------------------------------------------------------------------------------------- -void vpHomography::HartleyDenormalization(vpHomography &aHbn, vpHomography &aHb, double xg1, double yg1, double coef1, +void vpHomography::hartleyDenormalization(vpHomography &aHbn, vpHomography &aHb, double xg1, double yg1, double coef1, double xg2, double yg2, double coef2) { @@ -154,26 +161,30 @@ void vpHomography::HartleyDenormalization(vpHomography &aHbn, vpHomography &aHb, T2.eye(); T2T.eye(); - T1[0][0] = T1[1][1] = coef1; + T1[0][0] = (T1[1][1] = coef1); T1[0][2] = -coef1 * xg1; T1[1][2] = -coef1 * yg1; - T2[0][0] = T2[1][1] = coef2; + T2[0][0] = (T2[1][1] = coef2); T2[0][2] = -coef2 * xg2; T2[1][2] = -coef2 * yg2; T2T = T2.pseudoInverse(1e-16); vpMatrix aHbn_(3, 3); - for (unsigned int i = 0; i < 3; i++) - for (unsigned int j = 0; j < 3; j++) + for (unsigned int i = 0; i < 3; ++i) { + for (unsigned int j = 0; j < 3; ++j) { aHbn_[i][j] = aHbn[i][j]; + } + } vpMatrix maHb = T2T * aHbn_ * T1; - for (unsigned int i = 0; i < 3; i++) - for (unsigned int j = 0; j < 3; j++) + for (unsigned int i = 0; i < 3; ++i) { + for (unsigned int j = 0; j < 3; ++j) { aHb[i][j] = maHb[i][j]; + } + } } #endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS @@ -181,119 +192,119 @@ void vpHomography::HartleyDenormalization(vpHomography &aHbn, vpHomography &aHb, void vpHomography::DLT(const std::vector &xb, const std::vector &yb, const std::vector &xa, const std::vector &ya, vpHomography &aHb, bool normalization) { - unsigned int n = (unsigned int)xb.size(); - if (yb.size() != n || xa.size() != n || ya.size() != n) + unsigned int n = static_cast(xb.size()); + if ((yb.size() != n) || (xa.size() != n) || (ya.size() != n)) { throw(vpException(vpException::dimensionError, "Bad dimension for DLT homography estimation")); + } // 4 point are required - if (n < 4) + const unsigned int nbRequiredPoints = 4; + if (n < nbRequiredPoints) { throw(vpException(vpException::fatalError, "There must be at least 4 matched points")); + } - try { - std::vector xan, yan, xbn, ybn; + std::vector xan, yan, xbn, ybn; - double xg1 = 0., yg1 = 0., coef1 = 0., xg2 = 0., yg2 = 0., coef2 = 0.; + double xg1 = 0., yg1 = 0., coef1 = 0., xg2 = 0., yg2 = 0., coef2 = 0.; - vpHomography aHbn; + vpHomography aHbn; - if (normalization) { - vpHomography::HartleyNormalization(xb, yb, xbn, ybn, xg1, yg1, coef1); - vpHomography::HartleyNormalization(xa, ya, xan, yan, xg2, yg2, coef2); - } else { - xbn = xb; - ybn = yb; - xan = xa; - yan = ya; - } + if (normalization) { + vpHomography::hartleyNormalization(xb, yb, xbn, ybn, xg1, yg1, coef1); + vpHomography::hartleyNormalization(xa, ya, xan, yan, xg2, yg2, coef2); + } + else { + xbn = xb; + ybn = yb; + xan = xa; + yan = ya; + } - vpMatrix A(2 * n, 9); - vpColVector h(9); - vpColVector D(9); - vpMatrix V(9, 9); - - // We need here to compute the SVD on a (n*2)*9 matrix (where n is - // the number of points). if n == 4, the matrix has more columns - // than rows. This kind of matrix is not supported by GSL for - // SVD. The solution is to add an extra line with zeros - if (n == 4) - A.resize(2 * n + 1, 9); - - // build matrix A - for (unsigned int i = 0; i < n; i++) { - A[2 * i][0] = 0; - A[2 * i][1] = 0; - A[2 * i][2] = 0; - A[2 * i][3] = -xbn[i]; - A[2 * i][4] = -ybn[i]; - A[2 * i][5] = -1; - A[2 * i][6] = xbn[i] * yan[i]; - A[2 * i][7] = ybn[i] * yan[i]; - A[2 * i][8] = yan[i]; - - A[2 * i + 1][0] = xbn[i]; - A[2 * i + 1][1] = ybn[i]; - A[2 * i + 1][2] = 1; - A[2 * i + 1][3] = 0; - A[2 * i + 1][4] = 0; - A[2 * i + 1][5] = 0; - A[2 * i + 1][6] = -xbn[i] * xan[i]; - A[2 * i + 1][7] = -ybn[i] * xan[i]; - A[2 * i + 1][8] = -xan[i]; - } + vpMatrix A(2 * n, 9); + vpColVector h(9); + vpColVector D(9); + vpMatrix V(9, 9); + + // We need here to compute the SVD on a (n*2)*9 matrix (where n is + // the number of points). if n == 4, the matrix has more columns + // than rows. This kind of matrix is not supported by GSL for + // SVD. The solution is to add an extra line with zeros + if (n == 4) { + A.resize((2 * n) + 1, 9); + } - // Add an extra line with zero. - if (n == 4) { - for (unsigned int i = 0; i < 9; i++) { - A[2 * n][i] = 0; - } + // build matrix A + for (unsigned int i = 0; i < n; ++i) { + A[2 * i][0] = 0; + A[2 * i][1] = 0; + A[2 * i][2] = 0; + A[2 * i][3] = -xbn[i]; + A[2 * i][4] = -ybn[i]; + A[2 * i][5] = -1; + A[2 * i][6] = xbn[i] * yan[i]; + A[2 * i][7] = ybn[i] * yan[i]; + A[2 * i][8] = yan[i]; + + A[(2 * i) + 1][0] = xbn[i]; + A[(2 * i) + 1][1] = ybn[i]; + A[(2 * i) + 1][2] = 1; + A[(2 * i) + 1][3] = 0; + A[(2 * i) + 1][4] = 0; + A[(2 * i) + 1][5] = 0; + A[(2 * i) + 1][6] = -xbn[i] * xan[i]; + A[(2 * i) + 1][7] = -ybn[i] * xan[i]; + A[(2 * i) + 1][8] = -xan[i]; + } + + // Add an extra line with zero. + if (n == 4) { + for (unsigned int i = 0; i < 9; ++i) { + A[2 * n][i] = 0; } + } - // solve Ah = 0 - // SVD Decomposition A = UDV^T (destructive wrt A) - A.svd(D, V); - - // on en profite pour effectuer un controle sur le rang de la matrice : - // pas plus de 2 valeurs singulieres quasi=0 - int rank = 0; - for (unsigned int i = 0; i < 9; i++) - if (D[i] > 1e-7) - rank++; - if (rank < 7) { - throw(vpMatrixException(vpMatrixException::rankDeficient, "Matrix rank %d is deficient (should be 8)", rank)); + // solve Ah = 0 + // SVD Decomposition A = UDV^T (destructive wrt A) + A.svd(D, V); + + // on en profite pour effectuer un controle sur le rang de la matrice : + // pas plus de 2 valeurs singulieres quasi=0 + int rank = 0; + for (unsigned int i = 0; i < 9; ++i) { + if (D[i] > 1e-7) { + ++rank; } - // h = is the column of V associated with the smallest singular value of A - - // since we are not sure that the svd implemented sort the - // singular value... we seek for the smallest - double smallestSv = 1e30; - unsigned int indexSmallestSv = 0; - for (unsigned int i = 0; i < 9; i++) - if ((D[i] < smallestSv)) { - smallestSv = D[i]; - indexSmallestSv = i; - } - - h = V.getCol(indexSmallestSv); - - // build the homography - for (unsigned int i = 0; i < 3; i++) { - for (unsigned int j = 0; j < 3; j++) - aHbn[i][j] = h[3 * i + j]; + } + if (rank < 7) { + throw(vpMatrixException(vpMatrixException::rankDeficient, "Matrix rank %d is deficient (should be 8)", rank)); + } + // h = is the column of V associated with the smallest singular value of A + + // since we are not sure that the svd implemented sort the + // singular value... we seek for the smallest + double smallestSv = 1e30; + unsigned int indexSmallestSv = 0; + for (unsigned int i = 0; i < 9; ++i) { + if (D[i] < smallestSv) { + smallestSv = D[i]; + indexSmallestSv = i; } + } - if (normalization) { - // H after denormalization - vpHomography::HartleyDenormalization(aHbn, aHb, xg1, yg1, coef1, xg2, yg2, coef2); - } else { - aHb = aHbn; + h = V.getCol(indexSmallestSv); + + // build the homography + for (unsigned int i = 0; i < 3; ++i) { + for (unsigned int j = 0; j < 3; ++j) { + aHbn[i][j] = h[(3 * i) + j]; } + } - } catch (vpMatrixException &me) { - vpTRACE("Matrix Exception "); - throw(me); - } catch (const vpException &me) { - vpERROR_TRACE("caught another error"); - std::cout << std::endl << me << std::endl; - throw(me); + if (normalization) { + // H after denormalization + vpHomography::hartleyDenormalization(aHbn, aHb, xg1, yg1, coef1, xg2, yg2, coef2); + } + else { + aHb = aHbn; } } diff --git a/modules/vision/src/homography-estimation/vpHomographyExtract.cpp b/modules/vision/src/homography-estimation/vpHomographyExtract.cpp index 5f52272eee..3af2843954 100644 --- a/modules/vision/src/homography-estimation/vpHomographyExtract.cpp +++ b/modules/vision/src/homography-estimation/vpHomographyExtract.cpp @@ -97,8 +97,6 @@ void vpHomography::computeDisplacement(const vpHomography &aHb, const vpColVecto double s, determinantU, determinantV; unsigned int vOrdre[3]; - // vpColVector normaleDesiree(3) ; - // normaleDesiree[0]=0;normaleDesiree[1]=0;normaleDesiree[2]=1; vpColVector normaleDesiree(nd); /**** Corps de la focntion ****/ @@ -114,9 +112,11 @@ void vpHomography::computeDisplacement(const vpHomography &aHb, const vpColVecto vpMatrix aRbp(3, 3); vpMatrix mH(3, 3); - for (unsigned int i = 0; i < 3; i++) - for (unsigned int j = 0; j < 3; j++) + for (unsigned int i = 0; i < 3; ++i) { + for (unsigned int j = 0; j < 3; ++j) { mH[i][j] = aHb[i][j]; + } + } /* Preparation au calcul de la SVD */ mTempU = mH; @@ -170,9 +170,9 @@ void vpHomography::computeDisplacement(const vpHomography &aHb, const vpColVecto hypothese : sv[0]>=sv[1]>=sv[2]>=0 *****/ - for (unsigned int i = 0; i < 3; i++) { + for (unsigned int i = 0; i < 3; ++i) { sv[i] = svTemp[vOrdre[i]]; - for (unsigned int j = 0; j < 3; j++) { + for (unsigned int j = 0; j < 3; ++j) { mU[i][j] = mTempU[i][vOrdre[j]]; mV[i][j] = mTempV[i][vOrdre[j]]; } @@ -196,8 +196,9 @@ void vpHomography::computeDisplacement(const vpHomography &aHb, const vpColVecto #ifdef DEBUG_Homographie printf("s = det(U) * det(V) = %f * %f = %f\n", determinantU, determinantV, s); #endif - if (s < 0) + if (s < 0) { mV *= -1; + } /* d' = d2 */ distanceFictive = sv[1]; @@ -206,10 +207,7 @@ void vpHomography::computeDisplacement(const vpHomography &aHb, const vpColVecto #endif n.resize(3); - if (((sv[0] - sv[1]) < m_sing_threshold) && (sv[0] - sv[2]) < m_sing_threshold) { - //#ifdef DEBUG_Homographie - // printf ("\nPure rotation\n"); - //#endif + if (((sv[0] - sv[1]) < m_sing_threshold) && ((sv[0] - sv[2]) < m_sing_threshold)) { /***** Cas ou le deplacement est une rotation pure et la normale reste indefini @@ -238,9 +236,9 @@ void vpHomography::computeDisplacement(const vpHomography &aHb, const vpColVecto Calcul de la normale au plan : n' = [ esp1*x1 , x2=0 , esp3*x3 ] dans l'ordre : cas (esp1=+1, esp3=+1) et (esp1=-1, esp3=+1) *****/ - mX[0][0] = sqrt((sv[0] * sv[0] - sv[1] * sv[1]) / (sv[0] * sv[0] - sv[2] * sv[2])); + mX[0][0] = sqrt(((sv[0] * sv[0]) - (sv[1] * sv[1])) / ((sv[0] * sv[0]) - (sv[2] * sv[2]))); mX[1][0] = 0.0; - mX[2][0] = sqrt((sv[1] * sv[1] - sv[2] * sv[2]) / (sv[0] * sv[0] - sv[2] * sv[2])); + mX[2][0] = sqrt(((sv[1] * sv[1]) - (sv[2] * sv[2])) / ((sv[0] * sv[0]) - (sv[2] * sv[2]))); mX[0][1] = -mX[0][0]; mX[1][1] = mX[1][0]; @@ -248,21 +246,22 @@ void vpHomography::computeDisplacement(const vpHomography &aHb, const vpColVecto /* Il y a 4 solutions pour n : 2 par cas => n1, -n1, n2, -n2 */ double cosinusAncien = 0.0; - for (unsigned int w = 0; w < 2; w++) { /* Pour les 2 cas */ - for (unsigned int k = 0; k < 2; k++) { /* Pour le signe */ + for (unsigned int w = 0; w < 2; ++w) { /* Pour les 2 cas */ + for (unsigned int k = 0; k < 2; ++k) { /* Pour le signe */ /* Calcul de la normale estimee : n = V.n' */ - for (unsigned int i = 0; i < 3; i++) { + for (unsigned int i = 0; i < 3; ++i) { normaleEstimee[i] = 0.0; - for (unsigned int j = 0; j < 3; j++) { - normaleEstimee[i] += (2.0 * k - 1.0) * mV[i][j] * mX[j][w]; + for (unsigned int j = 0; j < 3; ++j) { + normaleEstimee[i] += ((2.0 * k) - 1.0) * mV[i][j] * mX[j][w]; } } /* Calcul du cosinus de l'angle entre la normale reelle et desire */ double cosinusDesireeEstimee = 0.0; - for (unsigned int i = 0; i < 3; i++) + for (unsigned int i = 0; i < 3; ++i) { cosinusDesireeEstimee += normaleEstimee[i] * normaleDesiree[i]; + } /***** Si la solution est meilleur @@ -272,28 +271,31 @@ void vpHomography::computeDisplacement(const vpHomography &aHb, const vpColVecto cosinusAncien = cosinusDesireeEstimee; /* Affectation de la normale qui est retourner */ - for (unsigned int j = 0; j < 3; j++) + for (unsigned int j = 0; j < 3; ++j) { n[j] = normaleEstimee[j]; + } /* Construction du vecteur t'= +/- (d1-d3).[x1, 0, -x3] */ - aTbp[0] = (2.0 * k - 1.0) * (sv[0] - sv[2]) * mX[0][w]; - aTbp[1] = (2.0 * k - 1.0) * (sv[0] - sv[2]) * mX[1][w]; - aTbp[2] = -(2.0 * k - 1.0) * (sv[0] - sv[2]) * mX[2][w]; + aTbp[0] = ((2.0 * k) - 1.0) * (sv[0] - sv[2]) * mX[0][w]; + aTbp[1] = ((2.0 * k) - 1.0) * (sv[0] - sv[2]) * mX[1][w]; + aTbp[2] = -((2.0 * k) - 1.0) * (sv[0] - sv[2]) * mX[2][w]; /* si c'est la deuxieme solution */ - if (w == 1) + if (w == 1) { signeSinus = -1; /* car esp1*esp3 = -1 */ - else + } + else { signeSinus = 1; + } } /* fin if (cosinusDesireeEstimee > cosinusAncien) */ } /* fin k */ } /* fin w */ } /* fin else */ /* Calcul du vecteur de translation qui est retourner : t = (U * t') / d */ - for (unsigned int i = 0; i < 3; i++) { + for (unsigned int i = 0; i < 3; ++i) { atb[i] = 0.0; - for (unsigned int j = 0; j < 3; j++) { + for (unsigned int j = 0; j < 3; ++j) { atb[i] += mU[i][j] * aTbp[j]; } atb[i] /= distanceFictive; @@ -316,9 +318,9 @@ void vpHomography::computeDisplacement(const vpHomography &aHb, const vpColVecto de la solution retenue (cf. ci-dessous) *****/ sinusTheta = - signeSinus * sqrt((sv[0] * sv[0] - sv[1] * sv[1]) * (sv[1] * sv[1] - sv[2] * sv[2])) / ((sv[0] + sv[2]) * sv[1]); + (signeSinus * sqrt(((sv[0] * sv[0]) - (sv[1] * sv[1])) * ((sv[1] * sv[1]) - (sv[2] * sv[2])))) / ((sv[0] + sv[2]) * sv[1]); - cosinusTheta = (sv[1] * sv[1] + sv[0] * sv[2]) / ((sv[0] + sv[2]) * sv[1]); + cosinusTheta = ((sv[1] * sv[1]) + (sv[0] * sv[2])) / ((sv[0] + sv[2]) * sv[1]); /* construction de la matrice de rotation R' */ aRbp[0][0] = cosinusTheta; @@ -332,25 +334,25 @@ void vpHomography::computeDisplacement(const vpHomography &aHb, const vpColVecto aRbp[2][2] = cosinusTheta; /* multiplication Rint = U R' */ - for (unsigned int i = 0; i < 3; i++) { - for (unsigned int j = 0; j < 3; j++) { + for (unsigned int i = 0; i < 3; ++i) { + for (unsigned int j = 0; j < 3; ++j) { aRbint[i][j] = 0.0; - for (unsigned int k = 0; k < 3; k++) { + for (unsigned int k = 0; k < 3; ++k) { aRbint[i][j] += mU[i][k] * aRbp[k][j]; } } } /* multiplication R = Rint . V^T */ - for (unsigned int i = 0; i < 3; i++) { - for (unsigned int j = 0; j < 3; j++) { + for (unsigned int i = 0; i < 3; ++i) { + for (unsigned int j = 0; j < 3; ++j) { aRb[i][j] = 0.0; - for (unsigned int k = 0; k < 3; k++) { + for (unsigned int k = 0; k < 3; ++k) { aRb[i][j] += aRbint[i][k] * mV[j][k]; } } } -/*transpose_carre(aRb,3); */ + #ifdef DEBUG_Homographie printf("R : %d\n", aRb.isARotationMatrix()); std::cout << aRb << std::endl; @@ -407,9 +409,11 @@ void vpHomography::computeDisplacement(const vpHomography &aHb, vpRotationMatrix vpMatrix aRbp(3, 3); vpMatrix mH(3, 3); - for (unsigned int i = 0; i < 3; i++) - for (unsigned int j = 0; j < 3; j++) + for (unsigned int i = 0; i < 3; ++i) { + for (unsigned int j = 0; j < 3; ++j) { mH[i][j] = aHb[i][j]; + } + } /* Preparation au calcul de la SVD */ mTempU = mH; @@ -463,9 +467,9 @@ void vpHomography::computeDisplacement(const vpHomography &aHb, vpRotationMatrix hypothese : sv[0]>=sv[1]>=sv[2]>=0 *****/ - for (unsigned int i = 0; i < 3; i++) { + for (unsigned int i = 0; i < 3; ++i) { sv[i] = svTemp[vOrdre[i]]; - for (unsigned int j = 0; j < 3; j++) { + for (unsigned int j = 0; j < 3; ++j) { mU[i][j] = mTempU[i][vOrdre[j]]; mV[i][j] = mTempV[i][vOrdre[j]]; } @@ -489,8 +493,9 @@ void vpHomography::computeDisplacement(const vpHomography &aHb, vpRotationMatrix #ifdef DEBUG_Homographie printf("s = det(U) * det(V) = %f * %f = %f\n", determinantU, determinantV, s); #endif - if (s < 0) + if (s < 0) { mV *= -1; + } /* d' = d2 */ distanceFictive = sv[1]; @@ -499,10 +504,7 @@ void vpHomography::computeDisplacement(const vpHomography &aHb, vpRotationMatrix #endif n.resize(3); - if (((sv[0] - sv[1]) < m_sing_threshold) && (sv[0] - sv[2]) < m_sing_threshold) { - //#ifdef DEBUG_Homographie - // printf ("\nPure rotation\n"); - //#endif + if (((sv[0] - sv[1]) < m_sing_threshold) && ((sv[0] - sv[2]) < m_sing_threshold)) { /***** Cas ou le deplacement est une rotation pure et la normale reste indefini @@ -531,9 +533,9 @@ void vpHomography::computeDisplacement(const vpHomography &aHb, vpRotationMatrix Calcul de la normale au plan : n' = [ esp1*x1 , x2=0 , esp3*x3 ] dans l'ordre : cas (esp1=+1, esp3=+1) et (esp1=-1, esp3=+1) *****/ - mX[0][0] = sqrt((sv[0] * sv[0] - sv[1] * sv[1]) / (sv[0] * sv[0] - sv[2] * sv[2])); + mX[0][0] = sqrt(((sv[0] * sv[0]) - (sv[1] * sv[1])) / ((sv[0] * sv[0]) - (sv[2] * sv[2]))); mX[1][0] = 0.0; - mX[2][0] = sqrt((sv[1] * sv[1] - sv[2] * sv[2]) / (sv[0] * sv[0] - sv[2] * sv[2])); + mX[2][0] = sqrt(((sv[1] * sv[1]) - (sv[2] * sv[2])) / ((sv[0] * sv[0]) - (sv[2] * sv[2]))); mX[0][1] = -mX[0][0]; mX[1][1] = mX[1][0]; @@ -541,21 +543,22 @@ void vpHomography::computeDisplacement(const vpHomography &aHb, vpRotationMatrix /* Il y a 4 solutions pour n : 2 par cas => n1, -n1, n2, -n2 */ double cosinusAncien = 0.0; - for (unsigned int w = 0; w < 2; w++) { /* Pour les 2 cas */ - for (unsigned int k = 0; k < 2; k++) { /* Pour le signe */ + for (unsigned int w = 0; w < 2; ++w) { /* Pour les 2 cas */ + for (unsigned int k = 0; k < 2; ++k) { /* Pour le signe */ /* Calcul de la normale estimee : n = V.n' */ - for (unsigned int i = 0; i < 3; i++) { + for (unsigned int i = 0; i < 3; ++i) { normaleEstimee[i] = 0.0; - for (unsigned int j = 0; j < 3; j++) { - normaleEstimee[i] += (2.0 * k - 1.0) * mV[i][j] * mX[j][w]; + for (unsigned int j = 0; j < 3; ++j) { + normaleEstimee[i] += ((2.0 * k )- 1.0) * mV[i][j] * mX[j][w]; } } /* Calcul du cosinus de l'angle entre la normale reelle et desire */ double cosinusDesireeEstimee = 0.0; - for (unsigned int i = 0; i < 3; i++) + for (unsigned int i = 0; i < 3; ++i) { cosinusDesireeEstimee += normaleEstimee[i] * normaleDesiree[i]; + } /***** Si la solution est meilleur @@ -565,28 +568,31 @@ void vpHomography::computeDisplacement(const vpHomography &aHb, vpRotationMatrix cosinusAncien = cosinusDesireeEstimee; /* Affectation de la normale qui est retourner */ - for (unsigned int j = 0; j < 3; j++) + for (unsigned int j = 0; j < 3; ++j) { n[j] = normaleEstimee[j]; + } /* Construction du vecteur t'= +/- (d1-d3).[x1, 0, -x3] */ - aTbp[0] = (2.0 * k - 1.0) * (sv[0] - sv[2]) * mX[0][w]; - aTbp[1] = (2.0 * k - 1.0) * (sv[0] - sv[2]) * mX[1][w]; - aTbp[2] = -(2.0 * k - 1.0) * (sv[0] - sv[2]) * mX[2][w]; + aTbp[0] = ((2.0 * k) - 1.0) * (sv[0] - sv[2]) * mX[0][w]; + aTbp[1] = ((2.0 * k) - 1.0) * (sv[0] - sv[2]) * mX[1][w]; + aTbp[2] = -((2.0 * k) - 1.0) * (sv[0] - sv[2]) * mX[2][w]; /* si c'est la deuxieme solution */ - if (w == 1) + if (w == 1) { signeSinus = -1; /* car esp1*esp3 = -1 */ - else + } + else { signeSinus = 1; + } } /* fin if (cosinusDesireeEstimee > cosinusAncien) */ } /* fin k */ } /* fin w */ } /* fin else */ /* Calcul du vecteur de translation qui est retourner : t = (U * t') / d */ - for (unsigned int i = 0; i < 3; i++) { + for (unsigned int i = 0; i < 3; ++i) { atb[i] = 0.0; - for (unsigned int j = 0; j < 3; j++) { + for (unsigned int j = 0; j < 3; ++j) { atb[i] += mU[i][j] * aTbp[j]; } atb[i] /= distanceFictive; @@ -609,9 +615,9 @@ void vpHomography::computeDisplacement(const vpHomography &aHb, vpRotationMatrix de la solution retenue (cf. ci-dessous) *****/ sinusTheta = - signeSinus * sqrt((sv[0] * sv[0] - sv[1] * sv[1]) * (sv[1] * sv[1] - sv[2] * sv[2])) / ((sv[0] + sv[2]) * sv[1]); + (signeSinus * sqrt(((sv[0] * sv[0]) - (sv[1] * sv[1])) * ((sv[1] * sv[1]) - (sv[2] * sv[2])))) / ((sv[0] + sv[2]) * sv[1]); - cosinusTheta = (sv[1] * sv[1] + sv[0] * sv[2]) / ((sv[0] + sv[2]) * sv[1]); + cosinusTheta = ((sv[1] * sv[1]) + (sv[0] * sv[2])) / ((sv[0] + sv[2]) * sv[1]); /* construction de la matrice de rotation R' */ aRbp[0][0] = cosinusTheta; @@ -625,25 +631,25 @@ void vpHomography::computeDisplacement(const vpHomography &aHb, vpRotationMatrix aRbp[2][2] = cosinusTheta; /* multiplication Rint = U R' */ - for (unsigned int i = 0; i < 3; i++) { - for (unsigned int j = 0; j < 3; j++) { + for (unsigned int i = 0; i < 3; ++i) { + for (unsigned int j = 0; j < 3; ++j) { aRbint[i][j] = 0.0; - for (unsigned int k = 0; k < 3; k++) { + for (unsigned int k = 0; k < 3; ++k) { aRbint[i][j] += mU[i][k] * aRbp[k][j]; } } } /* multiplication R = Rint . V^T */ - for (unsigned int i = 0; i < 3; i++) { - for (unsigned int j = 0; j < 3; j++) { + for (unsigned int i = 0; i < 3; ++i) { + for (unsigned int j = 0; j < 3; ++j) { aRb[i][j] = 0.0; - for (unsigned int k = 0; k < 3; k++) { + for (unsigned int k = 0; k < 3; ++k) { aRb[i][j] += aRbint[i][k] * mV[j][k]; } } } -/*transpose_carre(aRb,3); */ + #ifdef DEBUG_Homographie printf("R : %d\n", aRb.isARotationMatrix()); std::cout << aRb << std::endl; @@ -739,9 +745,9 @@ void vpHomography::computeDisplacement(const vpHomography &H, double x, double y en fonction des valeurs singulieres car hypothese : sv[0]>=sv[1]>=sv[2]>=0 *****/ - for (unsigned int i = 0; i < 3; i++) { + for (unsigned int i = 0; i < 3; ++i) { sv[i] = svTemp[vOrdre[i]]; - for (unsigned int j = 0; j < 3; j++) { + for (unsigned int j = 0; j < 3; ++j) { U[i][j] = mTempU[i][vOrdre[j]]; V[i][j] = mTempV[i][vOrdre[j]]; } @@ -757,11 +763,11 @@ void vpHomography::computeDisplacement(const vpHomography &H, double x, double y #endif // calcul du determinant de U et V - determinantU = U[0][0] * (U[1][1] * U[2][2] - U[1][2] * U[2][1]) + U[0][1] * (U[1][2] * U[2][0] - U[1][0] * U[2][2]) + - U[0][2] * (U[1][0] * U[2][1] - U[1][1] * U[2][0]); + determinantU = (U[0][0] * ((U[1][1] * U[2][2]) - (U[1][2] * U[2][1]))) + (U[0][1] * ((U[1][2] * U[2][0]) - (U[1][0] * U[2][2]))) + + (U[0][2] * ((U[1][0] * U[2][1]) - (U[1][1] * U[2][0]))); - determinantV = V[0][0] * (V[1][1] * V[2][2] - V[1][2] * V[2][1]) + V[0][1] * (V[1][2] * V[2][0] - V[1][0] * V[2][2]) + - V[0][2] * (V[1][0] * V[2][1] - V[1][1] * V[2][0]); + determinantV = (V[0][0] * ((V[1][1] * V[2][2]) - (V[1][2] * V[2][1]))) + (V[0][1] * ((V[1][2] * V[2][0]) - (V[1][0] * V[2][2]))) + + (V[0][2] * ((V[1][0] * V[2][1]) - (V[1][1] * V[2][0]))); s = determinantU * determinantV; @@ -779,12 +785,14 @@ void vpHomography::computeDisplacement(const vpHomography &H, double x, double y // (h_31x_1 + h_32x_2 + h_33)/d > 0 // et d = sd' - tmp = H[2][0] * x + H[2][1] * y + H[2][2]; + tmp = (H[2][0] * x) + (H[2][1] * y) + H[2][2]; - if ((tmp / (sv[1] * s)) > 0) + if ((tmp / (sv[1] * s)) > 0) { distanceFictive = sv[1]; - else + } + else { distanceFictive = -sv[1]; + } // il faut ensuite considerer l'ordre de multiplicite de chaque variable // 1er cas : d1<>d2<>d3 @@ -793,15 +801,19 @@ void vpHomography::computeDisplacement(const vpHomography &H, double x, double y // 4eme cas : d1 =d2=d3 if ((sv[0] - sv[1]) < m_sing_threshold) { - if ((sv[1] - sv[2]) < m_sing_threshold) + if ((sv[1] - sv[2]) < m_sing_threshold) { cas = cas4; - else + } + else { cas = cas2; + } } else { - if ((sv[1] - sv[2]) < m_sing_threshold) + if ((sv[1] - sv[2]) < m_sing_threshold) { cas = cas3; - else + } + else { cas = cas1; + } } Nprim1 = 0.0; @@ -825,9 +837,9 @@ void vpHomography::computeDisplacement(const vpHomography &H, double x, double y if (cas == cas1) { // quatre normales sont possibles - Nprim1[0] = sqrt((sv[0] * sv[0] - sv[1] * sv[1]) / (sv[0] * sv[0] - sv[2] * sv[2])); + Nprim1[0] = sqrt(((sv[0] * sv[0]) - (sv[1] * sv[1])) / ((sv[0] * sv[0]) - (sv[2] * sv[2]))); - Nprim1[2] = sqrt((sv[1] * sv[1] - sv[2] * sv[2]) / (sv[0] * sv[0] - sv[2] * sv[2])); + Nprim1[2] = sqrt(((sv[1] * sv[1]) - (sv[2] * sv[2])) / ((sv[0] * sv[0]) - (sv[2] * sv[2]))); Nprim2[0] = Nprim1[0]; Nprim2[2] = -Nprim1[2]; @@ -859,46 +871,46 @@ void vpHomography::computeDisplacement(const vpHomography &H, double x, double y if (cas == cas1) { N1 = V * Nprim1; - tmp = N1[0] * x + N1[1] * y + N1[2]; + tmp = (N1[0] * x) + (N1[1] * y) + N1[2]; tmp /= (distanceFictive * s); norm1ok = (tmp > 0); N2 = V * Nprim2; - tmp = N2[0] * x + N2[1] * y + N2[2]; + tmp = (N2[0] * x) + (N2[1] * y) + N2[2]; tmp /= (distanceFictive * s); norm2ok = (tmp > 0); N3 = V * Nprim3; - tmp = N3[0] * x + N3[1] * y + N3[2]; + tmp = (N3[0] * x) + (N3[1] * y) + N3[2]; tmp /= (distanceFictive * s); norm3ok = (tmp > 0); N4 = V * Nprim4; - tmp = N4[0] * x + N4[1] * y + N4[2]; + tmp = (N4[0] * x) + (N4[1] * y) + N4[2]; tmp /= (distanceFictive * s); norm4ok = (tmp > 0); } if (cas == cas2) { N1 = V * Nprim1; - tmp = N1[0] * x + N1[1] * y + N1[2]; + tmp = (N1[0] * x) + (N1[1] * y) + N1[2]; tmp /= (distanceFictive * s); norm1ok = (tmp > 0); N2 = V * Nprim2; - tmp = N2[0] * x + N2[1] * y + N2[2]; + tmp = (N2[0] * x) + (N2[1] * y) + N2[2]; tmp /= (distanceFictive * s); norm2ok = (tmp > 0); } if (cas == cas3) { N1 = V * Nprim1; - tmp = N1[0] * x + N1[1] * y + N1[2]; + tmp = (N1[0] * x) + (N1[1] * y) + N1[2]; tmp /= (distanceFictive * s); norm1ok = (tmp > 0); N2 = V * Nprim2; - tmp = N2[0] * x + N2[1] * y + N2[2]; + tmp = (N2[0] * x) + (N2[1] * y) + N2[2]; tmp /= (distanceFictive * s); norm2ok = (tmp > 0); } @@ -912,7 +924,7 @@ void vpHomography::computeDisplacement(const vpHomography &H, double x, double y if (norm1ok) { // cos theta - Rprim1[0][0] = (vpMath::sqr(sv[1]) + sv[0] * sv[2]) / ((sv[0] + sv[2]) * sv[1]); + Rprim1[0][0] = (vpMath::sqr(sv[1]) + (sv[0] * sv[2])) / ((sv[0] + sv[2]) * sv[1]); Rprim1[2][2] = Rprim1[0][0]; @@ -934,7 +946,7 @@ void vpHomography::computeDisplacement(const vpHomography &H, double x, double y if (norm2ok) { // cos theta - Rprim2[0][0] = (vpMath::sqr(sv[1]) + sv[0] * sv[2]) / ((sv[0] + sv[2]) * sv[1]); + Rprim2[0][0] = (vpMath::sqr(sv[1]) + (sv[0] * sv[2])) / ((sv[0] + sv[2]) * sv[1]); Rprim2[2][2] = Rprim2[0][0]; @@ -956,7 +968,7 @@ void vpHomography::computeDisplacement(const vpHomography &H, double x, double y if (norm3ok) { // cos theta - Rprim3[0][0] = (vpMath::sqr(sv[1]) + sv[0] * sv[2]) / ((sv[0] + sv[2]) * sv[1]); + Rprim3[0][0] = (vpMath::sqr(sv[1]) + (sv[0] * sv[2])) / ((sv[0] + sv[2]) * sv[1]); Rprim3[2][2] = Rprim3[0][0]; @@ -978,7 +990,7 @@ void vpHomography::computeDisplacement(const vpHomography &H, double x, double y if (norm4ok) { // cos theta - Rprim4[0][0] = (vpMath::sqr(sv[1]) + sv[0] * sv[2]) / ((sv[0] + sv[2]) * sv[1]); + Rprim4[0][0] = (vpMath::sqr(sv[1]) + (sv[0] * sv[2])) / ((sv[0] + sv[2]) * sv[1]); Rprim4[2][2] = Rprim4[0][0]; @@ -1043,7 +1055,7 @@ void vpHomography::computeDisplacement(const vpHomography &H, double x, double y if (norm1ok) { // cos theta - Rprim1[0][0] = (sv[0] * sv[2] - vpMath::sqr(sv[1])) / ((sv[0] - sv[2]) * sv[1]); + Rprim1[0][0] = ((sv[0] * sv[2]) - vpMath::sqr(sv[1])) / ((sv[0] - sv[2]) * sv[1]); Rprim1[2][2] = -Rprim1[0][0]; @@ -1065,7 +1077,7 @@ void vpHomography::computeDisplacement(const vpHomography &H, double x, double y if (norm2ok) { // cos theta - Rprim2[0][0] = (sv[0] * sv[2] - vpMath::sqr(sv[1])) / ((sv[0] - sv[2]) * sv[1]); + Rprim2[0][0] = ((sv[0] * sv[2]) - vpMath::sqr(sv[1])) / ((sv[0] - sv[2]) * sv[1]); Rprim2[2][2] = -Rprim2[0][0]; @@ -1087,7 +1099,7 @@ void vpHomography::computeDisplacement(const vpHomography &H, double x, double y if (norm3ok) { // cos theta - Rprim3[0][0] = (sv[0] * sv[2] - vpMath::sqr(sv[1])) / ((sv[0] - sv[2]) * sv[1]); + Rprim3[0][0] = ((sv[0] * sv[2]) - vpMath::sqr(sv[1])) / ((sv[0] - sv[2]) * sv[1]); Rprim3[2][2] = -Rprim3[0][0]; @@ -1108,7 +1120,7 @@ void vpHomography::computeDisplacement(const vpHomography &H, double x, double y if (norm4ok) { // cos theta - Rprim4[0][0] = (sv[0] * sv[2] - vpMath::sqr(sv[1])) / ((sv[0] - sv[2]) * sv[1]); + Rprim4[0][0] = ((sv[0] * sv[2]) - vpMath::sqr(sv[1])) / ((sv[0] - sv[2]) * sv[1]); Rprim4[2][2] = -Rprim4[0][0]; @@ -1226,21 +1238,29 @@ void vpHomography::computeDisplacement(const vpHomography &H, double x, double y << std::endl; } + #ifdef DEBUG_Homographie // on peut ensuite afficher les resultats... - /* std::cout << "Analyse des resultats : "<< std::endl; */ - /* if (cas==cas1) */ - /* std::cout << "On est dans le cas 1" << std::endl; */ - /* if (cas==cas2) */ - /* std::cout << "On est dans le cas 2" << std::endl; */ - /* if (cas==cas3) */ - /* std::cout << "On est dans le cas 3" << std::endl; */ - /* if (cas==cas4) */ - /* std::cout << "On est dans le cas 4" << std::endl; */ - - /* if (distanceFictive < 0) */ - /* std::cout << "d'<0" << std::endl; */ - /* else */ - /* std::cout << "d'>0" << std::endl; */ + std::cout << "Analyse des resultats : "<< std::endl; + if (cas==cas1) { + std::cout << "On est dans le cas 1" << std::endl; + } + if (cas==cas2) { + std::cout << "On est dans le cas 2" << std::endl; + } + if (cas==cas3) { + std::cout << "On est dans le cas 3" << std::endl; + } + if (cas==cas4) { + std::cout << "On est dans le cas 4" << std::endl; + } + + if (distanceFictive < 0) { + std::cout << "d'<0" << std::endl; + } + else { + std::cout << "d'>0" << std::endl; + } + #endif #ifdef DEBUG_Homographie printf("fin : Homographie_EstimationDeplacementCamera\n"); diff --git a/modules/vision/src/homography-estimation/vpHomographyMalis.cpp b/modules/vision/src/homography-estimation/vpHomographyMalis.cpp index 5fde2674a6..69fb836691 100644 --- a/modules/vision/src/homography-estimation/vpHomographyMalis.cpp +++ b/modules/vision/src/homography-estimation/vpHomographyMalis.cpp @@ -31,16 +31,16 @@ * Homography estimation. */ -/*! - \file vpHomographyMalis.cpp + /*! + \file vpHomographyMalis.cpp - This file implements the fonctions related with the homography - estimation from non planar points using the Malis algorithms \cite Malis00b. + This file implements the fonctions related with the homography + estimation from non planar points using the Malis algorithms \cite Malis00b. - The algorithm for 2D scene implemented in this file is described in Ezio - Malis PhD thesis \cite TheseMalis. + The algorithm for 2D scene implemented in this file is described in Ezio + Malis PhD thesis \cite TheseMalis. -*/ + */ #include #include @@ -49,7 +49,10 @@ #include // std::fabs #include // numeric_limits + #ifndef DOXYGEN_SHOULD_SKIP_THIS +namespace +{ const double eps = 1e-6; /************************************************************************** @@ -83,11 +86,9 @@ const double eps = 1e-6; void changeFrame(unsigned int *pts_ref, unsigned int nb_pts, vpMatrix &pd, vpMatrix &p, vpMatrix &pnd, vpMatrix &pn, vpMatrix &M, vpMatrix &Mdp); -void HLM2D(unsigned int nb_pts, vpMatrix &points_des, vpMatrix &points_cour, vpMatrix &H); -void HLM3D(unsigned int nb_pts, vpMatrix &pd, vpMatrix &p, vpMatrix &H); -void HLM(unsigned int q_cible, unsigned int nbpt, double *xm, double *ym, double *xmi, double *ymi, vpMatrix &H); - -void HLM(unsigned int q_cible, const std::vector &xm, const std::vector &ym, +void hlm2D(unsigned int nb_pts, vpMatrix &points_des, vpMatrix &points_cour, vpMatrix &H); +void hlm3D(unsigned int nb_pts, vpMatrix &pd, vpMatrix &p, vpMatrix &H); +void hlm(unsigned int q_cible, const std::vector &xm, const std::vector &ym, const std::vector &xmi, const std::vector &ymi, vpMatrix &H); void changeFrame(unsigned int *pts_ref, unsigned int nb_pts, vpMatrix &pd, vpMatrix &p, vpMatrix &pnd, vpMatrix &pn, @@ -97,8 +98,8 @@ void changeFrame(unsigned int *pts_ref, unsigned int nb_pts, vpMatrix &pd, vpMat vpMatrix Md(3, 3); vpMatrix Mp(3, 3); - for (unsigned int i = 0; i < 3; i++) { - for (unsigned int j = 0; j < 3; j++) { + for (unsigned int i = 0; i < 3; ++i) { + for (unsigned int j = 0; j < 3; ++j) { M[j][i] = p[pts_ref[i]][j]; Md[j][i] = pd[pts_ref[i]][j]; } @@ -112,15 +113,15 @@ void changeFrame(unsigned int *pts_ref, unsigned int nb_pts, vpMatrix &pd, vpMat double lamb_des[3]; double lamb_cour[3]; - for (unsigned int i = 0; i < 3; i++) { - for (unsigned int j = 0; j < 3; j++) { + for (unsigned int i = 0; i < 3; ++i) { + for (unsigned int j = 0; j < 3; ++j) { lamb_cour[i] = Mp[i][j] * p[pts_ref[3]][j]; lamb_des[i] = Mdp[i][j] * pd[pts_ref[3]][j]; } } - for (unsigned int i = 0; i < 3; i++) { - for (unsigned int j = 0; j < 3; j++) { + for (unsigned int i = 0; i < 3; ++i) { + for (unsigned int j = 0; j < 3; ++j) { M[i][j] = M[i][j] * lamb_cour[j]; Md[i][j] = Md[i][j] * lamb_des[j]; } @@ -133,14 +134,14 @@ void changeFrame(unsigned int *pts_ref, unsigned int nb_pts, vpMatrix &pd, vpMat les trois points de reference */ unsigned int cont_pts = 0; - for (unsigned int k = 0; k < nb_pts; k++) { + for (unsigned int k = 0; k < nb_pts; ++k) { if ((pts_ref[0] != k) && (pts_ref[1] != k) && (pts_ref[2] != k)) { - for (unsigned int i = 0; i < 3; i++) { + for (unsigned int i = 0; i < 3; ++i) { pn[cont_pts][i] = 0.0; pnd[cont_pts][i] = 0.0; - for (unsigned int j = 0; j < 3; j++) { - pn[cont_pts][i] = pn[cont_pts][i] + Mp[i][j] * p[k][j]; - pnd[cont_pts][i] = pnd[cont_pts][i] + Mdp[i][j] * pd[k][j]; + for (unsigned int j = 0; j < 3; ++j) { + pn[cont_pts][i] = pn[cont_pts][i] + (Mp[i][j] * p[k][j]); + pnd[cont_pts][i] = pnd[cont_pts][i] + (Mdp[i][j] * pd[k][j]); } } cont_pts = cont_pts + 1; @@ -175,7 +176,7 @@ void changeFrame(unsigned int *pts_ref, unsigned int nb_pts, vpMatrix &pd, vpMat * DATES DE MISE A JOUR : * ****************************************************************************/ -void HLM2D(unsigned int nb_pts, vpMatrix &points_des, vpMatrix &points_cour, vpMatrix &H) +void hlm2D(unsigned int nb_pts, vpMatrix &points_des, vpMatrix &points_cour, vpMatrix &H) { double vals_inf; unsigned int contZeros, vect; @@ -186,7 +187,7 @@ void HLM2D(unsigned int nb_pts, vpMatrix &points_des, vpMatrix &points_cour, vpM vpColVector sv(9); /** construction de la matrice M des coefficients dans le cas general **/ - for (unsigned int j = 0; j < nb_pts; j++) { + for (unsigned int j = 0; j < nb_pts; ++j) { M[3 * j][0] = 0; M[3 * j][1] = 0; M[3 * j][2] = 0; @@ -197,25 +198,25 @@ void HLM2D(unsigned int nb_pts, vpMatrix &points_des, vpMatrix &points_cour, vpM M[3 * j][7] = points_des[j][1] * points_cour[j][1]; M[3 * j][8] = points_des[j][2] * points_cour[j][1]; - M[3 * j + 1][0] = points_des[j][0] * points_cour[j][2]; - M[3 * j + 1][1] = points_des[j][1] * points_cour[j][2]; - M[3 * j + 1][2] = points_des[j][2] * points_cour[j][2]; - M[3 * j + 1][3] = 0; - M[3 * j + 1][4] = 0; - M[3 * j + 1][5] = 0; - M[3 * j + 1][6] = -points_des[j][0] * points_cour[j][0]; - M[3 * j + 1][7] = -points_des[j][1] * points_cour[j][0]; - M[3 * j + 1][8] = -points_des[j][2] * points_cour[j][0]; - - M[3 * j + 2][0] = -points_des[j][0] * points_cour[j][1]; - M[3 * j + 2][1] = -points_des[j][1] * points_cour[j][1]; - M[3 * j + 2][2] = -points_des[j][2] * points_cour[j][1]; - M[3 * j + 2][3] = points_des[j][0] * points_cour[j][0]; - M[3 * j + 2][4] = points_des[j][1] * points_cour[j][0]; - M[3 * j + 2][5] = points_des[j][2] * points_cour[j][0]; - M[3 * j + 2][6] = 0; - M[3 * j + 2][7] = 0; - M[3 * j + 2][8] = 0; + M[(3 * j) + 1][0] = points_des[j][0] * points_cour[j][2]; + M[(3 * j) + 1][1] = points_des[j][1] * points_cour[j][2]; + M[(3 * j) + 1][2] = points_des[j][2] * points_cour[j][2]; + M[(3 * j) + 1][3] = 0; + M[(3 * j) + 1][4] = 0; + M[(3 * j) + 1][5] = 0; + M[(3 * j) + 1][6] = -points_des[j][0] * points_cour[j][0]; + M[(3 * j) + 1][7] = -points_des[j][1] * points_cour[j][0]; + M[(3 * j) + 1][8] = -points_des[j][2] * points_cour[j][0]; + + M[(3 * j) + 2][0] = -points_des[j][0] * points_cour[j][1]; + M[(3 * j) + 2][1] = -points_des[j][1] * points_cour[j][1]; + M[(3 * j) + 2][2] = -points_des[j][2] * points_cour[j][1]; + M[(3 * j) + 2][3] = points_des[j][0] * points_cour[j][0]; + M[(3 * j) + 2][4] = points_des[j][1] * points_cour[j][0]; + M[(3 * j) + 2][5] = points_des[j][2] * points_cour[j][0]; + M[(3 * j) + 2][6] = 0; + M[(3 * j) + 2][7] = 0; + M[(3 * j) + 2][8] = 0; } /** calcul de la pseudo-inverse V de M et des valeurs singulieres **/ @@ -235,7 +236,7 @@ void HLM2D(unsigned int nb_pts, vpMatrix &points_des, vpMatrix &points_cour, vpM if (sv[0] < eps) { contZeros = contZeros + 1; } - for (unsigned int j = 1; j < 9; j++) { + for (unsigned int j = 1; j < 9; ++j) { if (sv[j] < vals_inf) { vals_inf = sv[j]; vect = j; @@ -253,9 +254,9 @@ void HLM2D(unsigned int nb_pts, vpMatrix &points_des, vpMatrix &points_cour, vpM H.resize(3, 3); /** construction de la matrice H **/ - for (unsigned int i = 0; i < 3; i++) { - for (unsigned int j = 0; j < 3; j++) { - H[i][j] = V[3 * i + j][vect]; + for (unsigned int i = 0; i < 3; ++i) { + for (unsigned int j = 0; j < 3; ++j) { + H[i][j] = V[(3 * i) + j][vect]; } } } @@ -283,7 +284,7 @@ void HLM2D(unsigned int nb_pts, vpMatrix &points_des, vpMatrix &points_cour, vpM * **************************************************************************** **/ -void HLM3D(unsigned int nb_pts, vpMatrix &pd, vpMatrix &p, vpMatrix &H) +void hlm3D(unsigned int nb_pts, vpMatrix &pd, vpMatrix &p, vpMatrix &H) { unsigned int pts_ref[4]; /*** definit lesquels des points de l'image sont les points de reference***/ @@ -322,16 +323,17 @@ void HLM3D(unsigned int nb_pts, vpMatrix &pd, vpMatrix &p, vpMatrix &H) throw(vpMatrixException(vpMatrixException::matrixError, "Not enough point to compute the homography")); } - // nl = cont_pts*(cont_pts-1)*(cont_pts-2)/6 ; - unsigned int nc = 7; + unsigned int nc = 7; // nb cols /* Allocation matrice CtC */ vpMatrix CtC(nc, nc); /* Initialisation matrice CtC */ - for (unsigned int i = 0; i < nc; i++) - for (unsigned int j = 0; j < nc; j++) + for (unsigned int i = 0; i < nc; ++i) { + for (unsigned int j = 0; j < nc; ++j) { CtC[i][j] = 0.0; + } + } /* Allocation matrice M */ vpColVector C(nc); // Matrice des coefficients @@ -347,72 +349,72 @@ void HLM3D(unsigned int nb_pts, vpMatrix &pd, vpMatrix &p, vpMatrix &H) v5 = x3*x5 ; v6 = x3*x4 ; ****/ unsigned int cont = 0; - for (unsigned int i = 0; i < nb_pts - 5; i++) { - for (unsigned int j = i + 1; j < nb_pts - 4; j++) { - for (unsigned int k = j + 1; k < nb_pts - 3; k++) { + for (unsigned int i = 0; i < (nb_pts - 5); ++i) { + for (unsigned int j = i + 1; j < (nb_pts - 4); ++j) { + for (unsigned int k = j + 1; k < (nb_pts - 3); ++k) { /* coeff a^2*b */ - C[0] = pn[i][2] * pn[j][2] * pn[k][1] * pnd[k][0] // - * (pnd[j][0] * pnd[i][1] - pnd[j][1] * pnd[i][0]) // - + pn[i][2] * pn[k][2] * pn[j][1] * pnd[j][0] // - * (pnd[i][0] * pnd[k][1] - pnd[i][1] * pnd[k][0]) // - + pn[j][2] * pn[k][2] * pn[i][1] * pnd[i][0] // - * (pnd[k][0] * pnd[j][1] - pnd[k][1] * pnd[j][0]); // + C[0] = (pn[i][2] * pn[j][2] * pn[k][1] * pnd[k][0] // + * ((pnd[j][0] * pnd[i][1]) - (pnd[j][1] * pnd[i][0]))) // + + (pn[i][2] * pn[k][2] * pn[j][1] * pnd[j][0] // + * ((pnd[i][0] * pnd[k][1]) - (pnd[i][1] * pnd[k][0]))) // + + (pn[j][2] * pn[k][2] * pn[i][1] * pnd[i][0] // + * ((pnd[k][0] * pnd[j][1]) - (pnd[k][1] * pnd[j][0]))); // /* coeff a*b^2 */ - C[1] = pn[i][2] * pn[j][2] * pn[k][0] * pnd[k][1] // - * (pnd[i][0] * pnd[j][1] - pnd[i][1] * pnd[j][0]) // - + pn[i][2] * pn[k][2] * pn[j][0] * pnd[j][1] // - * (pnd[k][0] * pnd[i][1] - pnd[k][1] * pnd[i][0]) // - + pn[j][2] * pn[k][2] * pn[i][0] * pnd[i][1] // - * (pnd[j][0] * pnd[k][1] - pnd[j][1] * pnd[k][0]); // + C[1] = (pn[i][2] * pn[j][2] * pn[k][0] * pnd[k][1] // + * ((pnd[i][0] * pnd[j][1]) - (pnd[i][1] * pnd[j][0]))) // + + (pn[i][2] * pn[k][2] * pn[j][0] * pnd[j][1] // + * ((pnd[k][0] * pnd[i][1]) - (pnd[k][1] * pnd[i][0]))) // + + (pn[j][2] * pn[k][2] * pn[i][0] * pnd[i][1] // + * ((pnd[j][0] * pnd[k][1]) - (pnd[j][1] * pnd[k][0]))); // /* coeff a^2 */ - C[2] = +pn[i][1] * pn[k][1] * pn[j][2] * pnd[j][0] // - * (pnd[k][2] * pnd[i][0] - pnd[k][0] * pnd[i][2]) // - + pn[i][1] * pn[j][1] * pn[k][2] * pnd[k][0] // - * (pnd[i][2] * pnd[j][0] - pnd[i][0] * pnd[j][2]) + - pn[j][1] * pn[k][1] * pn[i][2] * pnd[i][0] // - * (pnd[j][2] * pnd[k][0] - pnd[j][0] * pnd[k][2]); // + C[2] = (pn[i][1] * pn[k][1] * pn[j][2] * pnd[j][0] // + * ((pnd[k][2] * pnd[i][0]) - (pnd[k][0] * pnd[i][2]))) // + + (pn[i][1] * pn[j][1] * pn[k][2] * pnd[k][0] // + * ((pnd[i][2] * pnd[j][0]) - (pnd[i][0] * pnd[j][2]))) + + (pn[j][1] * pn[k][1] * pn[i][2] * pnd[i][0] // + * ((pnd[j][2] * pnd[k][0]) - (pnd[j][0] * pnd[k][2]))); // /* coeff b^2 */ - C[3] = pn[i][0] * pn[j][0] * pn[k][2] * pnd[k][1] // - * (pnd[i][2] * pnd[j][1] - pnd[i][1] * pnd[j][2]) // - + pn[i][0] * pn[k][0] * pn[j][2] * pnd[j][1] // - * (pnd[k][2] * pnd[i][1] - pnd[k][1] * pnd[i][2]) // - + pn[j][0] * pn[k][0] * pn[i][2] * pnd[i][1] // - * (pnd[j][2] * pnd[k][1] - pnd[j][1] * pnd[k][2]); // + C[3] = (pn[i][0] * pn[j][0] * pn[k][2] * pnd[k][1] // + * ((pnd[i][2] * pnd[j][1]) - (pnd[i][1] * pnd[j][2]))) // + + (pn[i][0] * pn[k][0] * pn[j][2] * pnd[j][1] // + * ((pnd[k][2] * pnd[i][1]) - (pnd[k][1] * pnd[i][2]))) // + + (pn[j][0] * pn[k][0] * pn[i][2] * pnd[i][1] // + * ((pnd[j][2] * pnd[k][1]) - (pnd[j][1] * pnd[k][2]))); // /* coeff a */ - C[5] = pn[i][1] * pn[j][1] * pn[k][0] * pnd[k][2] // - * (pnd[i][0] * pnd[j][2] - pnd[i][2] * pnd[j][0]) // - + pn[i][1] * pn[k][1] * pn[j][0] * pnd[j][2] // - * (pnd[k][0] * pnd[i][2] - pnd[k][2] * pnd[i][0]) // - + pn[j][1] * pn[k][1] * pn[i][0] * pnd[i][2] // - * (pnd[j][0] * pnd[k][2] - pnd[j][2] * pnd[k][0]); // + C[5] = (pn[i][1] * pn[j][1] * pn[k][0] * pnd[k][2] // + * ((pnd[i][0] * pnd[j][2]) - (pnd[i][2] * pnd[j][0]))) // + + (pn[i][1] * pn[k][1] * pn[j][0] * pnd[j][2] // + * ((pnd[k][0] * pnd[i][2]) - (pnd[k][2] * pnd[i][0]))) // + + (pn[j][1] * pn[k][1] * pn[i][0] * pnd[i][2] // + * ((pnd[j][0] * pnd[k][2]) - (pnd[j][2] * pnd[k][0]))); // /* coeff b */ - C[6] = pn[i][0] * pn[j][0] * pn[k][1] * pnd[k][2] // - * (pnd[i][1] * pnd[j][2] - pnd[i][2] * pnd[j][1]) // - + pn[i][0] * pn[k][0] * pn[j][1] * pnd[j][2] // - * (pnd[k][1] * pnd[i][2] - pnd[k][2] * pnd[i][1]) // - + pn[j][0] * pn[k][0] * pn[i][1] * pnd[i][2] // - * (pnd[j][1] * pnd[k][2] - pnd[j][2] * pnd[k][1]); // + C[6] = (pn[i][0] * pn[j][0] * pn[k][1] * pnd[k][2] // + * ((pnd[i][1] * pnd[j][2]) - (pnd[i][2] * pnd[j][1]))) // + + (pn[i][0] * pn[k][0] * pn[j][1] * pnd[j][2] // + * ((pnd[k][1] * pnd[i][2]) - (pnd[k][2] * pnd[i][1]))) // + + (pn[j][0] * pn[k][0] * pn[i][1] * pnd[i][2] // + * ((pnd[j][1] * pnd[k][2]) - (pnd[j][2] * pnd[k][1]))); // /* coeff a*b */ - C[4] = pn[i][0] * pn[k][1] * pn[j][2] // - * (pnd[k][0] * pnd[j][1] * pnd[i][2] - pnd[j][0] * pnd[i][1] * pnd[k][2]) // - + pn[k][0] * pn[i][1] * pn[j][2] // - * (pnd[j][0] * pnd[k][1] * pnd[i][2] - pnd[i][0] * pnd[j][1] * pnd[k][2]) // - + pn[i][0] * pn[j][1] * pn[k][2] // - * (pnd[k][0] * pnd[i][1] * pnd[j][2] - pnd[j][0] * pnd[k][1] * pnd[i][2]) // - + pn[j][0] * pn[i][1] * pn[k][2] // - * (pnd[i][0] * pnd[k][1] * pnd[j][2] - pnd[k][0] * pnd[j][1] * pnd[i][2]) // - + pn[k][0] * pn[j][1] * pn[i][2] // - * (pnd[j][0] * pnd[i][1] * pnd[k][2] - pnd[i][0] * pnd[k][1] * pnd[j][2]) // - + pn[j][0] * pn[k][1] * pn[i][2] // - * (pnd[i][0] * pnd[j][1] * pnd[k][2] - pnd[k][0] * pnd[i][1] * pnd[j][2]); // + C[4] = (pn[i][0] * pn[k][1] * pn[j][2] // + * ((pnd[k][0] * pnd[j][1] * pnd[i][2]) - (pnd[j][0] * pnd[i][1] * pnd[k][2]))) // + + (pn[k][0] * pn[i][1] * pn[j][2] // + * ((pnd[j][0] * pnd[k][1] * pnd[i][2]) - (pnd[i][0] * pnd[j][1] * pnd[k][2]))) // + + (pn[i][0] * pn[j][1] * pn[k][2] // + * ((pnd[k][0] * pnd[i][1] * pnd[j][2]) - (pnd[j][0] * pnd[k][1] * pnd[i][2]))) // + + (pn[j][0] * pn[i][1] * pn[k][2] // + * ((pnd[i][0] * pnd[k][1] * pnd[j][2]) - (pnd[k][0] * pnd[j][1] * pnd[i][2]))) // + + (pn[k][0] * pn[j][1] * pn[i][2] // + * ((pnd[j][0] * pnd[i][1] * pnd[k][2]) - (pnd[i][0] * pnd[k][1] * pnd[j][2]))) // + + (pn[j][0] * pn[k][1] * pn[i][2] // + * ((pnd[i][0] * pnd[j][1] * pnd[k][2]) - (pnd[k][0] * pnd[i][1] * pnd[j][2]))); // cont = cont + 1; /* construction de la matrice CtC */ - for (unsigned int ii = 0; ii < nc; ii++) { - for (unsigned int jj = ii; jj < nc; jj++) { - CtC[ii][jj] = CtC[ii][jj] + C[ii] * C[jj]; + for (unsigned int ii = 0; ii < nc; ++ii) { + for (unsigned int jj = ii; jj < nc; ++jj) { + CtC[ii][jj] = CtC[ii][jj] + (C[ii] * C[jj]); } } } @@ -420,12 +422,12 @@ void HLM3D(unsigned int nb_pts, vpMatrix &pd, vpMatrix &p, vpMatrix &H) } /* calcul de CtC */ - for (unsigned int i = 0; i < nc; i++) { - for (unsigned int j = i + 1; j < nc; j++) + for (unsigned int i = 0; i < nc; ++i) { + for (unsigned int j = i + 1; j < nc; ++j) { CtC[j][i] = CtC[i][j]; + } } - // nl = cont ; /* nombre de lignes */ nc = 7; /* nombre de colonnes */ /* Creation de matrice CtC termine */ @@ -459,18 +461,19 @@ void HLM3D(unsigned int nb_pts, vpMatrix &pd, vpMatrix &p, vpMatrix &H) unsigned int vect = 0; int cont_zeros = 0; cont = 0; - for (unsigned int j = 0; j < nc; j++) { - // if (fabs(sv[j]) == svSorted[cont]) vect = j ; - if (std::fabs(sv[j] - svSorted[cont]) <= std::fabs(vpMath::maximum(sv[j], svSorted[cont]))) + for (unsigned int j = 0; j < nc; ++j) { + if (std::fabs(sv[j] - svSorted[cont]) <= std::fabs(vpMath::maximum(sv[j], svSorted[cont]))) { vect = j; - if (std::fabs(sv[j] / svSorted[nc - 1]) < eps) + } + if (std::fabs(sv[j] / svSorted[nc - 1]) < eps) { cont_zeros = cont_zeros + 1; + } } if (cont_zeros > 5) { - // printf("erreur dans le rang de la matrice: %d \r\n ",7-cont_zeros); - HLM2D(nb_pts, pd, p, H); - } else { + hlm2D(nb_pts, pd, p, H); + } + else { // estimation de a = 1,b,c ; je cherche le min de somme(i=1:n) // (0.5*(ei)^2) @@ -541,18 +544,19 @@ void HLM3D(unsigned int nb_pts, vpMatrix &pd, vpMatrix &p, vpMatrix &H) T[8][2] = V[2][vect]; vpMatrix Hd(3, 3); // diag(gu,gv,gw) - for (unsigned int i = 0; i < 3; i++) + for (unsigned int i = 0; i < 3; ++i) { Hd[i][i] = H_nr[i]; + } - // H = M diag(gu,gv,gw) M*-1 + // H is equal to M diag(gu,gv,gw) M*-1 H = M * Hd * Mdp; } } -void HLM(unsigned int q_cible, const std::vector &xm, const std::vector &ym, +void hlm(unsigned int q_cible, const std::vector &xm, const std::vector &ym, const std::vector &xmi, const std::vector &ymi, vpMatrix &H) { - unsigned int nbpt = (unsigned int)xm.size(); + unsigned int nbpt = static_cast(xm.size()); /**** on regarde si il y a au moins un point mais pour l'homographie @@ -561,7 +565,7 @@ void HLM(unsigned int q_cible, const std::vector &xm, const std::vector< vpMatrix pd(nbpt, 3); vpMatrix p(nbpt, 3); - for (unsigned int i = 0; i < nbpt; i++) { + for (unsigned int i = 0; i < nbpt; ++i) { /**** on assigne les points fournies par la structure robot pour la commande globale @@ -575,44 +579,51 @@ void HLM(unsigned int q_cible, const std::vector &xm, const std::vector< } switch (q_cible) { - case (1): - case (2): + case 1: + case 2: /* La cible est planaire de type points */ - HLM2D(nbpt, pd, p, H); + hlm2D(nbpt, pd, p, H); break; - case (3): /* cible non planaire : chateau */ + case 3: /* cible non planaire : chateau */ /* cible non planaire de type points */ - HLM3D(nbpt, pd, p, H); + hlm3D(nbpt, pd, p, H); + break; + default: break; } /* fin switch */ } /* fin procedure calcul_homogaphie */ +} // end namespace #endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS void vpHomography::HLM(const std::vector &xb, const std::vector &yb, const std::vector &xa, const std::vector &ya, bool isplanar, vpHomography &aHb) { - unsigned int n = (unsigned int)xb.size(); - if (yb.size() != n || xa.size() != n || ya.size() != n) + unsigned int n = static_cast(xb.size()); + if ((yb.size() != n) || (xa.size() != n) || (ya.size() != n)) { throw(vpException(vpException::dimensionError, "Bad dimension for HLM shomography estimation")); + } // 4 point are required - if (n < 4) + if (n < 4) { throw(vpException(vpException::fatalError, "There must be at least 4 matched points")); + } // The reference plane is the plane build from the 3 first points. unsigned int q_cible; vpMatrix H; // matrice d'homographie en metre - if (isplanar) + if (isplanar) { q_cible = 1; - else + } + else { q_cible = 3; + } - ::HLM(q_cible, xa, ya, xb, yb, H); + ::hlm(q_cible, xa, ya, xb, yb, H); aHb = H; } diff --git a/modules/vision/src/homography-estimation/vpHomographyRansac.cpp b/modules/vision/src/homography-estimation/vpHomographyRansac.cpp index dfb1ad5a6d..80db43742d 100644 --- a/modules/vision/src/homography-estimation/vpHomographyRansac.cpp +++ b/modules/vision/src/homography-estimation/vpHomographyRansac.cpp @@ -39,12 +39,12 @@ #include #include -#define vpEps 1e-6 +#define VPEPS 1e-6 -/*! - \file vpHomographyRansac.cpp - \brief function used to estimate an homography using the Ransac algorithm -*/ + /*! + \file vpHomographyRansac.cpp + \brief function used to estimate an homography using the Ransac algorithm + */ #ifndef DOXYGEN_SHOULD_SKIP_THIS @@ -57,16 +57,13 @@ bool iscolinear(double *x1, double *x2, double *x3) p1 << x1; p2 << x2; p3 << x3; - // vpColVector v; - // vpColVector::cross(p2-p1, p3-p1, v); - // return (v.sumSquare() < vpEps); // Assume inhomogeneous coords, or homogeneous coords with equal // scale. - return ((vpColVector::cross(p2 - p1, p3 - p1).sumSquare()) < vpEps); + return ((vpColVector::cross(p2 - p1, p3 - p1).sumSquare()) < VPEPS); } bool isColinear(vpColVector &p1, vpColVector &p2, vpColVector &p3) { - return ((vpColVector::cross(p2 - p1, p3 - p1).sumSquare()) < vpEps); + return ((vpColVector::cross(p2 - p1, p3 - p1).sumSquare()) < VPEPS); } bool vpHomography::degenerateConfiguration(const vpColVector &x, unsigned int *ind, double threshold_area) @@ -74,44 +71,47 @@ bool vpHomography::degenerateConfiguration(const vpColVector &x, unsigned int *i unsigned int i, j, k; - for (i = 1; i < 4; i++) - for (j = 0; j < i; j++) - if (ind[i] == ind[j]) + for (i = 1; i < 4; ++i) { + for (j = 0; j < i; ++j) { + if (ind[i] == ind[j]) { return true; + } + } + } unsigned int n = x.getRows() / 4; double pa[4][3]; double pb[4][3]; - for (i = 0; i < 4; i++) { + for (i = 0; i < 4; ++i) { pb[i][0] = x[2 * ind[i]]; - pb[i][1] = x[2 * ind[i] + 1]; + pb[i][1] = x[(2 * ind[i]) + 1]; pb[i][2] = 1; - pa[i][0] = x[2 * n + 2 * ind[i]]; - pa[i][1] = x[2 * n + 2 * ind[i] + 1]; + pa[i][0] = x[(2 * n) + (2 * ind[i])]; + pa[i][1] = x[(2 * n) + (2 * ind[i]) + 1]; pa[i][2] = 1; } i = 0, j = 1, k = 2; - double area012 = (-pa[j][0] * pa[i][1] + pa[k][0] * pa[i][1] + pa[i][0] * pa[j][1] - pa[k][0] * pa[j][1] + - -pa[i][0] * pa[k][1] + pa[1][j] * pa[k][1]); + double area012 = ((-pa[j][0] * pa[i][1]) + (pa[k][0] * pa[i][1]) + (pa[i][0] * pa[j][1]) - (pa[k][0] * pa[j][1]) + + (-pa[i][0] * pa[k][1]) + (pa[1][j] * pa[k][1])); i = 0; j = 1, k = 3; - double area013 = (-pa[j][0] * pa[i][1] + pa[k][0] * pa[i][1] + pa[i][0] * pa[j][1] - pa[k][0] * pa[j][1] + - -pa[i][0] * pa[k][1] + pa[1][j] * pa[k][1]); + double area013 = ((-pa[j][0] * pa[i][1]) + (pa[k][0] * pa[i][1]) + (pa[i][0] * pa[j][1]) - (pa[k][0] * pa[j][1]) + + (-pa[i][0] * pa[k][1]) + (pa[1][j] * pa[k][1])); i = 0; j = 2, k = 3; - double area023 = (-pa[j][0] * pa[i][1] + pa[k][0] * pa[i][1] + pa[i][0] * pa[j][1] - pa[k][0] * pa[j][1] + - -pa[i][0] * pa[k][1] + pa[1][j] * pa[k][1]); + double area023 = ((-pa[j][0] * pa[i][1]) + (pa[k][0] * pa[i][1]) + (pa[i][0] * pa[j][1]) - (pa[k][0] * pa[j][1]) + + (-pa[i][0] * pa[k][1]) + (pa[1][j] * pa[k][1])); i = 1; j = 2, k = 3; - double area123 = (-pa[j][0] * pa[i][1] + pa[k][0] * pa[i][1] + pa[i][0] * pa[j][1] - pa[k][0] * pa[j][1] + - -pa[i][0] * pa[k][1] + pa[1][j] * pa[k][1]); + double area123 = ((-pa[j][0] * pa[i][1]) + (pa[k][0] * pa[i][1]) + (pa[i][0] * pa[j][1]) - (pa[k][0] * pa[j][1]) + + (-pa[i][0] * pa[k][1]) + (pa[1][j] * pa[k][1])); double sum_area = area012 + area013 + area023 + area123; @@ -134,16 +134,19 @@ leading to 2*2*n */ bool vpHomography::degenerateConfiguration(const vpColVector &x, unsigned int *ind) { - for (unsigned int i = 1; i < 4; i++) - for (unsigned int j = 0; j < i; j++) - if (ind[i] == ind[j]) + for (unsigned int i = 1; i < 4; ++i) { + for (unsigned int j = 0; j < i; ++j) { + if (ind[i] == ind[j]) { return true; + } + } + } unsigned int n = x.getRows() / 4; double pa[4][3]; double pb[4][3]; unsigned int n2 = 2 * n; - for (unsigned int i = 0; i < 4; i++) { + for (unsigned int i = 0; i < 4; ++i) { unsigned int ind2 = 2 * ind[i]; pb[i][0] = x[ind2]; pb[i][1] = x[ind2 + 1]; @@ -160,12 +163,13 @@ bool vpHomography::degenerateConfiguration(const vpColVector &x, unsigned int *i bool vpHomography::degenerateConfiguration(const std::vector &xb, const std::vector &yb, const std::vector &xa, const std::vector &ya) { - unsigned int n = (unsigned int)xb.size(); - if (n < 4) + unsigned int n = static_cast(xb.size()); + if (n < 4) { throw(vpException(vpException::fatalError, "There must be at least 4 matched points")); + } std::vector pa(n), pb(n); - for (unsigned i = 0; i < n; i++) { + for (unsigned i = 0; i < n; ++i) { pa[i].resize(3); pa[i][0] = xa[i]; pa[i][1] = ya[i]; @@ -176,9 +180,9 @@ bool vpHomography::degenerateConfiguration(const std::vector &xb, const pb[i][2] = 1; } - for (unsigned int i = 0; i < n - 2; i++) { - for (unsigned int j = i + 1; j < n - 1; j++) { - for (unsigned int k = j + 1; k < n; k++) { + for (unsigned int i = 0; i < (n - 2); ++i) { + for (unsigned int j = i + 1; j < (n - 1); ++j) { + for (unsigned int k = j + 1; k < n; ++k) { if (isColinear(pa[i], pa[j], pa[k])) { return true; } @@ -197,7 +201,7 @@ void vpHomography::computeTransformation(vpColVector &x, unsigned int *ind, vpCo std::vector xa(4), xb(4); std::vector ya(4), yb(4); unsigned int n2 = n * 2; - for (unsigned int i = 0; i < 4; i++) { + for (unsigned int i = 0; i < 4; ++i) { unsigned int ind2 = 2 * ind[i]; xb[i] = x[ind2]; yb[i] = x[ind2 + 1]; @@ -209,12 +213,13 @@ void vpHomography::computeTransformation(vpColVector &x, unsigned int *ind, vpCo vpHomography aHb; try { vpHomography::HLM(xb, yb, xa, ya, true, aHb); - } catch (...) { + } + catch (...) { aHb.eye(); } M.resize(9); - for (unsigned int i = 0; i < 9; i++) { + for (unsigned int i = 0; i < 9; ++i) { M[i] = aHb.data[i]; } aHb /= aHb[2][2]; @@ -231,7 +236,7 @@ double vpHomography::computeResidual(vpColVector &x, vpColVector &M, vpColVector pa = new vpColVector[n]; pb = new vpColVector[n]; - for (unsigned int i = 0; i < n; i++) { + for (unsigned int i = 0; i < n; ++i) { unsigned int i2 = 2 * i; pb[i].resize(3); pb[i][0] = x[i2]; @@ -246,7 +251,7 @@ double vpHomography::computeResidual(vpColVector &x, vpColVector &M, vpColVector vpMatrix aHb(3, 3); - for (unsigned int i = 0; i < 9; i++) { + for (unsigned int i = 0; i < 9; ++i) { aHb.data[i] = M[i]; } @@ -255,7 +260,7 @@ double vpHomography::computeResidual(vpColVector &x, vpColVector &M, vpColVector d.resize(n); vpColVector Hpb; - for (unsigned int i = 0; i < n; i++) { + for (unsigned int i = 0; i < n; ++i) { Hpb = aHb * pb[i]; Hpb /= Hpb[2]; d[i] = sqrt((pa[i] - Hpb).sumSquare()); @@ -272,7 +277,7 @@ void vpHomography::initRansac(unsigned int n, double *xb, double *yb, double *xa { x.resize(4 * n); unsigned int n2 = n * 2; - for (unsigned int i = 0; i < n; i++) { + for (unsigned int i = 0; i < n; ++i) { unsigned int i2 = 2 * i; x[i2] = xb[i]; x[i2 + 1] = yb[i]; @@ -285,15 +290,17 @@ bool vpHomography::ransac(const std::vector &xb, const std::vector &ya, vpHomography &aHb, std::vector &inliers, double &residual, unsigned int nbInliersConsensus, double threshold, bool normalization) { - unsigned int n = (unsigned int)xb.size(); - if (yb.size() != n || xa.size() != n || ya.size() != n) + unsigned int n = static_cast(xb.size()); + if ((yb.size() != n) || (xa.size() != n) || (ya.size() != n)) { throw(vpException(vpException::dimensionError, "Bad dimension for robust homography estimation")); + } // 4 point are required - if (n < 4) + if (n < 4) { throw(vpException(vpException::fatalError, "There must be at least 4 matched points")); + } - vpUniRand random((long)time(nullptr)); + vpUniRand random(static_cast(time(nullptr))); std::vector best_consensus; std::vector cur_consensus; @@ -317,10 +324,11 @@ bool vpHomography::ransac(const std::vector &xb, const std::vector xb_rand(nbMinRandom); std::vector yb_rand(nbMinRandom); - if (inliers.size() != n) + if (inliers.size() != n) { inliers.resize(n); + } - while (nbTrials < ransacMaxTrials && nbInliers < nbInliersConsensus) { + while ((nbTrials < ransacMaxTrials) && (nbInliers < nbInliersConsensus)) { cur_outliers.clear(); cur_randoms.clear(); @@ -329,11 +337,11 @@ bool vpHomography::ransac(const std::vector &xb, const std::vector usedPt(n, false); rand_ind.clear(); - for (unsigned int i = 0; i < nbMinRandom; i++) { + for (unsigned int i = 0; i < nbMinRandom; ++i) { // Generate random indicies in the range 0..n - unsigned int r = (unsigned int)ceil(random() * n) - 1; + unsigned int r = static_cast(ceil(random() * n)) - 1; while (usedPt[r]) { - r = (unsigned int)ceil(random() * n) - 1; + r = static_cast(ceil(random() * n)) - 1; } usedPt[r] = true; rand_ind.push_back(r); @@ -349,11 +357,12 @@ bool vpHomography::ransac(const std::vector &xb, const std::vector maxDegenerateIter) { vpERROR_TRACE("Unable to select a nondegenerate data set"); @@ -366,7 +375,7 @@ bool vpHomography::ransac(const std::vector &xb, const std::vector &xb, const std::vector &xb, const std::vector nbInliers) { foundSolution = true; best_consensus = cur_consensus; @@ -416,7 +423,7 @@ bool vpHomography::ransac(const std::vector &xb, const std::vector= ransacMaxTrials) { vpERROR_TRACE("Ransac reached the maximum number of trials"); foundSolution = true; @@ -425,12 +432,13 @@ bool vpHomography::ransac(const std::vector &xb, const std::vector= nbInliersConsensus) { - std::vector xa_best(best_consensus.size()); - std::vector ya_best(best_consensus.size()); - std::vector xb_best(best_consensus.size()); - std::vector yb_best(best_consensus.size()); + const unsigned int nbConsensus = best_consensus.size(); + std::vector xa_best(nbConsensus); + std::vector ya_best(nbConsensus); + std::vector xb_best(nbConsensus); + std::vector yb_best(nbConsensus); - for (unsigned i = 0; i < best_consensus.size(); i++) { + for (unsigned i = 0; i < nbConsensus; ++i) { xa_best[i] = xa[best_consensus[i]]; ya_best[i] = ya[best_consensus[i]]; xb_best[i] = xb[best_consensus[i]]; @@ -442,7 +450,8 @@ bool vpHomography::ransac(const std::vector &xb, const std::vector &xb, const std::vector -//#include #include #include #include @@ -52,28 +50,30 @@ static void updatePoseRotation(vpColVector &dx, vpHomogeneousMatrix &mati) { vpRotationMatrix rd; - double s = sqrt(dx[0] * dx[0] + dx[1] * dx[1] + dx[2] * dx[2]); + double s = sqrt((dx[0] * dx[0]) + (dx[1] * dx[1]) + (dx[2] * dx[2])); if (s > 1.e-25) { double u[3]; - for (unsigned int i = 0; i < 3; i++) + for (unsigned int i = 0; i < 3; ++i) { u[i] = dx[i] / s; + } double sinu = sin(s); double cosi = cos(s); double mcosi = 1 - cosi; - rd[0][0] = cosi + mcosi * u[0] * u[0]; - rd[0][1] = -sinu * u[2] + mcosi * u[0] * u[1]; - rd[0][2] = sinu * u[1] + mcosi * u[0] * u[2]; - rd[1][0] = sinu * u[2] + mcosi * u[1] * u[0]; - rd[1][1] = cosi + mcosi * u[1] * u[1]; - rd[1][2] = -sinu * u[0] + mcosi * u[1] * u[2]; - rd[2][0] = -sinu * u[1] + mcosi * u[2] * u[0]; - rd[2][1] = sinu * u[0] + mcosi * u[2] * u[1]; - rd[2][2] = cosi + mcosi * u[2] * u[2]; + rd[0][0] = cosi + (mcosi * u[0] * u[0]); + rd[0][1] = (-sinu * u[2]) + (mcosi * u[0] * u[1]); + rd[0][2] = (sinu * u[1]) + (mcosi * u[0] * u[2]); + rd[1][0] = (sinu * u[2]) + (mcosi * u[1] * u[0]); + rd[1][1] = cosi + (mcosi * u[1] * u[1]); + rd[1][2] = (-sinu * u[0]) + (mcosi * u[1] * u[2]); + rd[2][0] = (-sinu * u[1]) + (mcosi * u[2] * u[0]); + rd[2][1] = (sinu * u[0]) + (mcosi * u[2] * u[1]); + rd[2][2] = cosi + (mcosi * u[2] * u[2]); } else { - for (unsigned int i = 0; i < 3; i++) { - for (unsigned int j = 0; j < 3; j++) + for (unsigned int i = 0; i < 3; ++i) { + for (unsigned int j = 0; j < 3; ++j) { rd[i][j] = 0.0; + } rd[i][i] = 1.0; } } @@ -105,15 +105,16 @@ double vpHomography::computeRotation(unsigned int nbpoint, vpPoint *c1P, vpPoint int iter = 0; unsigned int n = 0; - for (unsigned int i = 0; i < nbpoint; i++) { - // if ((c2P[i].get_x() !=-1) && (c1P[i].get_x() !=-1)) + for (unsigned int i = 0; i < nbpoint; ++i) { + // Check if c2P[i].get_x() and c1P[i].get_x() are different from -1 if ((std::fabs(c2P[i].get_x() + 1) > std::fabs(vpMath::maximum(c2P[i].get_x(), 1.))) && (std::fabs(c1P[i].get_x() + 1) > std::fabs(vpMath::maximum(c1P[i].get_x(), 1.)))) { - n++; + ++n; } } - if ((!only_1) && (!only_2)) + if ((!only_1) && (!only_2)) { n *= 2; + } vpRobust robust; vpColVector res(n); @@ -130,14 +131,15 @@ double vpHomography::computeRotation(unsigned int nbpoint, vpPoint *c1P, vpPoint // compute current position // Change frame (current) - for (unsigned int i = 0; i < 3; i++) - for (unsigned int j = 0; j < 3; j++) + for (unsigned int i = 0; i < 3; ++i) { + for (unsigned int j = 0; j < 3; ++j) { c2Rc1[i][j] = c2Mc1[i][j]; + } + } vpMatrix L(2, 3), Lp; int k = 0; - for (unsigned int i = 0; i < nbpoint; i++) { - // if ((c2P[i].get_x() !=-1) && (c1P[i].get_x() !=-1)) + for (unsigned int i = 0; i < nbpoint; ++i) { if ((std::fabs(c2P[i].get_x() + 1) > std::fabs(vpMath::maximum(c2P[i].get_x(), 1.))) && (std::fabs(c1P[i].get_x() + 1) > std::fabs(vpMath::maximum(c1P[i].get_x(), 1.)))) { p2[0] = c2P[i].get_x(); @@ -158,9 +160,9 @@ double vpHomography::computeRotation(unsigned int nbpoint, vpPoint *c1P, vpPoint double y = Hp2[1]; H2[0][0] = x * y; - H2[0][1] = -(1 + x * x); + H2[0][1] = -(1 + (x * x)); H2[0][2] = y; - H2[1][0] = 1 + y * y; + H2[1][0] = 1 + (y * y); H2[1][1] = -x * y; H2[1][2] = -x; H2 *= -1; @@ -175,9 +177,9 @@ double vpHomography::computeRotation(unsigned int nbpoint, vpPoint *c1P, vpPoint y = Hp1[1]; H1[0][0] = x * y; - H1[0][1] = -(1 + x * x); + H1[0][1] = -(1 + (x * x)); H1[0][2] = y; - H1[1][0] = 1 + y * y; + H1[1][0] = 1 + (y * y); H1[1][1] = -x * y; H1[1][2] = -x; @@ -213,57 +215,55 @@ double vpHomography::computeRotation(unsigned int nbpoint, vpPoint *c1P, vpPoint e = vpColVector::stack(e, e1); } - k++; + ++k; } } if (userobust) { - for (unsigned int l = 0; l < n; l++) { - res[l] = vpMath::sqr(e[2 * l]) + vpMath::sqr(e[2 * l + 1]); + for (unsigned int l = 0; l < n; ++l) { + res[l] = vpMath::sqr(e[2 * l]) + vpMath::sqr(e[(2 * l )+ 1]); } robust.MEstimator(vpRobust::TUKEY, res, w); // compute the pseudo inverse of the interaction matrix - for (unsigned int l = 0; l < n; l++) { + for (unsigned int l = 0; l < n; ++l) { W[2 * l][2 * l] = w[l]; - W[2 * l + 1][2 * l + 1] = w[l]; + W[(2 * l) + 1][(2 * l) + 1] = w[l]; } } else { - for (unsigned int l = 0; l < 2 * n; l++) + for (unsigned int l = 0; l < (2 * n); ++l) { W[l][l] = 1; + } } - // CreateDiagonalMatrix(w, W) ; - (L).pseudoInverse(Lp, 1e-6); + L.pseudoInverse(Lp, 1e-6); // Compute the camera velocity vpColVector c2rc1, v(6); c2rc1 = -2 * Lp * W * e; - for (unsigned int i = 0; i < 3; i++) + for (unsigned int i = 0; i < 3; ++i) { v[i + 3] = c2rc1[i]; + } // only for simulation updatePoseRotation(c2rc1, c2Mc1); r = e.sumSquare(); - if ((W * e).sumSquare() < 1e-10) - break; - if (iter > 25) + if (((W * e).sumSquare() < 1e-10) || (iter > 25)) { break; - iter++; // std::cout << iter <<" e=" <<(e).sumSquare() <<" e=" - // <<(W*e).sumSquare() <(c1Rc2) + ((c1Tc2 * N2.t()) / d2)) * p2; // p2 = Hp1 + Hp1 = (static_cast(c2Rc1) + ((c2Tc1 * N1.t()) / d1)) * p1; // p1 = Hp2 Hp2 /= Hp2[2]; // normalization Hp1 /= Hp1[2]; @@ -353,7 +349,7 @@ double vpHomography::computeDisplacement(unsigned int nbpoint, vpPoint *c1P, vpP double y = Hp2[1]; double Z1; - Z1 = (N1[0] * x + N1[1] * y + N1[2]) / d1; // 1/z + Z1 = ((N1[0] * x) + (N1[1] * y) + N1[2]) / d1; // 1/z H2[0][0] = -Z1; H2[0][1] = 0; @@ -362,22 +358,23 @@ double vpHomography::computeDisplacement(unsigned int nbpoint, vpPoint *c1P, vpP H2[1][1] = -Z1; H2[1][2] = y * Z1; H2[0][3] = x * y; - H2[0][4] = -(1 + x * x); + H2[0][4] = -(1 + (x * x)); H2[0][5] = y; - H2[1][3] = 1 + y * y; + H2[1][3] = 1 + (y * y); H2[1][4] = -x * y; H2[1][5] = -x; H2 *= -1; vpMatrix c1CFc2(6, 6); { - vpMatrix sTR = c1Tc2.skew() * (vpMatrix)c1Rc2; - for (unsigned int k_ = 0; k_ < 3; k_++) - for (unsigned int l = 0; l < 3; l++) { + vpMatrix sTR = c1Tc2.skew() * static_cast(c1Rc2); + for (unsigned int k_ = 0; k_ < 3; ++k_) { + for (unsigned int l = 0; l < 3; ++l) { c1CFc2[k_][l] = c1Rc2[k_][l]; c1CFc2[k_ + 3][l + 3] = c1Rc2[k_][l]; c1CFc2[k_][l + 3] = sTR[k_][l]; } + } } H2 = H2 * c1CFc2; @@ -388,7 +385,7 @@ double vpHomography::computeDisplacement(unsigned int nbpoint, vpPoint *c1P, vpP x = Hp1[0]; y = Hp1[1]; - Z1 = (N2[0] * x + N2[1] * y + N2[2]) / d2; // 1/z + Z1 = ((N2[0] * x) + (N2[1] * y) + N2[2]) / d2; // 1/z H1[0][0] = -Z1; H1[0][1] = 0; @@ -397,9 +394,9 @@ double vpHomography::computeDisplacement(unsigned int nbpoint, vpPoint *c1P, vpP H1[1][1] = -Z1; H1[1][2] = y * Z1; H1[0][3] = x * y; - H1[0][4] = -(1 + x * x); + H1[0][4] = -(1 + (x * x)); H1[0][5] = y; - H1[1][3] = 1 + y * y; + H1[1][3] = 1 + (y * y); H1[1][4] = -x * y; H1[1][5] = -x; @@ -435,23 +432,24 @@ double vpHomography::computeDisplacement(unsigned int nbpoint, vpPoint *c1P, vpP e = vpColVector::stack(e, e1); } - k++; + ++k; } if (userobust) { - for (unsigned int l = 0; l < n; l++) { - res[l] = vpMath::sqr(e[2 * l]) + vpMath::sqr(e[2 * l + 1]); + for (unsigned int l = 0; l < n; ++l) { + res[l] = vpMath::sqr(e[2 * l]) + vpMath::sqr(e[(2 * l )+ 1]); } robust.MEstimator(vpRobust::TUKEY, res, w); // compute the pseudo inverse of the interaction matrix - for (unsigned int l = 0; l < n; l++) { + for (unsigned int l = 0; l < n; ++l) { W[2 * l][2 * l] = w[l]; - W[2 * l + 1][2 * l + 1] = w[l]; + W[(2 * l) + 1][(2 * l) + 1] = w[l]; } } else { - for (unsigned int l = 0; l < 2 * n; l++) + for (unsigned int l = 0; l < (2 * n); ++l) { W[l][l] = 1; + } } (W * L).pseudoInverse(Lp, 1e-16); // Compute the camera velocity @@ -462,19 +460,12 @@ double vpHomography::computeDisplacement(unsigned int nbpoint, vpPoint *c1P, vpP // only for simulation c2Mc1 = vpExponentialMap::direct(c2Tcc1).inverse() * c2Mc1; - // UpdatePose2(c2Tcc1, c2Mc1) ; r = (W * e).sumSquare(); - if (r < 1e-15) { + if ((r < 1e-15) || (iter > 1000) || (r > r_1)) { break; } - if (iter > 1000) { - break; - } - if (r > r_1) { - break; - } - iter++; + ++iter; } return (W * e).sumSquare(); @@ -504,10 +495,6 @@ double vpHomography::computeDisplacement(unsigned int nbpoint, vpPoint *c1P, vpP unsigned int n = 0; n = nbpoint; - // next 2 lines are useless (detected by Coverity Scan) - // if ( (! only_1) && (! only_2) ) - // n *=2 ; - vpRobust robust; vpColVector res(n); vpColVector w(n); @@ -521,7 +508,7 @@ double vpHomography::computeDisplacement(unsigned int nbpoint, vpPoint *c1P, vpP double r = 1e10; iter = 0; - while (vpMath::equal(r_1, r, m_threshold_displacement) == false) { + while (!vpMath::equal(r_1, r, m_threshold_displacement)) { r_1 = r; // compute current position @@ -539,7 +526,7 @@ double vpHomography::computeDisplacement(unsigned int nbpoint, vpPoint *c1P, vpP vpMatrix L(2, 3), Lp; int k = 0; - for (i = 0; i < nbpoint; i++) { + for (i = 0; i < nbpoint; ++i) { getPlaneInfo(oN[i], c1Mo, N1, d1); getPlaneInfo(oN[i], c2Mo, N2, d2); p2[0] = c2P[i].get_x(); @@ -551,8 +538,8 @@ double vpHomography::computeDisplacement(unsigned int nbpoint, vpPoint *c1P, vpP vpMatrix H(3, 3); - Hp2 = ((vpMatrix)c1Rc2 + (c1Tc2 * N2.t()) / d2) * p2; // p2 = Hp1 - Hp1 = ((vpMatrix)c2Rc1 + (c2Tc1 * N1.t()) / d1) * p1; // p1 = Hp2 + Hp2 = (static_cast(c1Rc2) + ((c1Tc2 * N2.t()) / d2)) * p2; // p2 = Hp1 + Hp1 = (static_cast(c2Rc1) + ((c2Tc1 * N1.t()) / d1)) * p1; // p1 = Hp2 Hp2 /= Hp2[2]; // normalization Hp1 /= Hp1[2]; @@ -562,7 +549,7 @@ double vpHomography::computeDisplacement(unsigned int nbpoint, vpPoint *c1P, vpP double y = Hp2[1]; double Z1; - Z1 = (N1[0] * x + N1[1] * y + N1[2]) / d1; // 1/z + Z1 = ((N1[0] * x) + (N1[1] * y) + N1[2]) / d1; // 1/z H2[0][0] = -Z1; H2[0][1] = 0; @@ -571,22 +558,23 @@ double vpHomography::computeDisplacement(unsigned int nbpoint, vpPoint *c1P, vpP H2[1][1] = -Z1; H2[1][2] = y * Z1; H2[0][3] = x * y; - H2[0][4] = -(1 + x * x); + H2[0][4] = -(1 + (x * x)); H2[0][5] = y; - H2[1][3] = 1 + y * y; + H2[1][3] = 1 + (y * y); H2[1][4] = -x * y; H2[1][5] = -x; H2 *= -1; vpMatrix c1CFc2(6, 6); { - vpMatrix sTR = c1Tc2.skew() * (vpMatrix)c1Rc2; - for (unsigned int k_ = 0; k_ < 3; k_++) - for (unsigned int l = 0; l < 3; l++) { + vpMatrix sTR = c1Tc2.skew() * static_cast(c1Rc2); + for (unsigned int k_ = 0; k_ < 3; ++k_) { + for (unsigned int l = 0; l < 3; ++l) { c1CFc2[k_][l] = c1Rc2[k_][l]; c1CFc2[k_ + 3][l + 3] = c1Rc2[k_][l]; c1CFc2[k_][l + 3] = sTR[k_][l]; } + } } H2 = H2 * c1CFc2; @@ -597,7 +585,7 @@ double vpHomography::computeDisplacement(unsigned int nbpoint, vpPoint *c1P, vpP x = Hp1[0]; y = Hp1[1]; - Z1 = (N2[0] * x + N2[1] * y + N2[2]) / d2; // 1/z + Z1 = ((N2[0] * x) + (N2[1] * y) + N2[2]) / d2; // 1/z H1[0][0] = -Z1; H1[0][1] = 0; @@ -606,9 +594,9 @@ double vpHomography::computeDisplacement(unsigned int nbpoint, vpPoint *c1P, vpP H1[1][1] = -Z1; H1[1][2] = y * Z1; H1[0][3] = x * y; - H1[0][4] = -(1 + x * x); + H1[0][4] = -(1 + (x * x)); H1[0][5] = y; - H1[1][3] = 1 + y * y; + H1[1][3] = 1 + (y * y); H1[1][4] = -x * y; H1[1][5] = -x; @@ -644,23 +632,24 @@ double vpHomography::computeDisplacement(unsigned int nbpoint, vpPoint *c1P, vpP e = vpColVector::stack(e, e1); } - k++; + ++k; } if (userobust) { - for (unsigned int k_ = 0; k_ < n; k_++) { - res[k_] = vpMath::sqr(e[2 * k_]) + vpMath::sqr(e[2 * k_ + 1]); + for (unsigned int k_ = 0; k_ < n; ++k_) { + res[k_] = vpMath::sqr(e[2 * k_]) + vpMath::sqr(e[(2 * k_) + 1]); } robust.MEstimator(vpRobust::TUKEY, res, w); // compute the pseudo inverse of the interaction matrix - for (unsigned int k_ = 0; k_ < n; k_++) { + for (unsigned int k_ = 0; k_ < n; ++k_) { W[2 * k_][2 * k_] = w[k_]; - W[2 * k_ + 1][2 * k_ + 1] = w[k_]; + W[(2 * k_) + 1][(2 * k_) + 1] = w[k_]; } } else { - for (unsigned int k_ = 0; k_ < 2 * n; k_++) + for (unsigned int k_ = 0; k_ < (2 * n); ++k_) { W[k_][k_] = 1; + } } (W * L).pseudoInverse(Lp, 1e-16); // Compute the camera velocity @@ -671,19 +660,12 @@ double vpHomography::computeDisplacement(unsigned int nbpoint, vpPoint *c1P, vpP // only for simulation c2Mc1 = vpExponentialMap::direct(c2Tcc1).inverse() * c2Mc1; - // UpdatePose2(c2Tcc1, c2Mc1) ; r = (W * e).sumSquare(); - if (r < 1e-15) { - break; - } - if (iter > 1000) { - break; - } - if (r > r_1) { + if ((r < 1e-15) || (iter > 1000) || (r > r_1)) { break; } - iter++; + ++iter; } return (W * e).sumSquare(); diff --git a/modules/vision/src/key-point/vpKeyPoint.cpp b/modules/vision/src/key-point/vpKeyPoint.cpp index 994b18ec01..83d87ea345 100644 --- a/modules/vision/src/key-point/vpKeyPoint.cpp +++ b/modules/vision/src/key-point/vpKeyPoint.cpp @@ -39,7 +39,9 @@ #if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_FEATURES2D) +#if defined(VISP_HAVE_PUGIXML) #include +#endif namespace { @@ -2414,6 +2416,7 @@ void vpKeyPoint::insertImageMatching(const vpImage &ICurrent, vpImage #include +#if defined(VISP_HAVE_PUGIXML) #include #include @@ -545,3 +546,9 @@ bool vpXmlConfigParserKeyPoint::getUseRansacVVSPoseEstimation() const { return m_impl->getUseRansacVVSPoseEstimation(); } + +#elif !defined(VISP_BUILD_SHARED_LIBS) +// Work around to avoid warning: libvisp_core.a(vpXmlConfigParserKeyPoint.cpp.o) has no symbols +void dummy_vpXmlConfigParserKeyPoint() { }; + +#endif diff --git a/modules/vision/test/keypoint-with-dataset/testKeyPoint-2.cpp b/modules/vision/test/keypoint-with-dataset/testKeyPoint-2.cpp index 8c0391e565..c9a67b3ee4 100644 --- a/modules/vision/test/keypoint-with-dataset/testKeyPoint-2.cpp +++ b/modules/vision/test/keypoint-with-dataset/testKeyPoint-2.cpp @@ -179,9 +179,10 @@ void run_test(const std::string &env_ipath, bool opt_click_allowed, bool opt_dis // Load config for tracker std::string tracker_config_file = vpIoTools::createFilePath(env_ipath, "mbt/cube.xml"); +#if defined(VISP_HAVE_PUGYXML) tracker.loadConfigFile(tracker_config_file); tracker.getCameraParameters(cam); -#if 0 +#else // Corresponding parameters manually set to have an example code vpMe me; me.setMaskSize(5); diff --git a/modules/vision/test/keypoint-with-dataset/testKeyPoint-4.cpp b/modules/vision/test/keypoint-with-dataset/testKeyPoint-4.cpp index 55282dc3f3..2dfd21d68f 100644 --- a/modules/vision/test/keypoint-with-dataset/testKeyPoint-4.cpp +++ b/modules/vision/test/keypoint-with-dataset/testKeyPoint-4.cpp @@ -180,9 +180,10 @@ void run_test(const std::string &env_ipath, bool opt_click_allowed, bool opt_dis // Load config for tracker std::string tracker_config_file = vpIoTools::createFilePath(env_ipath, "mbt/cube.xml"); +#if defined(VISP_HAVE_PUGYXML) tracker.loadConfigFile(tracker_config_file); tracker.getCameraParameters(cam); -#if 0 +#else // Corresponding parameters manually set to have an example code vpMe me; me.setMaskSize(5); diff --git a/modules/vision/test/keypoint-with-dataset/testKeyPoint-7.cpp b/modules/vision/test/keypoint-with-dataset/testKeyPoint-7.cpp index e1143220c8..253dd7ebad 100644 --- a/modules/vision/test/keypoint-with-dataset/testKeyPoint-7.cpp +++ b/modules/vision/test/keypoint-with-dataset/testKeyPoint-7.cpp @@ -149,43 +149,43 @@ bool compareKeyPoints(const std::vector &keypoints1, const std::ve for (size_t cpt = 0; cpt < keypoints1.size(); cpt++) { if (!vpMath::equal(keypoints1[cpt].angle, keypoints2[cpt].angle, std::numeric_limits::epsilon())) { std::cerr << std::fixed << std::setprecision(9) << "keypoints1[cpt].angle=" << keypoints1[cpt].angle - << " ; keypoints2[cpt].angle=" << keypoints2[cpt].angle << std::endl; + << " ; keypoints2[cpt].angle=" << keypoints2[cpt].angle << std::endl; return false; } if (keypoints1[cpt].class_id != keypoints2[cpt].class_id) { std::cerr << "keypoints1[cpt].class_id=" << keypoints1[cpt].class_id - << " ; keypoints2[cpt].class_id=" << keypoints2[cpt].class_id << std::endl; + << " ; keypoints2[cpt].class_id=" << keypoints2[cpt].class_id << std::endl; return false; } if (keypoints1[cpt].octave != keypoints2[cpt].octave) { std::cerr << "keypoints1[cpt].octave=" << keypoints1[cpt].octave - << " ; keypoints2[cpt].octave=" << keypoints2[cpt].octave << std::endl; + << " ; keypoints2[cpt].octave=" << keypoints2[cpt].octave << std::endl; return false; } if (!vpMath::equal(keypoints1[cpt].pt.x, keypoints2[cpt].pt.x, std::numeric_limits::epsilon())) { std::cerr << std::fixed << std::setprecision(9) << "keypoints1[cpt].pt.x=" << keypoints1[cpt].pt.x - << " ; keypoints2[cpt].pt.x=" << keypoints2[cpt].pt.x << std::endl; + << " ; keypoints2[cpt].pt.x=" << keypoints2[cpt].pt.x << std::endl; return false; } if (!vpMath::equal(keypoints1[cpt].pt.y, keypoints2[cpt].pt.y, std::numeric_limits::epsilon())) { std::cerr << std::fixed << std::setprecision(9) << "keypoints1[cpt].pt.y=" << keypoints1[cpt].pt.y - << " ; keypoints2[cpt].pt.y=" << keypoints2[cpt].pt.y << std::endl; + << " ; keypoints2[cpt].pt.y=" << keypoints2[cpt].pt.y << std::endl; return false; } if (!vpMath::equal(keypoints1[cpt].response, keypoints2[cpt].response, std::numeric_limits::epsilon())) { std::cerr << std::fixed << std::setprecision(9) << "keypoints1[cpt].response=" << keypoints1[cpt].response - << " ; keypoints2[cpt].response=" << keypoints2[cpt].response << std::endl; + << " ; keypoints2[cpt].response=" << keypoints2[cpt].response << std::endl; return false; } if (!vpMath::equal(keypoints1[cpt].size, keypoints2[cpt].size, std::numeric_limits::epsilon())) { std::cerr << std::fixed << std::setprecision(9) << "keypoints1[cpt].size=" << keypoints1[cpt].size - << " ; keypoints2[cpt].size=" << keypoints2[cpt].size << std::endl; + << " ; keypoints2[cpt].size=" << keypoints2[cpt].size << std::endl; return false; } } @@ -214,7 +214,7 @@ bool compareDescriptors(const cv::Mat &descriptors1, const cv::Mat &descriptors2 case CV_8U: if (descriptors1.at(i, j) != descriptors2.at(i, j)) { std::cerr << "descriptors1.at(i,j)=" << descriptors1.at(i, j) - << " ; descriptors2.at(i,j)=" << descriptors2.at(i, j) << std::endl; + << " ; descriptors2.at(i,j)=" << descriptors2.at(i, j) << std::endl; return false; } break; @@ -222,7 +222,7 @@ bool compareDescriptors(const cv::Mat &descriptors1, const cv::Mat &descriptors2 case CV_8S: if (descriptors1.at(i, j) != descriptors2.at(i, j)) { std::cerr << "descriptors1.at(i,j)=" << descriptors1.at(i, j) - << " ; descriptors2.at(i,j)=" << descriptors2.at(i, j) << std::endl; + << " ; descriptors2.at(i,j)=" << descriptors2.at(i, j) << std::endl; return false; } break; @@ -230,7 +230,7 @@ bool compareDescriptors(const cv::Mat &descriptors1, const cv::Mat &descriptors2 case CV_16U: if (descriptors1.at(i, j) != descriptors2.at(i, j)) { std::cerr << "descriptors1.at(i,j)=" << descriptors1.at(i, j) - << " ; descriptors2.at(i,j)=" << descriptors2.at(i, j) << std::endl; + << " ; descriptors2.at(i,j)=" << descriptors2.at(i, j) << std::endl; return false; } break; @@ -238,7 +238,7 @@ bool compareDescriptors(const cv::Mat &descriptors1, const cv::Mat &descriptors2 case CV_16S: if (descriptors1.at(i, j) != descriptors2.at(i, j)) { std::cerr << "descriptors1.at(i,j)=" << descriptors1.at(i, j) - << " ; descriptors2.at(i,j)=" << descriptors2.at(i, j) << std::endl; + << " ; descriptors2.at(i,j)=" << descriptors2.at(i, j) << std::endl; return false; } break; @@ -246,7 +246,7 @@ bool compareDescriptors(const cv::Mat &descriptors1, const cv::Mat &descriptors2 case CV_32S: if (descriptors1.at(i, j) != descriptors2.at(i, j)) { std::cerr << "descriptors1.at(i,j)=" << descriptors1.at(i, j) - << " ; descriptors2.at(i,j)=" << descriptors2.at(i, j) << std::endl; + << " ; descriptors2.at(i,j)=" << descriptors2.at(i, j) << std::endl; return false; } break; @@ -255,8 +255,8 @@ bool compareDescriptors(const cv::Mat &descriptors1, const cv::Mat &descriptors2 if (!vpMath::equal(descriptors1.at(i, j), descriptors2.at(i, j), std::numeric_limits::epsilon())) { std::cerr << std::fixed << std::setprecision(9) - << "descriptors1.at(i,j)=" << descriptors1.at(i, j) - << " ; descriptors2.at(i,j)=" << descriptors2.at(i, j) << std::endl; + << "descriptors1.at(i,j)=" << descriptors1.at(i, j) + << " ; descriptors2.at(i,j)=" << descriptors2.at(i, j) << std::endl; return false; } break; @@ -265,8 +265,8 @@ bool compareDescriptors(const cv::Mat &descriptors1, const cv::Mat &descriptors2 if (!vpMath::equal(descriptors1.at(i, j), descriptors2.at(i, j), std::numeric_limits::epsilon())) { std::cerr << std::fixed << std::setprecision(17) - << "descriptors1.at(i,j)=" << descriptors1.at(i, j) - << " ; descriptors2.at(i,j)=" << descriptors2.at(i, j) << std::endl; + << "descriptors1.at(i,j)=" << descriptors1.at(i, j) + << " ; descriptors2.at(i,j)=" << descriptors2.at(i, j) << std::endl; return false; } break; @@ -325,7 +325,9 @@ template void run_test(const std::string &env_ipath, const std:: // Test if read is ok vpKeyPoint read_keypoint1; + read_keypoint1.loadLearningData(filename, true); + std::vector trainKeyPoints_read; read_keypoint1.getTrainKeyPoints(trainKeyPoints_read); cv::Mat trainDescriptors_read = read_keypoint1.getTrainDescriptors(); @@ -341,7 +343,7 @@ template void run_test(const std::string &env_ipath, const std:: "binary with train images saved !"); } - // Save in binary with no training images + // Save in binary without training images filename = vpIoTools::createFilePath(opath, "bin_without_img"); vpIoTools::makeDirectory(filename); filename = vpIoTools::createFilePath(filename, "test_save_in_bin_without_img.bin"); @@ -372,6 +374,7 @@ template void run_test(const std::string &env_ipath, const std:: "binary without train images !"); } +#if defined(VISP_HAVE_PUGYXML) // Save in xml with training images filename = vpIoTools::createFilePath(opath, "xml_with_img"); vpIoTools::makeDirectory(filename); @@ -433,7 +436,7 @@ template void run_test(const std::string &env_ipath, const std:: "learning file saved in " "xml without train images saved !"); } - +#endif std::cout << "Saving / loading learning files with binary descriptor are ok !" << std::endl; } @@ -524,6 +527,7 @@ template void run_test(const std::string &env_ipath, const std:: "binary without train images saved !"); } +#if defined(VISP_HAVE_PUGYXML) // Save in xml with training images filename = vpIoTools::createFilePath(opath, "xml_with_img"); vpIoTools::makeDirectory(filename); @@ -585,10 +589,8 @@ template void run_test(const std::string &env_ipath, const std:: "learning file saved in " "xml without train images saved !"); } - - std::cout << "Saving / loading learning files with floating point " - "descriptor are ok !" - << std::endl; +#endif + std::cout << "Saving / loading learning files with floating point descriptor are ok !" << std::endl; // Test vpKeyPoint::reset() vpKeyPoint keypoint_reset; @@ -623,8 +625,8 @@ template void run_test(const std::string &env_ipath, const std:: } std::cout << "vpKeyPoint::reset() is ok with trainKeyPoints and " - "trainDescriptors !" - << std::endl; + "trainDescriptors !" + << std::endl; #endif // OpenCV != 4.5.4 on macOS } #endif @@ -688,7 +690,8 @@ int main(int argc, const char **argv) run_test(env_ipath, opath, I); } - } catch (const vpException &e) { + } + catch (const vpException &e) { std::cerr << e.what() << std::endl; return EXIT_FAILURE; } diff --git a/modules/vision/test/keypoint-with-dataset/testXmlConfigParserKeyPoint.cpp b/modules/vision/test/keypoint-with-dataset/testXmlConfigParserKeyPoint.cpp index 9f5116eae4..549f5a4c7a 100644 --- a/modules/vision/test/keypoint-with-dataset/testXmlConfigParserKeyPoint.cpp +++ b/modules/vision/test/keypoint-with-dataset/testXmlConfigParserKeyPoint.cpp @@ -42,6 +42,7 @@ int main() { +#if defined(VISP_HAVE_PUGIXML) std::string visp_images_dir = vpIoTools::getViSPImagesDataPath(); if (vpIoTools::checkDirectory(visp_images_dir + "/xml")) { double eps = std::numeric_limits::epsilon(); @@ -76,6 +77,6 @@ int main() } } } - +#endif return EXIT_SUCCESS; } diff --git a/tutorial/computer-vision/tutorial-pose-from-planar-object.cpp b/tutorial/computer-vision/tutorial-pose-from-planar-object.cpp index 5dc9ef13b7..9606151915 100644 --- a/tutorial/computer-vision/tutorial-pose-from-planar-object.cpp +++ b/tutorial/computer-vision/tutorial-pose-from-planar-object.cpp @@ -21,7 +21,7 @@ #include // Check if std:c++17 or higher -#if ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) && defined(VISP_HAVE_DISPLAY) +#if ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) && defined(VISP_HAVE_DISPLAY) && defined(VISP_HAVE_PUGIXML) // Local helper namespace @@ -340,7 +340,7 @@ int main(int, char *argv[]) { #if ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) -#if defined(VISP_HAVE_DISPLAY) +#if defined(VISP_HAVE_DISPLAY) && defined(VISP_HAVE_PUGIXML) // Get prior data //! [Prior_Data] @@ -459,7 +459,7 @@ int main(int, char *argv[]) #else (void)argv; - std::cout << "There is no display available to run this tutorial." << std::endl; + std::cout << "There is no display and pugixml available to run this tutorial." << std::endl; #endif // defined(VISP_HAVE_DISPLAY) #else (void)argv; diff --git a/tutorial/computer-vision/tutorial-pose-from-points-live.cpp b/tutorial/computer-vision/tutorial-pose-from-points-live.cpp index a28d490b74..b6c99b2c54 100644 --- a/tutorial/computer-vision/tutorial-pose-from-points-live.cpp +++ b/tutorial/computer-vision/tutorial-pose-from-points-live.cpp @@ -30,7 +30,7 @@ int main(int argc, char **argv) { -#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)) && \ +#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)) && defined(VISP_HAVE_PUGIXML) && \ (defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_CMU1394) || \ defined(HAVE_OPENCV_VIDEOIO) || defined(VISP_HAVE_FLYCAPTURE) || defined(VISP_HAVE_REALSENSE2)) try { diff --git a/tutorial/detection/object/tutorial-detection-object-mbt-deprecated.cpp b/tutorial/detection/object/tutorial-detection-object-mbt-deprecated.cpp index 7d9e4e4b99..391d81ba60 100644 --- a/tutorial/detection/object/tutorial-detection-object-mbt-deprecated.cpp +++ b/tutorial/detection/object/tutorial-detection-object-mbt-deprecated.cpp @@ -58,11 +58,13 @@ int main(int argc, char **argv) vpMbEdgeTracker tracker; bool usexml = false; +#if defined(VISP_HAVE_PUGIXML) if (vpIoTools::checkFilename(objectname + ".xml")) { tracker.loadConfigFile(objectname + ".xml"); tracker.getCameraParameters(cam); usexml = true; } +#endif if (!usexml) { vpMe me; me.setMaskSize(5); diff --git a/tutorial/detection/object/tutorial-detection-object-mbt.cpp b/tutorial/detection/object/tutorial-detection-object-mbt.cpp index 014ff2b484..d9d0ac470e 100644 --- a/tutorial/detection/object/tutorial-detection-object-mbt.cpp +++ b/tutorial/detection/object/tutorial-detection-object-mbt.cpp @@ -58,11 +58,13 @@ int main(int argc, char **argv) vpMbGenericTracker tracker(vpMbGenericTracker::EDGE_TRACKER); bool usexml = false; +#if defined(VISP_HAVE_PUGIXML) if (vpIoTools::checkFilename(objectname + ".xml")) { tracker.loadConfigFile(objectname + ".xml"); tracker.getCameraParameters(cam); usexml = true; } +#endif if (!usexml) { vpMe me; me.setMaskSize(5); diff --git a/tutorial/detection/object/tutorial-detection-object-mbt2-deprecated.cpp b/tutorial/detection/object/tutorial-detection-object-mbt2-deprecated.cpp index 8e55dddd85..e612206bb2 100644 --- a/tutorial/detection/object/tutorial-detection-object-mbt2-deprecated.cpp +++ b/tutorial/detection/object/tutorial-detection-object-mbt2-deprecated.cpp @@ -77,11 +77,13 @@ int main(int argc, char **argv) vpMbEdgeTracker tracker; bool usexml = false; +#if defined(VISP_HAVE_PUGIXML) if (vpIoTools::checkFilename(objectname + ".xml")) { tracker.loadConfigFile(objectname + ".xml"); tracker.getCameraParameters(cam); usexml = true; } +#endif if (!usexml) { vpMe me; me.setMaskSize(5); @@ -137,8 +139,8 @@ int main(int argc, char **argv) /* * Start the part of the code dedicated to object learning from 3 images */ - std::string imageName [] = { "cube0001.png", "cube0150.png", "cube0200.png" }; - vpHomogeneousMatrix initPoseTab [] = { + std::string imageName[] = { "cube0001.png", "cube0150.png", "cube0200.png" }; + vpHomogeneousMatrix initPoseTab[] = { vpHomogeneousMatrix(0.02143385294, 0.1098083886, 0.5127439561, 2.087159614, 1.141775176, -0.4701291124), vpHomogeneousMatrix(0.02651282185, -0.03713587374, 0.6873765919, 2.314744454, 0.3492296488, -0.1226054828), vpHomogeneousMatrix(0.02965448956, -0.07283091786, 0.7253526051, 2.300529617, -0.4286674806, 0.1788761025) }; diff --git a/tutorial/detection/object/tutorial-detection-object-mbt2.cpp b/tutorial/detection/object/tutorial-detection-object-mbt2.cpp index 5b825e6f59..bbe9259b55 100644 --- a/tutorial/detection/object/tutorial-detection-object-mbt2.cpp +++ b/tutorial/detection/object/tutorial-detection-object-mbt2.cpp @@ -77,11 +77,13 @@ int main(int argc, char **argv) vpMbGenericTracker tracker(vpMbGenericTracker::EDGE_TRACKER); bool usexml = false; +#if defined(VISP_HAVE_PUGIXML) if (vpIoTools::checkFilename(objectname + ".xml")) { tracker.loadConfigFile(objectname + ".xml"); tracker.getCameraParameters(cam); usexml = true; } +#endif if (!usexml) { vpMe me; me.setMaskSize(5); @@ -137,8 +139,8 @@ int main(int argc, char **argv) /* * Start the part of the code dedicated to object learning from 3 images */ - std::string imageName [] = { "cube0001.png", "cube0150.png", "cube0200.png" }; - vpHomogeneousMatrix initPoseTab [] = { + std::string imageName[] = { "cube0001.png", "cube0150.png", "cube0200.png" }; + vpHomogeneousMatrix initPoseTab[] = { vpHomogeneousMatrix(0.02143385294, 0.1098083886, 0.5127439561, 2.087159614, 1.141775176, -0.4701291124), vpHomogeneousMatrix(0.02651282185, -0.03713587374, 0.6873765919, 2.314744454, 0.3492296488, -0.1226054828), vpHomogeneousMatrix(0.02965448956, -0.07283091786, 0.7253526051, 2.300529617, -0.4286674806, 0.1788761025) }; diff --git a/tutorial/detection/tag/tutorial-apriltag-detector-live-T265-realsense.cpp b/tutorial/detection/tag/tutorial-apriltag-detector-live-T265-realsense.cpp index c2eaa1cc83..a19648d3be 100644 --- a/tutorial/detection/tag/tutorial-apriltag-detector-live-T265-realsense.cpp +++ b/tutorial/detection/tag/tutorial-apriltag-detector-live-T265-realsense.cpp @@ -10,7 +10,6 @@ #include #include #include -#include #include #include #include @@ -43,36 +42,46 @@ int main(int argc, const char **argv) for (int i = 1; i < argc; i++) { if (std::string(argv[i]) == "--pose_method" && i + 1 < argc) { poseEstimationMethod = (vpDetectorAprilTag::vpPoseEstimationMethod)atoi(argv[i + 1]); - } else if (std::string(argv[i]) == "--tag_size" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--tag_size" && i + 1 < argc) { tagSize = atof(argv[i + 1]); - } else if (std::string(argv[i]) == "--quad_decimate" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--quad_decimate" && i + 1 < argc) { quad_decimate = (float)atof(argv[i + 1]); - } else if (std::string(argv[i]) == "--nthreads" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--nthreads" && i + 1 < argc) { nThreads = atoi(argv[i + 1]); - } else if (std::string(argv[i]) == "--display_tag") { + } + else if (std::string(argv[i]) == "--display_tag") { display_tag = true; - } else if (std::string(argv[i]) == "--display_off") { + } + else if (std::string(argv[i]) == "--display_off") { display_off = true; - } else if (std::string(argv[i]) == "--color" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--color" && i + 1 < argc) { color_id = atoi(argv[i + 1]); - } else if (std::string(argv[i]) == "--thickness" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--thickness" && i + 1 < argc) { thickness = (unsigned int)atoi(argv[i + 1]); - } else if (std::string(argv[i]) == "--tag_family" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--tag_family" && i + 1 < argc) { tagFamily = (vpDetectorAprilTag::vpAprilTagFamily)atoi(argv[i + 1]); - } else if (std::string(argv[i]) == "--z_aligned") { + } + else if (std::string(argv[i]) == "--z_aligned") { align_frame = true; - } else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") { + } + else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") { std::cout << "Usage: " << argv[0] - << " [--tag_size (default: 0.053)]" - " [--quad_decimate (default: 1)]" - " [--nthreads (default: 1)]" - " [--pose_method (0: HOMOGRAPHY, 1: HOMOGRAPHY_VIRTUAL_VS, " - " 2: DEMENTHON_VIRTUAL_VS, 3: LAGRANGE_VIRTUAL_VS, " - " 4: BEST_RESIDUAL_VIRTUAL_VS, 5: HOMOGRAPHY_ORTHOGONAL_ITERATION) (default: 0)]" - " [--tag_family (0: TAG_36h11, 1: TAG_36h10 (DEPRECATED), 2: TAG_36ARTOOLKIT (DEPRECATED)," - " 3: TAG_25h9, 4: TAG_25h7 (DEPRECATED), 5: TAG_16h5, 6: TAG_CIRCLE21h7, 7: TAG_CIRCLE49h12," - " 8: TAG_CUSTOM48h12, 9: TAG_STANDARD41h12, 10: TAG_STANDARD52h13) (default: 0)]" - " [--display_tag] [--z_aligned]"; + << " [--tag_size (default: 0.053)]" + " [--quad_decimate (default: 1)]" + " [--nthreads (default: 1)]" + " [--pose_method (0: HOMOGRAPHY, 1: HOMOGRAPHY_VIRTUAL_VS, " + " 2: DEMENTHON_VIRTUAL_VS, 3: LAGRANGE_VIRTUAL_VS, " + " 4: BEST_RESIDUAL_VIRTUAL_VS, 5: HOMOGRAPHY_ORTHOGONAL_ITERATION) (default: 0)]" + " [--tag_family (0: TAG_36h11, 1: TAG_36h10 (DEPRECATED), 2: TAG_36ARTOOLKIT (DEPRECATED)," + " 3: TAG_25h9, 4: TAG_25h7 (DEPRECATED), 5: TAG_16h5, 6: TAG_CIRCLE21h7, 7: TAG_CIRCLE49h12," + " 8: TAG_CUSTOM48h12, 9: TAG_STANDARD41h12, 10: TAG_STANDARD52h13) (default: 0)]" + " [--display_tag] [--z_aligned]"; #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)) std::cout << " [--display_off] [--color ] [--thickness ]"; #endif @@ -194,15 +203,16 @@ int main(int argc, const char **argv) std::cout << "Benchmark loop processing time" << std::endl; std::cout << "Mean / Median / Std: " << vpMath::getMean(time_vec) << " ms" - << " ; " << vpMath::getMedian(time_vec) << " ms" - << " ; " << vpMath::getStdev(time_vec) << " ms" << std::endl; + << " ; " << vpMath::getMedian(time_vec) << " ms" + << " ; " << vpMath::getStdev(time_vec) << " ms" << std::endl; if (!display_off) { delete display_left; delete display_undistort; } - } catch (const vpException &e) { + } + catch (const vpException &e) { std::cerr << "Catch an exception: " << e.getMessage() << std::endl; } @@ -214,7 +224,7 @@ int main(int argc, const char **argv) std::cout << "Enable Apriltag support, configure and build ViSP to run this tutorial" << std::endl; #elif defined(VISP_HAVE_REALSENSE2) && !(RS2_API_VERSION > ((2 * 10000) + (31 * 100) + 0)) std::cout << "Realsense T265 device needs librealsense API > 2.31.0. ViSP is linked with librealsense API " - << RS2_API_VERSION_STR << ". You need to upgrade librealsense to use this example." << std::endl; + << RS2_API_VERSION_STR << ". You need to upgrade librealsense to use this example." << std::endl; #else std::cout << "Install librealsense 3rd party, configure and build ViSP again to use this example." << std::endl; #endif diff --git a/tutorial/detection/tag/tutorial-apriltag-detector-live-rgbd-realsense.cpp b/tutorial/detection/tag/tutorial-apriltag-detector-live-rgbd-realsense.cpp index d721296063..a5cd897381 100644 --- a/tutorial/detection/tag/tutorial-apriltag-detector-live-rgbd-realsense.cpp +++ b/tutorial/detection/tag/tutorial-apriltag-detector-live-rgbd-realsense.cpp @@ -7,7 +7,6 @@ #include //! [Include] #include -#include #include #include #include @@ -39,36 +38,46 @@ int main(int argc, const char **argv) for (int i = 1; i < argc; i++) { if (std::string(argv[i]) == "--pose_method" && i + 1 < argc) { poseEstimationMethod = (vpDetectorAprilTag::vpPoseEstimationMethod)atoi(argv[i + 1]); - } else if (std::string(argv[i]) == "--tag_size" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--tag_size" && i + 1 < argc) { tagSize = atof(argv[i + 1]); - } else if (std::string(argv[i]) == "--quad_decimate" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--quad_decimate" && i + 1 < argc) { quad_decimate = (float)atof(argv[i + 1]); - } else if (std::string(argv[i]) == "--nthreads" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--nthreads" && i + 1 < argc) { nThreads = atoi(argv[i + 1]); - } else if (std::string(argv[i]) == "--display_tag") { + } + else if (std::string(argv[i]) == "--display_tag") { display_tag = true; - } else if (std::string(argv[i]) == "--display_off") { + } + else if (std::string(argv[i]) == "--display_off") { display_off = true; - } else if (std::string(argv[i]) == "--color" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--color" && i + 1 < argc) { color_id = atoi(argv[i + 1]); - } else if (std::string(argv[i]) == "--thickness" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--thickness" && i + 1 < argc) { thickness = (unsigned int)atoi(argv[i + 1]); - } else if (std::string(argv[i]) == "--tag_family" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--tag_family" && i + 1 < argc) { tagFamily = (vpDetectorAprilTag::vpAprilTagFamily)atoi(argv[i + 1]); - } else if (std::string(argv[i]) == "--z_aligned") { + } + else if (std::string(argv[i]) == "--z_aligned") { align_frame = true; - } else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") { + } + else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") { std::cout << "Usage: " << argv[0] - << " [--tag_size (default: 0.053)]" - " [--quad_decimate (default: 1)]" - " [--nthreads (default: 1)]" - " [--pose_method (0: HOMOGRAPHY, 1: HOMOGRAPHY_VIRTUAL_VS, " - " 2: DEMENTHON_VIRTUAL_VS, 3: LAGRANGE_VIRTUAL_VS, " - " 4: BEST_RESIDUAL_VIRTUAL_VS, 5: HOMOGRAPHY_ORTHOGONAL_ITERATION) (default: 0)]" - " [--tag_family (0: TAG_36h11, 1: TAG_36h10 (DEPRECATED), 2: TAG_36ARTOOLKIT (DEPRECATED)," - " 3: TAG_25h9, 4: TAG_25h7 (DEPRECATED), 5: TAG_16h5, 6: TAG_CIRCLE21h7, 7: TAG_CIRCLE49h12," - " 8: TAG_CUSTOM48h12, 9: TAG_STANDARD41h12, 10: TAG_STANDARD52h13) (default: 0)]" - " [--display_tag] [--z_aligned]"; + << " [--tag_size (default: 0.053)]" + " [--quad_decimate (default: 1)]" + " [--nthreads (default: 1)]" + " [--pose_method (0: HOMOGRAPHY, 1: HOMOGRAPHY_VIRTUAL_VS, " + " 2: DEMENTHON_VIRTUAL_VS, 3: LAGRANGE_VIRTUAL_VS, " + " 4: BEST_RESIDUAL_VIRTUAL_VS, 5: HOMOGRAPHY_ORTHOGONAL_ITERATION) (default: 0)]" + " [--tag_family (0: TAG_36h11, 1: TAG_36h10 (DEPRECATED), 2: TAG_36ARTOOLKIT (DEPRECATED)," + " 3: TAG_25h9, 4: TAG_25h7 (DEPRECATED), 5: TAG_16h5, 6: TAG_CIRCLE21h7, 7: TAG_CIRCLE49h12," + " 8: TAG_CUSTOM48h12, 9: TAG_STANDARD41h12, 10: TAG_STANDARD52h13) (default: 0)]" + " [--display_tag] [--z_aligned]"; #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)) std::cout << " [--display_off] [--color ] [--thickness ]"; #endif @@ -168,7 +177,8 @@ int main(int argc, const char **argv) if (I_depth_raw[i][j]) { float Z = I_depth_raw[i][j] * depth_scale; depthMap[i][j] = Z; - } else { + } + else { depthMap[i][j] = 0; } } @@ -199,9 +209,11 @@ int main(int argc, const char **argv) &confidence_index)) { if (confidence_index > 0.5) { vpDisplay::displayFrame(I_color2, cMo, cam, tagSize / 2, vpColor::none, 3); - } else if (confidence_index > 0.25) { + } + else if (confidence_index > 0.25) { vpDisplay::displayFrame(I_color2, cMo, cam, tagSize / 2, vpColor::orange, 3); - } else { + } + else { vpDisplay::displayFrame(I_color2, cMo, cam, tagSize / 2, vpColor::red, 3); } std::stringstream ss; @@ -231,8 +243,8 @@ int main(int argc, const char **argv) std::cout << "Benchmark loop processing time" << std::endl; std::cout << "Mean / Median / Std: " << vpMath::getMean(time_vec) << " ms" - << " ; " << vpMath::getMedian(time_vec) << " ms" - << " ; " << vpMath::getStdev(time_vec) << " ms" << std::endl; + << " ; " << vpMath::getMedian(time_vec) << " ms" + << " ; " << vpMath::getStdev(time_vec) << " ms" << std::endl; if (!display_off) { delete d1; @@ -240,7 +252,8 @@ int main(int argc, const char **argv) delete d3; } - } catch (const vpException &e) { + } + catch (const vpException &e) { std::cerr << "Catch an exception: " << e.getMessage() << std::endl; } diff --git a/tutorial/detection/tag/tutorial-apriltag-detector-live.cpp b/tutorial/detection/tag/tutorial-apriltag-detector-live.cpp index f855805155..b3e758e41e 100644 --- a/tutorial/detection/tag/tutorial-apriltag-detector-live.cpp +++ b/tutorial/detection/tag/tutorial-apriltag-detector-live.cpp @@ -31,8 +31,8 @@ int main(int argc, const char **argv) { //! [Macro defined] -#if defined(VISP_HAVE_APRILTAG) && \ - (defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_CMU1394) || \ +#if defined(VISP_HAVE_APRILTAG) && \ + (defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_CMU1394) || \ defined(HAVE_OPENCV_VIDEOIO) || defined(VISP_HAVE_FLYCAPTURE) || defined(VISP_HAVE_REALSENSE2)) //! [Macro defined] @@ -123,11 +123,13 @@ int main(int argc, const char **argv) try { vpCameraParameters cam; cam.initPersProjWithoutDistortion(615.1674805, 615.1675415, 312.1889954, 243.4373779); +#if defined(VISP_HAVE_PUGIXML) vpXmlParserCamera parser; if (!intrinsic_file.empty() && !camera_name.empty()) parser.parse(cam, intrinsic_file, camera_name, vpCameraParameters::perspectiveProjWithoutDistortion); +#endif - //! [Construct grabber] +//! [Construct grabber] #if defined(VISP_HAVE_V4L2) vpV4l2Grabber g; std::ostringstream device; diff --git a/tutorial/detection/tag/tutorial-apriltag-detector.cpp b/tutorial/detection/tag/tutorial-apriltag-detector.cpp index 447e35b6dc..87ba939cfa 100644 --- a/tutorial/detection/tag/tutorial-apriltag-detector.cpp +++ b/tutorial/detection/tag/tutorial-apriltag-detector.cpp @@ -30,52 +30,67 @@ int main(int argc, const char **argv) for (int i = 1; i < argc; i++) { if (std::string(argv[i]) == "--pose_method" && i + 1 < argc) { poseEstimationMethod = (vpDetectorAprilTag::vpPoseEstimationMethod)atoi(argv[i + 1]); - } else if (std::string(argv[i]) == "--tag_size" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--tag_size" && i + 1 < argc) { tagSize = atof(argv[i + 1]); - } else if (std::string(argv[i]) == "--input" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--input" && i + 1 < argc) { input_filename = std::string(argv[i + 1]); - } else if (std::string(argv[i]) == "--quad_decimate" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--quad_decimate" && i + 1 < argc) { quad_decimate = (float)atof(argv[i + 1]); - } else if (std::string(argv[i]) == "--nthreads" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--nthreads" && i + 1 < argc) { nThreads = atoi(argv[i + 1]); - } else if (std::string(argv[i]) == "--intrinsic" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--intrinsic" && i + 1 < argc) { intrinsic_file = std::string(argv[i + 1]); - } else if (std::string(argv[i]) == "--camera_name" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--camera_name" && i + 1 < argc) { camera_name = std::string(argv[i + 1]); - } else if (std::string(argv[i]) == "--display_tag") { + } + else if (std::string(argv[i]) == "--display_tag") { display_tag = true; - } else if (std::string(argv[i]) == "--color" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--color" && i + 1 < argc) { color_id = atoi(argv[i + 1]); - } else if (std::string(argv[i]) == "--thickness" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--thickness" && i + 1 < argc) { thickness = (unsigned int)atoi(argv[i + 1]); - } else if (std::string(argv[i]) == "--tag_family" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--tag_family" && i + 1 < argc) { tagFamily = (vpDetectorAprilTag::vpAprilTagFamily)atoi(argv[i + 1]); - } else if (std::string(argv[i]) == "--z_aligned") { + } + else if (std::string(argv[i]) == "--z_aligned") { z_aligned = true; - } else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") { + } + else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") { std::cout << "Usage: " << argv[0] - << " [--input ] [--tag_size ]" - " [--quad_decimate ] [--nthreads ]" - " [--intrinsic ] [--camera_name ]" - " [--pose_method (0: HOMOGRAPHY, 1: HOMOGRAPHY_VIRTUAL_VS, " - " 2: DEMENTHON_VIRTUAL_VS, 3: LAGRANGE_VIRTUAL_VS, " - " 4: BEST_RESIDUAL_VIRTUAL_VS, 5: HOMOGRAPHY_ORTHOGONAL_ITERATION) (default: 0)]" - " [--tag_family (0: TAG_36h11, 1: TAG_36h10 (DEPRECATED), 2: TAG_36ARTOOLKIT (DEPRECATED)," - " 3: TAG_25h9, 4: TAG_25h7 (DEPRECATED), 5: TAG_16h5, 6: TAG_CIRCLE21h7, 7: TAG_CIRCLE49h12," - " 8: TAG_CUSTOM48h12, 9: TAG_STANDARD41h12, 10: TAG_STANDARD52h13) (default: 0)]" - " [--display_tag] [--color ]" - " [--thickness ] [--z_aligned]" - " [--help]" - << std::endl; + << " [--input ] [--tag_size ]" + " [--quad_decimate ] [--nthreads ]" + " [--intrinsic ] [--camera_name ]" + " [--pose_method (0: HOMOGRAPHY, 1: HOMOGRAPHY_VIRTUAL_VS, " + " 2: DEMENTHON_VIRTUAL_VS, 3: LAGRANGE_VIRTUAL_VS, " + " 4: BEST_RESIDUAL_VIRTUAL_VS, 5: HOMOGRAPHY_ORTHOGONAL_ITERATION) (default: 0)]" + " [--tag_family (0: TAG_36h11, 1: TAG_36h10 (DEPRECATED), 2: TAG_36ARTOOLKIT (DEPRECATED)," + " 3: TAG_25h9, 4: TAG_25h7 (DEPRECATED), 5: TAG_16h5, 6: TAG_CIRCLE21h7, 7: TAG_CIRCLE49h12," + " 8: TAG_CUSTOM48h12, 9: TAG_STANDARD41h12, 10: TAG_STANDARD52h13) (default: 0)]" + " [--display_tag] [--color ]" + " [--thickness ] [--z_aligned]" + " [--help]" + << std::endl; return EXIT_SUCCESS; } } vpCameraParameters cam; cam.initPersProjWithoutDistortion(615.1674805, 615.1675415, 312.1889954, 243.4373779); +#if defined(VISP_HAVE_PUGIXML) vpXmlParserCamera parser; - if (!intrinsic_file.empty() && !camera_name.empty()) + if (!intrinsic_file.empty() && !camera_name.empty()) { parser.parse(cam, intrinsic_file, camera_name, vpCameraParameters::perspectiveProjWithoutDistortion); + } +#endif std::cout << cam << std::endl; std::cout << "poseEstimationMethod: " << poseEstimationMethod << std::endl; @@ -187,7 +202,8 @@ int main(int argc, const char **argv) vpDisplay::displayText(I_color, 20, 20, "Click to quit.", vpColor::red); vpDisplay::flush(I_color); vpDisplay::getClick(I_color); - } catch (const vpException &e) { + } + catch (const vpException &e) { std::cerr << "Catch an exception: " << e.getMessage() << std::endl; } diff --git a/tutorial/image/tutorial-undistort.cpp b/tutorial/image/tutorial-undistort.cpp index 5601624e89..d72b2d8932 100644 --- a/tutorial/image/tutorial-undistort.cpp +++ b/tutorial/image/tutorial-undistort.cpp @@ -14,17 +14,20 @@ int main(int argc, char **argv) for (int i = 1; i < argc; i++) { if (std::string(argv[i]) == "--image" && i + 1 < argc) { opt_input_image = std::string(argv[i + 1]); - } else if (std::string(argv[i]) == "--camera-file" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--camera-file" && i + 1 < argc) { opt_camera_file = std::string(argv[i + 1]); - } else if (std::string(argv[i]) == "--camera-name" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--camera-name" && i + 1 < argc) { opt_camera_name = std::string(argv[i + 1]); - } else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") { + } + else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") { std::cout << argv[0] << " [--image ]" - << " [--camera-file ] [--camera-name ] [--help] [-h]\n" - << std::endl; + << " [--camera-file ] [--camera-name ] [--help] [-h]\n" + << std::endl; std::cout << "Examples: " << std::endl - << argv[0] << std::endl - << argv[0] << " --image chessboard.jpg --camera-file camera.xml --camera-name Camera" << std::endl; + << argv[0] << std::endl + << argv[0] << " --image chessboard.jpg --camera-file camera.xml --camera-name Camera" << std::endl; return EXIT_SUCCESS; } } @@ -35,8 +38,10 @@ int main(int argc, char **argv) vpImageIo::read(I, opt_input_image); //! [Load image] - //! [Load camera parameters from xml] + vpCameraParameters cam; +#if defined(VISP_HAVE_PUGIXML) + //! [Load camera parameters from xml] vpXmlParserCamera p; vpCameraParameters::vpCameraParametersProjType projModel; projModel = vpCameraParameters::perspectiveProjWithDistortion; @@ -44,12 +49,12 @@ int main(int argc, char **argv) vpXmlParserCamera::SEQUENCE_OK) { std::cout << "Cannot found parameters for camera named \"Camera\"" << std::endl; } -//! [Load camera parameters from xml] -//! [Set camera parameters] -#if 0 + //! [Load camera parameters from xml] +#else + //! [Set camera parameters] cam.initPersProjWithDistortion(582.7, 580.6, 326.6, 215.0, -0.3372, 0.4021); -#endif //! [Set camera parameters] +#endif std::cout << cam << std::endl; @@ -62,7 +67,8 @@ int main(int argc, char **argv) std::cout << "Save undistorted image in: " << output_image << std::endl; vpImageIo::write(Iud, output_image); //! [Create image without distortion] - } catch (const vpException &e) { + } + catch (const vpException &e) { std::cout << "Catch an exception: " << e << std::endl; return EXIT_FAILURE; } diff --git a/tutorial/robot/flir-ptu/tutorial-flir-ptu-ibvs.cpp b/tutorial/robot/flir-ptu/tutorial-flir-ptu-ibvs.cpp index 14ad56fd52..11222668e0 100644 --- a/tutorial/robot/flir-ptu/tutorial-flir-ptu-ibvs.cpp +++ b/tutorial/robot/flir-ptu/tutorial-flir-ptu-ibvs.cpp @@ -58,7 +58,6 @@ #include #include -#include #include #include #include @@ -91,55 +90,60 @@ int main(int argc, char **argv) for (int i = 1; i < argc; i++) { if ((std::string(argv[i]) == "--portname" || std::string(argv[i]) == "-p") && (i + 1 < argc)) { opt_portname = std::string(argv[i + 1]); - } else if ((std::string(argv[i]) == "--baudrate" || std::string(argv[i]) == "-b") && (i + 1 < argc)) { + } + else if ((std::string(argv[i]) == "--baudrate" || std::string(argv[i]) == "-b") && (i + 1 < argc)) { opt_baudrate = std::atoi(argv[i + 1]); - } else if ((std::string(argv[i]) == "--network" || std::string(argv[i]) == "-n")) { + } + else if ((std::string(argv[i]) == "--network" || std::string(argv[i]) == "-n")) { opt_network = true; - } else if (std::string(argv[i]) == "--extrinsic" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--extrinsic" && i + 1 < argc) { opt_extrinsic = std::string(argv[i + 1]); - } else if (std::string(argv[i]) == "--constant-gain" || std::string(argv[i]) == "-g") { + } + else if (std::string(argv[i]) == "--constant-gain" || std::string(argv[i]) == "-g") { opt_constant_gain = std::stod(argv[i + 1]); ; - } else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") { + } + else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") { std::cout << "SYNOPSIS" << std::endl - << " " << argv[0] << " [--portname ] [--baudrate ] [--network] " - << "[--extrinsic ] [--constant-gain] [--help] [-h]" << std::endl - << std::endl; + << " " << argv[0] << " [--portname ] [--baudrate ] [--network] " + << "[--extrinsic ] [--constant-gain] [--help] [-h]" << std::endl + << std::endl; std::cout << "DESCRIPTION" << std::endl - << " --portname, -p " << std::endl - << " Set serial or tcp port name." << std::endl - << std::endl - << " --baudrate, -b " << std::endl - << " Set serial communication baud rate. Default: " << opt_baudrate << "." << std::endl - << std::endl - << " --network, -n" << std::endl - << " Get PTU network information (Hostname, IP, Gateway) and exit. " << std::endl - << std::endl - << " --extrinsic " << std::endl - << " YAML file containing extrinsic camera parameters as a vpHomogeneousMatrix." << std::endl - << " It corresponds to the homogeneous transformation eMc, between end-effector" << std::endl - << " and camera frame." << std::endl - << std::endl - << " --constant-gain, -g" << std::endl - << " Constant gain value. Default value: " << opt_constant_gain << std::endl - << std::endl - << " --help, -h" << std::endl - << " Print this helper message. " << std::endl - << std::endl; + << " --portname, -p " << std::endl + << " Set serial or tcp port name." << std::endl + << std::endl + << " --baudrate, -b " << std::endl + << " Set serial communication baud rate. Default: " << opt_baudrate << "." << std::endl + << std::endl + << " --network, -n" << std::endl + << " Get PTU network information (Hostname, IP, Gateway) and exit. " << std::endl + << std::endl + << " --extrinsic " << std::endl + << " YAML file containing extrinsic camera parameters as a vpHomogeneousMatrix." << std::endl + << " It corresponds to the homogeneous transformation eMc, between end-effector" << std::endl + << " and camera frame." << std::endl + << std::endl + << " --constant-gain, -g" << std::endl + << " Constant gain value. Default value: " << opt_constant_gain << std::endl + << std::endl + << " --help, -h" << std::endl + << " Print this helper message. " << std::endl + << std::endl; std::cout << "EXAMPLE" << std::endl - << " - How to get network IP" << std::endl + << " - How to get network IP" << std::endl #ifdef _WIN32 - << " $ " << argv[0] << " --portname COM1 --network" << std::endl - << " Try to connect FLIR PTU to port: COM1 with baudrate: 9600" << std::endl + << " $ " << argv[0] << " --portname COM1 --network" << std::endl + << " Try to connect FLIR PTU to port: COM1 with baudrate: 9600" << std::endl #else - << " $ " << argv[0] << " --portname /dev/ttyUSB0 --network" << std::endl - << " Try to connect FLIR PTU to port: /dev/ttyUSB0 with baudrate: 9600" << std::endl + << " $ " << argv[0] << " --portname /dev/ttyUSB0 --network" << std::endl + << " Try to connect FLIR PTU to port: /dev/ttyUSB0 with baudrate: 9600" << std::endl #endif - << " PTU HostName: PTU-5" << std::endl - << " PTU IP : 169.254.110.254" << std::endl - << " PTU Gateway : 0.0.0.0" << std::endl - << " - How to run this binary using network communication" << std::endl - << " $ " << argv[0] << " --portname tcp:169.254.110.254 --tag-size 0.1 --gain 0.1" << std::endl; + << " PTU HostName: PTU-5" << std::endl + << " PTU IP : 169.254.110.254" << std::endl + << " PTU Gateway : 0.0.0.0" << std::endl + << " - How to run this binary using network communication" << std::endl + << " $ " << argv[0] << " --portname tcp:169.254.110.254 --tag-size 0.1 --gain 0.1" << std::endl; return EXIT_SUCCESS; } @@ -285,7 +289,8 @@ int main(int argc, char **argv) } std::cout << "Stop the robot " << std::endl; robot.setRobotState(vpRobot::STATE_STOP); - } catch (const vpRobotException &e) { + } + catch (const vpRobotException &e) { std::cout << "Catch Flir Ptu exception: " << e.getMessage() << std::endl; robot.setRobotState(vpRobot::STATE_STOP); } diff --git a/tutorial/robot/mbot/raspberry/visp/mbot-apriltag-ibvs.cpp b/tutorial/robot/mbot/raspberry/visp/mbot-apriltag-ibvs.cpp index 9a5ce986db..44650e7db5 100644 --- a/tutorial/robot/mbot/raspberry/visp/mbot-apriltag-ibvs.cpp +++ b/tutorial/robot/mbot/raspberry/visp/mbot-apriltag-ibvs.cpp @@ -37,37 +37,48 @@ int main(int argc, const char **argv) for (int i = 1; i < argc; i++) { if (std::string(argv[i]) == "--tag_size" && i + 1 < argc) { tagSize = std::atof(argv[i + 1]); - } else if (std::string(argv[i]) == "--input" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--input" && i + 1 < argc) { device = std::atoi(argv[i + 1]); - } else if (std::string(argv[i]) == "--quad_decimate" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--quad_decimate" && i + 1 < argc) { quad_decimate = (float)atof(argv[i + 1]); - } else if (std::string(argv[i]) == "--nthreads" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--nthreads" && i + 1 < argc) { nThreads = std::atoi(argv[i + 1]); - } else if (std::string(argv[i]) == "--intrinsic" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--intrinsic" && i + 1 < argc) { intrinsic_file = std::string(argv[i + 1]); - } else if (std::string(argv[i]) == "--camera_name" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--camera_name" && i + 1 < argc) { camera_name = std::string(argv[i + 1]); - } else if (std::string(argv[i]) == "--display_tag") { + } + else if (std::string(argv[i]) == "--display_tag") { display_tag = true; #if defined(VISP_HAVE_X11) - } else if (std::string(argv[i]) == "--display_on") { + } + else if (std::string(argv[i]) == "--display_on") { display_on = true; - } else if (std::string(argv[i]) == "--save_image") { + } + else if (std::string(argv[i]) == "--save_image") { save_image = true; #endif - } else if (std::string(argv[i]) == "--serial_off") { + } + else if (std::string(argv[i]) == "--serial_off") { serial_off = true; - } else if (std::string(argv[i]) == "--tag_family" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--tag_family" && i + 1 < argc) { tagFamily = (vpDetectorAprilTag::vpAprilTagFamily)std::atoi(argv[i + 1]); - } else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") { + } + else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") { std::cout << "Usage: " << argv[0] - << " [--input ] [--tag_size ]" - " [--quad_decimate ] [--nthreads ]" - " [--intrinsic ] [--camera_name ]" - " [--tag_family (0: TAG_36h11, 1: TAG_36h10, 2: " - "TAG_36ARTOOLKIT," - " 3: TAG_25h9, 4: TAG_25h7, 5: TAG_16h5)]" - " [--display_tag]"; + << " [--input ] [--tag_size ]" + " [--quad_decimate ] [--nthreads ]" + " [--intrinsic ] [--camera_name ]" + " [--tag_family (0: TAG_36h11, 1: TAG_36h10, 2: " + "TAG_36ARTOOLKIT," + " 3: TAG_25h9, 4: TAG_25h7, 5: TAG_16h5)]" + " [--display_tag]"; #if defined(VISP_HAVE_X11) std::cout << " [--display_on] [--save_image]"; #endif @@ -111,9 +122,13 @@ int main(int argc, const char **argv) vpCameraParameters cam; cam.initPersProjWithoutDistortion(615.1674805, 615.1675415, I.getWidth() / 2., I.getHeight() / 2.); + +#if defined(VISP_HAVE_PUGIXML) vpXmlParserCamera parser; - if (!intrinsic_file.empty() && !camera_name.empty()) + if (!intrinsic_file.empty() && !camera_name.empty()) { parser.parse(cam, intrinsic_file, camera_name, vpCameraParameters::perspectiveProjWithoutDistortion); + } +#endif std::cout << "cam:\n" << cam << std::endl; std::cout << "tagFamily: " << tagFamily << std::endl; @@ -160,8 +175,8 @@ int main(int argc, const char **argv) double Z_d = 0.4; // Define the desired polygon corresponding the the AprilTag CLOCKWISE - double X[4] = {tagSize / 2., tagSize / 2., -tagSize / 2., -tagSize / 2.}; - double Y[4] = {tagSize / 2., -tagSize / 2., -tagSize / 2., tagSize / 2.}; + double X[4] = { tagSize / 2., tagSize / 2., -tagSize / 2., -tagSize / 2. }; + double Y[4] = { tagSize / 2., -tagSize / 2., -tagSize / 2., tagSize / 2. }; std::vector vec_P, vec_P_d; for (int i = 0; i < 4; i++) { @@ -177,7 +192,7 @@ int main(int argc, const char **argv) vpMomentGravityCenter mg, mg_d; vpMomentCentered mc, mc_d; vpMomentAreaNormalized man(0, Z_d), - man_d(0, Z_d); // Declare normalized area. Desired area parameter will be updated below with m00 + man_d(0, Z_d); // Declare normalized area. Desired area parameter will be updated below with m00 vpMomentGravityCenterNormalized mgn, mgn_d; // Declare normalized gravity center // Desired moments @@ -313,8 +328,9 @@ int main(int argc, const char **argv) if (!serial_off) { serial->write(ss.str()); } - } else { - // stop the robot + } + else { + // stop the robot if (!serial_off) { serial->write("LED_RING=2,10,0,0\n"); // Switch on led 2 to red: tag not detected // serial->write("LED_RING=3,0,0,0\n"); // Switch on led 3 to blue: motor left not servoed @@ -340,15 +356,16 @@ int main(int argc, const char **argv) std::cout << "Benchmark computation time" << std::endl; std::cout << "Mean / Median / Std: " << vpMath::getMean(time_vec) << " ms" - << " ; " << vpMath::getMedian(time_vec) << " ms" - << " ; " << vpMath::getStdev(time_vec) << " ms" << std::endl; + << " ; " << vpMath::getMedian(time_vec) << " ms" + << " ; " << vpMath::getStdev(time_vec) << " ms" << std::endl; if (display_on) delete d; if (!serial_off) { delete serial; } - } catch (const vpException &e) { + } + catch (const vpException &e) { std::cerr << "Catch an exception: " << e.getMessage() << std::endl; if (!serial_off) { serial->write("LED_RING=1,10,0,0\n"); // Switch on led 1 to red diff --git a/tutorial/robot/mbot/raspberry/visp/mbot-apriltag-pbvs.cpp b/tutorial/robot/mbot/raspberry/visp/mbot-apriltag-pbvs.cpp index d9726cf305..c9d7d1c1ae 100644 --- a/tutorial/robot/mbot/raspberry/visp/mbot-apriltag-pbvs.cpp +++ b/tutorial/robot/mbot/raspberry/visp/mbot-apriltag-pbvs.cpp @@ -28,36 +28,47 @@ int main(int argc, const char **argv) for (int i = 1; i < argc; i++) { if (std::string(argv[i]) == "--tag_size" && i + 1 < argc) { tagSize = std::atof(argv[i + 1]); - } else if (std::string(argv[i]) == "--input" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--input" && i + 1 < argc) { device = std::atoi(argv[i + 1]); - } else if (std::string(argv[i]) == "--quad_decimate" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--quad_decimate" && i + 1 < argc) { quad_decimate = (float)atof(argv[i + 1]); - } else if (std::string(argv[i]) == "--nthreads" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--nthreads" && i + 1 < argc) { nThreads = std::atoi(argv[i + 1]); - } else if (std::string(argv[i]) == "--intrinsic" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--intrinsic" && i + 1 < argc) { intrinsic_file = std::string(argv[i + 1]); - } else if (std::string(argv[i]) == "--camera_name" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--camera_name" && i + 1 < argc) { camera_name = std::string(argv[i + 1]); - } else if (std::string(argv[i]) == "--display_tag") { + } + else if (std::string(argv[i]) == "--display_tag") { display_tag = true; #if defined(VISP_HAVE_X11) - } else if (std::string(argv[i]) == "--display_on") { + } + else if (std::string(argv[i]) == "--display_on") { display_on = true; - } else if (std::string(argv[i]) == "--save_image") { + } + else if (std::string(argv[i]) == "--save_image") { save_image = true; #endif - } else if (std::string(argv[i]) == "--serial_off") { + } + else if (std::string(argv[i]) == "--serial_off") { serial_off = true; - } else if (std::string(argv[i]) == "--tag_family" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--tag_family" && i + 1 < argc) { tagFamily = (vpDetectorAprilTag::vpAprilTagFamily)atoi(argv[i + 1]); - } else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") { + } + else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") { std::cout << "Usage: " << argv[0] - << " [--input ] [--tag_size ]" - " [--quad_decimate ] [--nthreads ]" - " [--intrinsic ] [--camera_name ]" - " [--tag_family (0: TAG_36h11, 1: TAG_36h10, 2: TAG_36ARTOOLKIT," - " 3: TAG_25h9, 4: TAG_25h7, 5: TAG_16h5)]" - " [--display_tag]"; + << " [--input ] [--tag_size ]" + " [--quad_decimate ] [--nthreads ]" + " [--intrinsic ] [--camera_name ]" + " [--tag_family (0: TAG_36h11, 1: TAG_36h10, 2: TAG_36ARTOOLKIT," + " 3: TAG_25h9, 4: TAG_25h7, 5: TAG_16h5)]" + " [--display_tag]"; #if defined(VISP_HAVE_X11) std::cout << " [--display_on] [--save_image]"; #endif @@ -101,9 +112,13 @@ int main(int argc, const char **argv) vpCameraParameters cam; cam.initPersProjWithoutDistortion(615.1674805, 615.1675415, I.getWidth() / 2., I.getHeight() / 2.); + +#if defined(VISP_HAVE_PUGIXML) vpXmlParserCamera parser; - if (!intrinsic_file.empty() && !camera_name.empty()) + if (!intrinsic_file.empty() && !camera_name.empty()) { parser.parse(cam, intrinsic_file, camera_name, vpCameraParameters::perspectiveProjWithoutDistortion); + } +#endif std::cout << "cam:\n" << cam << std::endl; std::cout << "tagFamily: " << tagFamily << std::endl; @@ -226,8 +241,9 @@ int main(int argc, const char **argv) if (!serial_off) { serial->write(ss.str()); } - } else { - // stop the robot + } + else { + // stop the robot if (!serial_off) { serial->write("LED_RING=2,10,0,0\n"); // Switch on led 2 to red: tag not detected // serial->write("LED_RING=3,0,0,0\n"); // Switch on led 3 to blue: motor left not servoed @@ -252,15 +268,16 @@ int main(int argc, const char **argv) std::cout << "Benchmark computation time" << std::endl; std::cout << "Mean / Median / Std: " << vpMath::getMean(time_vec) << " ms" - << " ; " << vpMath::getMedian(time_vec) << " ms" - << " ; " << vpMath::getStdev(time_vec) << " ms" << std::endl; + << " ; " << vpMath::getMedian(time_vec) << " ms" + << " ; " << vpMath::getStdev(time_vec) << " ms" << std::endl; if (display_on) delete d; if (!serial_off) { delete serial; } - } catch (const vpException &e) { + } + catch (const vpException &e) { std::cerr << "Catch an exception: " << e.getMessage() << std::endl; if (!serial_off) { serial->write("LED_RING=1,10,0,0\n"); // Switch on led 1 to red diff --git a/tutorial/tracking/model-based/generic-apriltag/tutorial-mb-generic-tracker-apriltag-rs2.cpp b/tutorial/tracking/model-based/generic-apriltag/tutorial-mb-generic-tracker-apriltag-rs2.cpp index 0a15fd9487..349fdd3a5e 100644 --- a/tutorial/tracking/model-based/generic-apriltag/tutorial-mb-generic-tracker-apriltag-rs2.cpp +++ b/tutorial/tracking/model-based/generic-apriltag/tutorial-mb-generic-tracker-apriltag-rs2.cpp @@ -3,7 +3,6 @@ #include #include -#include #include #include #include diff --git a/tutorial/tracking/model-based/generic-apriltag/tutorial-mb-generic-tracker-apriltag-webcam.cpp b/tutorial/tracking/model-based/generic-apriltag/tutorial-mb-generic-tracker-apriltag-webcam.cpp index 4f25ea7001..c49f2bfb03 100644 --- a/tutorial/tracking/model-based/generic-apriltag/tutorial-mb-generic-tracker-apriltag-webcam.cpp +++ b/tutorial/tracking/model-based/generic-apriltag/tutorial-mb-generic-tracker-apriltag-webcam.cpp @@ -193,11 +193,13 @@ int main(int argc, const char **argv) vpCameraParameters cam; bool camIsInit = false; +#if defined(VISP_HAVE_PUGIXML) vpXmlParserCamera parser; if (!opt_intrinsic_file.empty() && !opt_camera_name.empty()) { parser.parse(cam, opt_intrinsic_file, opt_camera_name, vpCameraParameters::perspectiveProjWithoutDistortion); camIsInit = true; } +#endif try { vpImage I; diff --git a/tutorial/tracking/model-based/generic-rgbd-blender/tutorial-mb-generic-tracker-rgbd-blender.cpp b/tutorial/tracking/model-based/generic-rgbd-blender/tutorial-mb-generic-tracker-rgbd-blender.cpp index 20f6dd453c..3cdde9ab07 100644 --- a/tutorial/tracking/model-based/generic-rgbd-blender/tutorial-mb-generic-tracker-rgbd-blender.cpp +++ b/tutorial/tracking/model-based/generic-rgbd-blender/tutorial-mb-generic-tracker-rgbd-blender.cpp @@ -10,7 +10,7 @@ #include #include -#if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGCODECS) +#if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGCODECS) && defined(VISP_HAVE_PUGIXML) namespace { bool read_data(unsigned int cpt, const std::string &video_color_images, const std::string &video_depth_images, @@ -225,6 +225,7 @@ int main(int argc, const char **argv) != vpXmlParserCamera::SEQUENCE_OK) { std::cout << "Cannot found intrinsics for camera " << depth_camera_name << std::endl; } + if (!opt_disable_depth) tracker.setCameraParameters(cam_color, cam_depth); else diff --git a/tutorial/tracking/model-based/generic-rgbd/CMakeLists.txt b/tutorial/tracking/model-based/generic-rgbd/CMakeLists.txt index 89030ec6d4..b3b12cb239 100644 --- a/tutorial/tracking/model-based/generic-rgbd/CMakeLists.txt +++ b/tutorial/tracking/model-based/generic-rgbd/CMakeLists.txt @@ -60,4 +60,5 @@ foreach(cpp ${tutorial_cpp}) endforeach() # Copy the data files -visp_copy_dir(tutorial-mb-generic-tracker-rgbd.cpp "${CMAKE_CURRENT_SOURCE_DIR}" data model) +visp_copy_dir(tutorial-mb-generic-tracker-rgbd.cpp "${CMAKE_CURRENT_SOURCE_DIR}" data) +visp_copy_dir(tutorial-mb-generic-tracker-rgbd.cpp "${CMAKE_CURRENT_SOURCE_DIR}" model) diff --git a/tutorial/tracking/model-based/generic-rgbd/tutorial-mb-generic-tracker-rgbd-realsense.cpp b/tutorial/tracking/model-based/generic-rgbd/tutorial-mb-generic-tracker-rgbd-realsense.cpp index d9997ada0c..f5e2a33140 100644 --- a/tutorial/tracking/model-based/generic-rgbd/tutorial-mb-generic-tracker-rgbd-realsense.cpp +++ b/tutorial/tracking/model-based/generic-rgbd/tutorial-mb-generic-tracker-rgbd-realsense.cpp @@ -3,10 +3,9 @@ #include -#if defined(VISP_HAVE_REALSENSE2) && defined(VISP_HAVE_OPENCV) +#if defined(VISP_HAVE_REALSENSE2) && defined(VISP_HAVE_OPENCV) && defined(VISP_HAVE_PUGIXML) #include #include -#include #include #include #include @@ -33,56 +32,71 @@ int main(int argc, char *argv[]) for (int i = 1; i < argc; i++) { if (std::string(argv[i]) == "--config_color" && i + 1 < argc) { config_color = std::string(argv[i + 1]); - } else if (std::string(argv[i]) == "--config_depth" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--config_depth" && i + 1 < argc) { config_depth = std::string(argv[i + 1]); - } else if (std::string(argv[i]) == "--model_color" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--model_color" && i + 1 < argc) { model_color = std::string(argv[i + 1]); - } else if (std::string(argv[i]) == "--model_depth" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--model_depth" && i + 1 < argc) { model_depth = std::string(argv[i + 1]); - } else if (std::string(argv[i]) == "--init_file" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--init_file" && i + 1 < argc) { init_file = std::string(argv[i + 1]); - } else if (std::string(argv[i]) == "--proj_error_threshold" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--proj_error_threshold" && i + 1 < argc) { proj_error_threshold = std::atof(argv[i + 1]); - } else if (std::string(argv[i]) == "--use_ogre") { + } + else if (std::string(argv[i]) == "--use_ogre") { use_ogre = true; - } else if (std::string(argv[i]) == "--use_scanline") { + } + else if (std::string(argv[i]) == "--use_scanline") { use_scanline = true; - } else if (std::string(argv[i]) == "--use_edges" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--use_edges" && i + 1 < argc) { use_edges = (std::atoi(argv[i + 1]) == 0 ? false : true); - } else if (std::string(argv[i]) == "--use_klt" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--use_klt" && i + 1 < argc) { use_klt = (std::atoi(argv[i + 1]) == 0 ? false : true); - } else if (std::string(argv[i]) == "--use_depth" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--use_depth" && i + 1 < argc) { use_depth = (std::atoi(argv[i + 1]) == 0 ? false : true); - } else if (std::string(argv[i]) == "--learn") { + } + else if (std::string(argv[i]) == "--learn") { learn = true; - } else if (std::string(argv[i]) == "--learning_data" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--learning_data" && i + 1 < argc) { learning_data = argv[i + 1]; - } else if (std::string(argv[i]) == "--auto_init") { + } + else if (std::string(argv[i]) == "--auto_init") { auto_init = true; - } else if (std::string(argv[i]) == "--display_proj_error") { + } + else if (std::string(argv[i]) == "--display_proj_error") { display_projection_error = true; - } else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") { + } + else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") { std::cout << "Usage: \n" - << argv[0] - << " [--model_color ] [--model_depth ]" - " [--config_color ] [--config_depth ]" - " [--init_file ] [--use_ogre] [--use_scanline]" - " [--proj_error_threshold (default: " - << proj_error_threshold - << ")]" - " [--use_edges <0|1> (default: 1)] [--use_klt <0|1> (default: 1)] [--use_depth <0|1> (default: 1)]" - " [--learn] [--auto_init] [--learning_data (default: learning/data-learned.bin)]" - " [--display_proj_error]" - << std::endl; + << argv[0] + << " [--model_color ] [--model_depth ]" + " [--config_color ] [--config_depth ]" + " [--init_file ] [--use_ogre] [--use_scanline]" + " [--proj_error_threshold (default: " + << proj_error_threshold + << ")]" + " [--use_edges <0|1> (default: 1)] [--use_klt <0|1> (default: 1)] [--use_depth <0|1> (default: 1)]" + " [--learn] [--auto_init] [--learning_data (default: learning/data-learned.bin)]" + " [--display_proj_error]" + << std::endl; std::cout << "\n** How to track a 4.2 cm width cube with manual initialization:\n" - << argv[0] << " --model_color model/cube/cube.cao --use_edges 1 --use_klt 1 --use_depth 1" << std::endl; + << argv[0] << " --model_color model/cube/cube.cao --use_edges 1 --use_klt 1 --use_depth 1" << std::endl; std::cout << "\n** How to learn the cube and create a learning database:\n" - << argv[0] << " --model_color model/cube/cube.cao --use_edges 1 --use_klt 1 --use_depth 1 --learn" - << std::endl; + << argv[0] << " --model_color model/cube/cube.cao --use_edges 1 --use_klt 1 --use_depth 1 --learn" + << std::endl; std::cout << "\n** How to track the cube with initialization from learning database:\n" - << argv[0] << " --model_color model/cube/cube.cao --use_edges 1 --use_klt 1 --use_depth 1 --auto_init" - << std::endl; + << argv[0] << " --model_color model/cube/cube.cao --use_edges 1 --use_klt 1 --use_depth 1 --auto_init" + << std::endl; return EXIT_SUCCESS; } @@ -112,15 +126,15 @@ int main(int argc, char *argv[]) std::cout << " Display proj. error: " << display_projection_error << std::endl; std::cout << "Config files: " << std::endl; std::cout << " Config color: " - << "\"" << config_color << "\"" << std::endl; + << "\"" << config_color << "\"" << std::endl; std::cout << " Config depth: " - << "\"" << config_depth << "\"" << std::endl; + << "\"" << config_depth << "\"" << std::endl; std::cout << " Model color : " - << "\"" << model_color << "\"" << std::endl; + << "\"" << model_color << "\"" << std::endl; std::cout << " Model depth : " - << "\"" << model_depth << "\"" << std::endl; + << "\"" << model_depth << "\"" << std::endl; std::cout << " Init file : " - << "\"" << init_file << "\"" << std::endl; + << "\"" << init_file << "\"" << std::endl; std::cout << "Learning options : " << std::endl; std::cout << " Learn : " << learn << std::endl; std::cout << " Auto init : " << auto_init << std::endl; @@ -133,8 +147,8 @@ int main(int argc, char *argv[]) if (config_color.empty() || config_depth.empty() || model_color.empty() || model_depth.empty() || init_file.empty()) { std::cout << "config_color.empty() || config_depth.empty() || model_color.empty() || model_depth.empty() || " - "init_file.empty()" - << std::endl; + "init_file.empty()" + << std::endl; return EXIT_FAILURE; } @@ -147,16 +161,17 @@ int main(int argc, char *argv[]) try { realsense.open(config); - } catch (const vpException &e) { + } + catch (const vpException &e) { std::cout << "Catch an exception: " << e.what() << std::endl; std::cout << "Check if the Realsense camera is connected..." << std::endl; return EXIT_SUCCESS; } vpCameraParameters cam_color = - realsense.getCameraParameters(RS2_STREAM_COLOR, vpCameraParameters::perspectiveProjWithoutDistortion); + realsense.getCameraParameters(RS2_STREAM_COLOR, vpCameraParameters::perspectiveProjWithoutDistortion); vpCameraParameters cam_depth = - realsense.getCameraParameters(RS2_STREAM_DEPTH, vpCameraParameters::perspectiveProjWithoutDistortion); + realsense.getCameraParameters(RS2_STREAM_DEPTH, vpCameraParameters::perspectiveProjWithoutDistortion); std::cout << "Sensor internal camera parameters for color camera: " << cam_color << std::endl; std::cout << "Sensor internal camera parameters for depth camera: " << cam_depth << std::endl; @@ -239,11 +254,13 @@ int main(int argc, char *argv[]) mapOfImages["Camera2"] = &I_depth; mapOfInitFiles["Camera1"] = init_file; tracker.setCameraParameters(cam_color, cam_depth); - } else if (use_edges || use_klt) { + } + else if (use_edges || use_klt) { tracker.loadConfigFile(config_color); tracker.loadModel(model_color); tracker.setCameraParameters(cam_color); - } else if (use_depth) { + } + else if (use_depth) { tracker.loadConfigFile(config_depth); tracker.loadModel(model_depth); tracker.setCameraParameters(cam_depth); @@ -288,7 +305,8 @@ int main(int argc, char *argv[]) return EXIT_FAILURE; } keypoint.loadLearningData(learning_data, true); - } else { + } + else { if ((use_edges || use_klt) && use_depth) tracker.initClick(mapOfImages, mapOfInitFiles, true); else if (use_edges || use_klt) @@ -335,9 +353,11 @@ int main(int argc, char *argv[]) mapOfPointclouds["Camera2"] = &pointcloud; mapOfWidths["Camera2"] = width; mapOfHeights["Camera2"] = height; - } else if (use_edges || use_klt) { + } + else if (use_edges || use_klt) { mapOfImages["Camera"] = &I_gray; - } else if (use_depth) { + } + else if (use_depth) { mapOfPointclouds["Camera"] = &pointcloud; mapOfWidths["Camera"] = width; mapOfHeights["Camera"] = height; @@ -351,12 +371,15 @@ int main(int argc, char *argv[]) mapOfCameraPoses["Camera1"] = cMo; mapOfCameraPoses["Camera2"] = depth_M_color * cMo; tracker.initFromPose(mapOfImages, mapOfCameraPoses); - } else if (use_edges || use_klt) { + } + else if (use_edges || use_klt) { tracker.initFromPose(I_gray, cMo); - } else if (use_depth) { + } + else if (use_depth) { tracker.initFromPose(I_depth, depth_M_color * cMo); } - } else { + } + else { if (use_edges || use_klt) { vpDisplay::flush(I_gray); } @@ -377,12 +400,15 @@ int main(int argc, char *argv[]) } if ((use_edges || use_klt) && use_depth) { tracker.track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights); - } else if (use_edges || use_klt) { + } + else if (use_edges || use_klt) { tracker.track(I_gray); - } else if (use_depth) { + } + else if (use_depth) { tracker.track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights); } - } catch (const vpException &e) { + } + catch (const vpException &e) { std::cout << "Tracker exception: " << e.getStringMessage() << std::endl; tracking_failed = true; if (auto_init) { @@ -399,7 +425,8 @@ int main(int argc, char *argv[]) if (tracker.getTrackerType() & vpMbGenericTracker::EDGE_TRACKER) { // Check tracking errors proj_error = tracker.getProjectionError(); - } else { + } + else { proj_error = tracker.computeCurrentProjectionError(I_gray, cMo, cam_color); } @@ -418,10 +445,12 @@ int main(int argc, char *argv[]) tracker.display(I_gray, I_depth, cMo, depth_M_color * cMo, cam_color, cam_depth, vpColor::red, 3); vpDisplay::displayFrame(I_gray, cMo, cam_color, 0.05, vpColor::none, 3); vpDisplay::displayFrame(I_depth, depth_M_color * cMo, cam_depth, 0.05, vpColor::none, 3); - } else if (use_edges || use_klt) { + } + else if (use_edges || use_klt) { tracker.display(I_gray, cMo, cam_color, vpColor::red, 3); vpDisplay::displayFrame(I_gray, cMo, cam_color, 0.05, vpColor::none, 3); - } else if (use_depth) { + } + else if (use_depth) { tracker.display(I_depth, cMo, cam_depth, vpColor::red, 3); vpDisplay::displayFrame(I_depth, cMo, cam_depth, 0.05, vpColor::none, 3); } @@ -434,7 +463,7 @@ int main(int argc, char *argv[]) { std::stringstream ss; ss << "Features: edges " << tracker.getNbFeaturesEdge() << ", klt " << tracker.getNbFeaturesKlt() - << ", depth " << tracker.getNbFeaturesDepthDense(); + << ", depth " << tracker.getNbFeaturesDepthDense(); vpDisplay::displayText(I_gray, I_gray.getHeight() - 30, 20, ss.str(), vpColor::red); } } @@ -457,9 +486,11 @@ int main(int argc, char *argv[]) if (vpDisplay::getClick(I_gray, button, false)) { if (button == vpMouseButton::button3) { quit = true; - } else if (button == vpMouseButton::button1 && learn) { + } + else if (button == vpMouseButton::button1 && learn) { learn_position = true; - } else if (button == vpMouseButton::button1 && auto_init && !learn) { + } + else if (button == vpMouseButton::button1 && auto_init && !learn) { run_auto_init = true; } } @@ -507,14 +538,15 @@ int main(int argc, char *argv[]) std::cout << "Save learning file: " << learning_data << std::endl; keypoint.saveLearningData(learning_data, true, true); } - } catch (const vpException &e) { + } + catch (const vpException &e) { std::cout << "Catch an exception: " << e.what() << std::endl; } if (!times_vec.empty()) { std::cout << "\nProcessing time, Mean: " << vpMath::getMean(times_vec) - << " ms ; Median: " << vpMath::getMedian(times_vec) << " ; Std: " << vpMath::getStdev(times_vec) << " ms" - << std::endl; + << " ms ; Median: " << vpMath::getMedian(times_vec) << " ; Std: " << vpMath::getStdev(times_vec) << " ms" + << std::endl; } return EXIT_SUCCESS; diff --git a/tutorial/tracking/model-based/generic-rgbd/tutorial-mb-generic-tracker-rgbd-structure-core.cpp b/tutorial/tracking/model-based/generic-rgbd/tutorial-mb-generic-tracker-rgbd-structure-core.cpp index 623f510f33..408fd79777 100644 --- a/tutorial/tracking/model-based/generic-rgbd/tutorial-mb-generic-tracker-rgbd-structure-core.cpp +++ b/tutorial/tracking/model-based/generic-rgbd/tutorial-mb-generic-tracker-rgbd-structure-core.cpp @@ -3,7 +3,7 @@ #include -#if defined(VISP_HAVE_OCCIPITAL_STRUCTURE) && defined(VISP_HAVE_OPENCV) +#if defined(VISP_HAVE_OCCIPITAL_STRUCTURE) && defined(VISP_HAVE_OPENCV) && defined(VISP_HAVE_PUGIXML) #include #include #include @@ -33,56 +33,71 @@ int main(int argc, char *argv[]) for (int i = 1; i < argc; i++) { if (std::string(argv[i]) == "--config_color" && i + 1 < argc) { config_color = std::string(argv[i + 1]); - } else if (std::string(argv[i]) == "--config_depth" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--config_depth" && i + 1 < argc) { config_depth = std::string(argv[i + 1]); - } else if (std::string(argv[i]) == "--model_color" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--model_color" && i + 1 < argc) { model_color = std::string(argv[i + 1]); - } else if (std::string(argv[i]) == "--model_depth" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--model_depth" && i + 1 < argc) { model_depth = std::string(argv[i + 1]); - } else if (std::string(argv[i]) == "--init_file" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--init_file" && i + 1 < argc) { init_file = std::string(argv[i + 1]); - } else if (std::string(argv[i]) == "--proj_error_threshold" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--proj_error_threshold" && i + 1 < argc) { proj_error_threshold = std::atof(argv[i + 1]); - } else if (std::string(argv[i]) == "--use_ogre") { + } + else if (std::string(argv[i]) == "--use_ogre") { use_ogre = true; - } else if (std::string(argv[i]) == "--use_scanline") { + } + else if (std::string(argv[i]) == "--use_scanline") { use_scanline = true; - } else if (std::string(argv[i]) == "--use_edges" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--use_edges" && i + 1 < argc) { use_edges = (std::atoi(argv[i + 1]) == 0 ? false : true); - } else if (std::string(argv[i]) == "--use_klt" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--use_klt" && i + 1 < argc) { use_klt = (std::atoi(argv[i + 1]) == 0 ? false : true); - } else if (std::string(argv[i]) == "--use_depth" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--use_depth" && i + 1 < argc) { use_depth = (std::atoi(argv[i + 1]) == 0 ? false : true); - } else if (std::string(argv[i]) == "--learn") { + } + else if (std::string(argv[i]) == "--learn") { learn = true; - } else if (std::string(argv[i]) == "--learning_data" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--learning_data" && i + 1 < argc) { learning_data = argv[i + 1]; - } else if (std::string(argv[i]) == "--auto_init") { + } + else if (std::string(argv[i]) == "--auto_init") { auto_init = true; - } else if (std::string(argv[i]) == "--display_proj_error") { + } + else if (std::string(argv[i]) == "--display_proj_error") { display_projection_error = true; - } else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") { + } + else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") { std::cout << "Usage: \n" - << argv[0] - << " [--model_color ] [--model_depth ]" - " [--config_color ] [--config_depth ]" - " [--init_file ] [--use_ogre] [--use_scanline]" - " [--proj_error_threshold (default: " - << proj_error_threshold - << ")]" - " [--use_edges <0|1> (default: 1)] [--use_klt <0|1> (default: 1)] [--use_depth <0|1> (default: 1)]" - " [--learn] [--auto_init] [--learning_data (default: learning/data-learned.bin)]" - " [--display_proj_error]" - << std::endl; + << argv[0] + << " [--model_color ] [--model_depth ]" + " [--config_color ] [--config_depth ]" + " [--init_file ] [--use_ogre] [--use_scanline]" + " [--proj_error_threshold (default: " + << proj_error_threshold + << ")]" + " [--use_edges <0|1> (default: 1)] [--use_klt <0|1> (default: 1)] [--use_depth <0|1> (default: 1)]" + " [--learn] [--auto_init] [--learning_data (default: learning/data-learned.bin)]" + " [--display_proj_error]" + << std::endl; std::cout << "\n** How to track a 4.2 cm width cube with manual initialization:\n" - << argv[0] << " --model_color model/cube/cube.cao --use_edges 1 --use_klt 1 --use_depth 1" << std::endl; + << argv[0] << " --model_color model/cube/cube.cao --use_edges 1 --use_klt 1 --use_depth 1" << std::endl; std::cout << "\n** How to learn the cube and create a learning database:\n" - << argv[0] << " --model_color model/cube/cube.cao --use_edges 1 --use_klt 1 --use_depth 1 --learn" - << std::endl; + << argv[0] << " --model_color model/cube/cube.cao --use_edges 1 --use_klt 1 --use_depth 1 --learn" + << std::endl; std::cout << "\n** How to track the cube with initialization from learning database:\n" - << argv[0] << " --model_color model/cube/cube.cao --use_edges 1 --use_klt 1 --use_depth 1 --auto_init" - << std::endl; + << argv[0] << " --model_color model/cube/cube.cao --use_edges 1 --use_klt 1 --use_depth 1 --auto_init" + << std::endl; return EXIT_SUCCESS; } @@ -112,15 +127,15 @@ int main(int argc, char *argv[]) std::cout << " Display proj. error: " << display_projection_error << std::endl; std::cout << "Config files: " << std::endl; std::cout << " Config color: " - << "\"" << config_color << "\"" << std::endl; + << "\"" << config_color << "\"" << std::endl; std::cout << " Config depth: " - << "\"" << config_depth << "\"" << std::endl; + << "\"" << config_depth << "\"" << std::endl; std::cout << " Model color : " - << "\"" << model_color << "\"" << std::endl; + << "\"" << model_color << "\"" << std::endl; std::cout << " Model depth : " - << "\"" << model_depth << "\"" << std::endl; + << "\"" << model_depth << "\"" << std::endl; std::cout << " Init file : " - << "\"" << init_file << "\"" << std::endl; + << "\"" << init_file << "\"" << std::endl; std::cout << "Learning options : " << std::endl; std::cout << " Learn : " << learn << std::endl; std::cout << " Auto init : " << auto_init << std::endl; @@ -133,8 +148,8 @@ int main(int argc, char *argv[]) if (config_color.empty() || config_depth.empty() || model_color.empty() || model_depth.empty() || init_file.empty()) { std::cout << "config_color.empty() || config_depth.empty() || model_color.empty() || model_depth.empty() || " - "init_file.empty()" - << std::endl; + "init_file.empty()" + << std::endl; return EXIT_FAILURE; } @@ -146,7 +161,8 @@ int main(int argc, char *argv[]) try { sc.open(settings); - } catch (const vpException &e) { + } + catch (const vpException &e) { std::cout << "Catch an exception: " << e.what() << std::endl; std::cout << "Check if the Structure Core camera is connected..." << std::endl; return EXIT_SUCCESS; @@ -236,11 +252,13 @@ int main(int argc, char *argv[]) mapOfImages["Camera2"] = &I_depth; mapOfInitFiles["Camera1"] = init_file; tracker.setCameraParameters(cam_color, cam_depth); - } else if (use_edges || use_klt) { + } + else if (use_edges || use_klt) { tracker.loadConfigFile(config_color); tracker.loadModel(model_color); tracker.setCameraParameters(cam_color); - } else if (use_depth) { + } + else if (use_depth) { tracker.loadConfigFile(config_depth); tracker.loadModel(model_depth); tracker.setCameraParameters(cam_depth); @@ -285,7 +303,8 @@ int main(int argc, char *argv[]) return EXIT_FAILURE; } keypoint.loadLearningData(learning_data, true); - } else { + } + else { if ((use_edges || use_klt) && use_depth) tracker.initClick(mapOfImages, mapOfInitFiles, true); else if (use_edges || use_klt) @@ -332,9 +351,11 @@ int main(int argc, char *argv[]) mapOfPointclouds["Camera2"] = &pointcloud; mapOfWidths["Camera2"] = width; mapOfHeights["Camera2"] = height; - } else if (use_edges || use_klt) { + } + else if (use_edges || use_klt) { mapOfImages["Camera"] = &I_gray; - } else if (use_depth) { + } + else if (use_depth) { mapOfPointclouds["Camera"] = &pointcloud; mapOfWidths["Camera"] = width; mapOfHeights["Camera"] = height; @@ -348,12 +369,15 @@ int main(int argc, char *argv[]) mapOfCameraPoses["Camera1"] = cMo; mapOfCameraPoses["Camera2"] = depth_M_color * cMo; tracker.initFromPose(mapOfImages, mapOfCameraPoses); - } else if (use_edges || use_klt) { + } + else if (use_edges || use_klt) { tracker.initFromPose(I_gray, cMo); - } else if (use_depth) { + } + else if (use_depth) { tracker.initFromPose(I_depth, depth_M_color * cMo); } - } else { + } + else { if (use_edges || use_klt) { vpDisplay::flush(I_gray); } @@ -374,12 +398,15 @@ int main(int argc, char *argv[]) } if ((use_edges || use_klt) && use_depth) { tracker.track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights); - } else if (use_edges || use_klt) { + } + else if (use_edges || use_klt) { tracker.track(I_gray); - } else if (use_depth) { + } + else if (use_depth) { tracker.track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights); } - } catch (const vpException &e) { + } + catch (const vpException &e) { std::cout << "Tracker exception: " << e.getStringMessage() << std::endl; tracking_failed = true; if (auto_init) { @@ -396,7 +423,8 @@ int main(int argc, char *argv[]) if (tracker.getTrackerType() & vpMbGenericTracker::EDGE_TRACKER) { // Check tracking errors proj_error = tracker.getProjectionError(); - } else { + } + else { proj_error = tracker.computeCurrentProjectionError(I_gray, cMo, cam_color); } @@ -415,10 +443,12 @@ int main(int argc, char *argv[]) tracker.display(I_gray, I_depth, cMo, depth_M_color * cMo, cam_color, cam_depth, vpColor::red, 3); vpDisplay::displayFrame(I_gray, cMo, cam_color, 0.05, vpColor::none, 3); vpDisplay::displayFrame(I_depth, depth_M_color * cMo, cam_depth, 0.05, vpColor::none, 3); - } else if (use_edges || use_klt) { + } + else if (use_edges || use_klt) { tracker.display(I_gray, cMo, cam_color, vpColor::red, 3); vpDisplay::displayFrame(I_gray, cMo, cam_color, 0.05, vpColor::none, 3); - } else if (use_depth) { + } + else if (use_depth) { tracker.display(I_depth, cMo, cam_depth, vpColor::red, 3); vpDisplay::displayFrame(I_depth, cMo, cam_depth, 0.05, vpColor::none, 3); } @@ -431,7 +461,7 @@ int main(int argc, char *argv[]) { std::stringstream ss; ss << "Features: edges " << tracker.getNbFeaturesEdge() << ", klt " << tracker.getNbFeaturesKlt() - << ", depth " << tracker.getNbFeaturesDepthDense(); + << ", depth " << tracker.getNbFeaturesDepthDense(); vpDisplay::displayText(I_gray, I_gray.getHeight() - 30, 20, ss.str(), vpColor::red); } } @@ -454,9 +484,11 @@ int main(int argc, char *argv[]) if (vpDisplay::getClick(I_gray, button, false)) { if (button == vpMouseButton::button3) { quit = true; - } else if (button == vpMouseButton::button1 && learn) { + } + else if (button == vpMouseButton::button1 && learn) { learn_position = true; - } else if (button == vpMouseButton::button1 && auto_init && !learn) { + } + else if (button == vpMouseButton::button1 && auto_init && !learn) { run_auto_init = true; } } @@ -504,14 +536,15 @@ int main(int argc, char *argv[]) std::cout << "Save learning file: " << learning_data << std::endl; keypoint.saveLearningData(learning_data, true, true); } - } catch (const vpException &e) { + } + catch (const vpException &e) { std::cout << "Catch an exception: " << e.what() << std::endl; } if (!times_vec.empty()) { std::cout << "\nProcessing time, Mean: " << vpMath::getMean(times_vec) - << " ms ; Median: " << vpMath::getMedian(times_vec) << " ; Std: " << vpMath::getStdev(times_vec) << " ms" - << std::endl; + << " ms ; Median: " << vpMath::getMedian(times_vec) << " ; Std: " << vpMath::getStdev(times_vec) << " ms" + << std::endl; } return EXIT_SUCCESS; diff --git a/tutorial/tracking/model-based/generic-rgbd/tutorial-mb-generic-tracker-rgbd.cpp b/tutorial/tracking/model-based/generic-rgbd/tutorial-mb-generic-tracker-rgbd.cpp index 017e2b1cc3..8013831c88 100644 --- a/tutorial/tracking/model-based/generic-rgbd/tutorial-mb-generic-tracker-rgbd.cpp +++ b/tutorial/tracking/model-based/generic-rgbd/tutorial-mb-generic-tracker-rgbd.cpp @@ -3,7 +3,6 @@ #include #include -#include #include #include #include @@ -17,7 +16,8 @@ namespace { -struct vpRealsenseIntrinsics_t { +struct vpRealsenseIntrinsics_t +{ float ppx; /**< Horizontal coordinate of the principal point of the image, as a pixel offset from the left edge */ float ppy; /**< Vertical coordinate of the principal point of the image, as @@ -111,7 +111,7 @@ bool read_data(unsigned int cpt, const std::string &input_directory, vpImagepoints[(size_t)(i * width + j)].x = point[0]; pointcloud->points[(size_t)(i * width + j)].y = point[1]; @@ -135,30 +135,37 @@ int main(int argc, char *argv[]) for (int i = 1; i < argc; i++) { if (std::string(argv[i]) == "--input_directory" && i + 1 < argc) { input_directory = std::string(argv[i + 1]); - } else if (std::string(argv[i]) == "--config_color" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--config_color" && i + 1 < argc) { config_color = std::string(argv[i + 1]); - } else if (std::string(argv[i]) == "--config_depth" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--config_depth" && i + 1 < argc) { config_depth = std::string(argv[i + 1]); - } else if (std::string(argv[i]) == "--model_color" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--model_color" && i + 1 < argc) { model_color = std::string(argv[i + 1]); - } else if (std::string(argv[i]) == "--model_depth" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--model_depth" && i + 1 < argc) { model_depth = std::string(argv[i + 1]); - } else if (std::string(argv[i]) == "--init_file" && i + 1 < argc) { + } + else if (std::string(argv[i]) == "--init_file" && i + 1 < argc) { init_file = std::string(argv[i + 1]); - } else if (std::string(argv[i]) == "--disable_depth") { + } + else if (std::string(argv[i]) == "--disable_depth") { disable_depth = true; - } else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") { + } + else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") { std::cout << "Usage: \n" - << argv[0] - << " --input_directory --config_color --config_depth " - " --model_color --model_depth --init_file --disable_depth" - << std::endl; + << argv[0] + << " --input_directory --config_color --config_depth " + " --model_color --model_depth --init_file --disable_depth" + << std::endl; std::cout - << "\nExample:\n" - << argv[0] - << " --config_color model/cube/cube.xml --config_depth model/cube/cube.xml" - " --model_color model/cube/cube.cao --model_depth model/cube/cube.cao --init_file model/cube/cube.init\n" - << std::endl; + << "\nExample:\n" + << argv[0] + << " --config_color model/cube/cube.xml --config_depth model/cube/cube.xml" + " --model_color model/cube/cube.cao --model_depth model/cube/cube.cao --init_file model/cube/cube.init\n" + << std::endl; return EXIT_SUCCESS; } } @@ -179,17 +186,17 @@ int main(int argc, char *argv[]) #endif std::cout << "Config files: " << std::endl; std::cout << " Input directory: " - << "\"" << input_directory << "\"" << std::endl; + << "\"" << input_directory << "\"" << std::endl; std::cout << " Config color: " - << "\"" << config_color << "\"" << std::endl; + << "\"" << config_color << "\"" << std::endl; std::cout << " Config depth: " - << "\"" << config_depth << "\"" << std::endl; + << "\"" << config_depth << "\"" << std::endl; std::cout << " Model color : " - << "\"" << model_color << "\"" << std::endl; + << "\"" << model_color << "\"" << std::endl; std::cout << " Model depth : " - << "\"" << model_depth << "\"" << std::endl; + << "\"" << model_depth << "\"" << std::endl; std::cout << " Init file : " - << "\"" << init_file << "\"" << std::endl; + << "\"" << init_file << "\"" << std::endl; vpImage I_color; //! [Images] @@ -232,9 +239,60 @@ int main(int argc, char *argv[]) trackerTypes.push_back(vpMbGenericTracker::DEPTH_DENSE_TRACKER); vpMbGenericTracker tracker(trackerTypes); //! [Constructor] +#if defined(VISP_HAVE_PUGIXML) //! [Load config file] tracker.loadConfigFile(config_color, config_depth); //! [Load config file] +#else + { + vpCameraParameters cam_color, cam_depth; + cam_color.initPersProjWithoutDistortion(614.9, 614.9, 320.2, 241.5); + cam_depth.initPersProjWithoutDistortion(384.0, 384.0, 320.5, 235.6); + tracker.setCameraParameters(cam_color, cam_depth); + } + + // Edge + vpMe me; + me.setMaskSize(5); + me.setMaskNumber(180); + me.setRange(7); + me.setLikelihoodThresholdType(vpMe::NORMALIZED_THRESHOLD); + me.setThreshold(10); + me.setMu1(0.5); + me.setMu2(0.5); + me.setSampleStep(4); + tracker.setMovingEdge(me); + + // Klt +#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO) + vpKltOpencv klt; + tracker.setKltMaskBorder(5); + klt.setMaxFeatures(300); + klt.setWindowSize(5); + klt.setQuality(0.01); + klt.setMinDistance(5); + klt.setHarrisFreeParameter(0.01); + klt.setBlockSize(3); + klt.setPyramidLevels(3); + + tracker.setKltOpencv(klt); +#endif + + // Depth + tracker.setDepthNormalFeatureEstimationMethod(vpMbtFaceDepthNormal::ROBUST_FEATURE_ESTIMATION); + tracker.setDepthNormalPclPlaneEstimationMethod(2); + tracker.setDepthNormalPclPlaneEstimationRansacMaxIter(200); + tracker.setDepthNormalPclPlaneEstimationRansacThreshold(0.001); + tracker.setDepthNormalSamplingStep(2, 2); + + tracker.setDepthDenseSamplingStep(4, 4); + + tracker.setAngleAppear(vpMath::rad(80.0)); + tracker.setAngleDisappear(vpMath::rad(85.0)); + tracker.setNearClippingDistance(0.001); + tracker.setFarClippingDistance(5.0); + tracker.setClipping(tracker.getClipping() | vpMbtPolygon::FOV_CLIPPING); +#endif //! [Load cao] tracker.loadModel(model_color, model_depth); //! [Load cao] @@ -326,7 +384,7 @@ int main(int argc, char *argv[]) { std::stringstream ss; ss << "Features: edges " << tracker.getNbFeaturesEdge() << ", klt " << tracker.getNbFeaturesKlt() << ", depth " - << tracker.getNbFeaturesDepthDense(); + << tracker.getNbFeaturesDepthDense(); vpDisplay::displayText(I_gray, I_gray.getHeight() - 30, 20, ss.str(), vpColor::red); } @@ -340,13 +398,14 @@ int main(int argc, char *argv[]) frame_cpt++; } - } catch (const vpException &e) { + } + catch (const vpException &e) { std::cout << "Catch exception: " << e.getStringMessage() << std::endl; } std::cout << "\nProcessing time, Mean: " << vpMath::getMean(times_vec) - << " ms ; Median: " << vpMath::getMedian(times_vec) << " ; Std: " << vpMath::getStdev(times_vec) << " ms" - << std::endl; + << " ms ; Median: " << vpMath::getMedian(times_vec) << " ; Std: " << vpMath::getStdev(times_vec) << " ms" + << std::endl; vpDisplay::displayText(I_gray, 60, 20, "Click to quit", vpColor::red); vpDisplay::flush(I_gray); @@ -358,8 +417,8 @@ int main(int argc, char *argv[]) int main() { std::cout << "To run this tutorial, ViSP should be build with PCL library." - " Install libpcl, configure and build again ViSP..." - << std::endl; + " Install libpcl, configure and build again ViSP..." + << std::endl; return EXIT_SUCCESS; } #endif diff --git a/tutorial/tracking/model-based/generic-stereo/tutorial-mb-generic-tracker-stereo-mono.cpp b/tutorial/tracking/model-based/generic-stereo/tutorial-mb-generic-tracker-stereo-mono.cpp index 2d221f28a7..d891b94eb6 100644 --- a/tutorial/tracking/model-based/generic-stereo/tutorial-mb-generic-tracker-stereo-mono.cpp +++ b/tutorial/tracking/model-based/generic-stereo/tutorial-mb-generic-tracker-stereo-mono.cpp @@ -79,10 +79,12 @@ int main(int argc, char **argv) #endif //! [Set parameters] + +#if defined(VISP_HAVE_PUGIXML) //! [Load config file] tracker.loadConfigFile(objectname + ".xml"); //! [Load config file] -#if 0 +#else // Corresponding parameters manually set to have an example code if (opt_tracker == 1 || opt_tracker == 3) { vpMe me; diff --git a/tutorial/tracking/model-based/generic-stereo/tutorial-mb-generic-tracker-stereo.cpp b/tutorial/tracking/model-based/generic-stereo/tutorial-mb-generic-tracker-stereo.cpp index 22d729c08d..8ed5077a6a 100644 --- a/tutorial/tracking/model-based/generic-stereo/tutorial-mb-generic-tracker-stereo.cpp +++ b/tutorial/tracking/model-based/generic-stereo/tutorial-mb-generic-tracker-stereo.cpp @@ -13,7 +13,7 @@ int main(int argc, char **argv) { -#if defined(VISP_HAVE_OPENCV) +#if defined(VISP_HAVE_OPENCV) && defined(VISP_HAVE_PUGIXML) try { std::string opt_videoname_left = "teabox_left.mp4"; std::string opt_videoname_right = "teabox_right.mp4"; @@ -24,23 +24,25 @@ int main(int argc, char **argv) if (std::string(argv[i]) == "--name" && i + 2 < argc) { opt_videoname_left = std::string(argv[i + 1]); opt_videoname_right = std::string(argv[i + 2]); - } else if (std::string(argv[i]) == "--tracker" && i + 2 < argc) { + } + else if (std::string(argv[i]) == "--tracker" && i + 2 < argc) { opt_tracker1 = atoi(argv[i + 1]); opt_tracker2 = atoi(argv[i + 2]); - } else if (std::string(argv[i]) == "--help") { + } + else if (std::string(argv[i]) == "--help") { std::cout << "\nUsage: " << argv[0] - << " [--name