From eca04466f88d89f9bbe7c73d5395062f4a9dd17c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Martin=20G=C3=BCnther?= Date: Tue, 5 Nov 2019 14:18:22 +0100 Subject: [PATCH 01/75] camera_validator: Use ApproximateTimeSynchronizer --- aslam_offline_calibration/kalibr/python/kalibr_camera_validator | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_camera_validator b/aslam_offline_calibration/kalibr/python/kalibr_camera_validator index 759154eee..b61572c64 100755 --- a/aslam_offline_calibration/kalibr/python/kalibr_camera_validator +++ b/aslam_offline_calibration/kalibr/python/kalibr_camera_validator @@ -100,7 +100,7 @@ class CameraChainValidator(object): edge["A"][cidx_dest] = self.prepareStereoRectificationMaps(cidx_src, cidx_dest) #register the callback for the synchronized images - sync_sub = message_filters.TimeSynchronizer([val.image_sub for val in self.monovalidators],1) + sync_sub = message_filters.ApproximateTimeSynchronizer([val.image_sub for val in self.monovalidators], 10, 0.02) sync_sub.registerCallback(self.synchronizedCallback) #initialize message throttler From ef81bb9a91cd1e9739b1c2eaddb07ee66dc613c9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Martin=20G=C3=BCnther?= Date: Tue, 5 Nov 2019 14:22:35 +0100 Subject: [PATCH 02/75] camera_validator: Add support for bgra8 encoding --- aslam_offline_calibration/kalibr/python/kalibr_camera_validator | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_camera_validator b/aslam_offline_calibration/kalibr/python/kalibr_camera_validator index b61572c64..6276f27d1 100755 --- a/aslam_offline_calibration/kalibr/python/kalibr_camera_validator +++ b/aslam_offline_calibration/kalibr/python/kalibr_camera_validator @@ -120,7 +120,7 @@ class CameraChainValidator(object): #convert image to numpy try: - if (msg.encoding == "rgb8"): + if (msg.encoding == "rgb8" or msg.encoding == "bgra8"): cv_image = np.squeeze(np.array(self.bridge.imgmsg_to_cv2(msg, "mono8"))) else: cv_image = self.bridge.imgmsg_to_cv2(msg) From 71311022026bb2df7776a035af0d1e889422cc06 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Martin=20G=C3=BCnther?= Date: Tue, 5 Nov 2019 14:23:13 +0100 Subject: [PATCH 03/75] camera_validator: Fix thread getting stuck (empty windows) Moved the creation of the cv2.namedWindow from the MonoCameraValidator constructor to the callback function. Otherwise, this caused the following error: QObject::startTimer: Timers cannot be started from another thread ... with the result that the image windows remained black and the node became stuck. Fixes #201 . --- .../kalibr/python/kalibr_camera_validator | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_camera_validator b/aslam_offline_calibration/kalibr/python/kalibr_camera_validator index 6276f27d1..ae5257713 100755 --- a/aslam_offline_calibration/kalibr/python/kalibr_camera_validator +++ b/aslam_offline_calibration/kalibr/python/kalibr_camera_validator @@ -192,7 +192,7 @@ class CameraChainValidator(object): def generatePairView(self, camAnr, camBnr): #prepare the window windowName = "Rectified view (cam{0} and cam{1})".format(camAnr, camBnr) - cv2.namedWindow(windowName, 1) + cv2.namedWindow(windowName, cv2.WINDOW_AUTOSIZE) #get the mono validators for each cam camA = self.monovalidators[camAnr] @@ -378,7 +378,6 @@ class MonoCameraValidator(object): self.topic = camConfig.getRosTopic() self.windowName = "Camera: {0}".format(self.topic) - cv2.namedWindow(self.windowName, 1) #register the cam topic to the message synchronizer self.image_sub = message_filters.Subscriber(self.topic, Image) @@ -400,6 +399,7 @@ class MonoCameraValidator(object): def generateMonoview(self, np_image, observation, obs_valid): + cv2.namedWindow(self.windowName, cv2.WINDOW_AUTOSIZE) np_image = cv2.cvtColor(np_image, cv2.COLOR_GRAY2BGR) if obs_valid: From fc62fc75207eebe859d91341bc029d91cc47e922 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Martin=20G=C3=BCnther?= Date: Mon, 4 May 2020 16:47:09 +0200 Subject: [PATCH 04/75] Fix off-by-one error in kalibr_visualize_distortion --- .../kalibr/python/kalibr_visualize_distortion | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_visualize_distortion b/aslam_offline_calibration/kalibr/python/kalibr_visualize_distortion index 2fd34617f..61561e5e9 100755 --- a/aslam_offline_calibration/kalibr/python/kalibr_visualize_distortion +++ b/aslam_offline_calibration/kalibr/python/kalibr_visualize_distortion @@ -94,8 +94,8 @@ def distort(image, distortion_model, distortion_coeffs, focal_lengths, distortion_coeffs) distorted = denormalize(distorted_normalized, focal_lengths, principal_point) - if (int(distorted[0]) < 0 or int(distorted[0]) > image.shape[0] or - int(distorted[1]) < 0 or int(distorted[1]) > image.shape[1]): + if (int(distorted[0]) < 0 or int(distorted[0]) >= image.shape[0] or + int(distorted[1]) < 0 or int(distorted[1]) >= image.shape[1]): continue distorted_image[int(distorted[0]), int(distorted[1])] = image[i,j] return distorted_image From 9ad32bd10f53b9ee1e3502b887d2deb50a5f7a77 Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Tue, 15 Dec 2020 11:57:38 +0000 Subject: [PATCH 05/75] set correct python 3 version --- .../python_module/cmake/add_python_export_library.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Schweizer-Messer/python_module/cmake/add_python_export_library.cmake b/Schweizer-Messer/python_module/cmake/add_python_export_library.cmake index 99c48df02..dfd43d807 100755 --- a/Schweizer-Messer/python_module/cmake/add_python_export_library.cmake +++ b/Schweizer-Messer/python_module/cmake/add_python_export_library.cmake @@ -86,7 +86,7 @@ ${SETUP_PY_TEXT} list(APPEND BOOST_COMPONENTS python27) endif() else() - list(APPEND BOOST_COMPONENTS python3) + list(APPEND BOOST_COMPONENTS python38) endif() find_package(Boost REQUIRED COMPONENTS ${BOOST_COMPONENTS}) From 05f7895b6a885b8f1cf041fe4fc0a6336a96e3f1 Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Tue, 15 Dec 2020 13:15:48 +0000 Subject: [PATCH 06/75] port to OpenCV 4 API --- .../src/GridCalibrationTargetCheckerboard.cpp | 10 +++++----- .../src/GridCalibrationTargetCirclegrid.cpp | 8 ++++---- aslam_cv/aslam_cameras/src/GridDetector.cpp | 4 ++-- aslam_cv/aslam_cameras/src/ImageMask.cpp | 2 +- .../src/GridCalibrationTargetAprilgrid.cpp | 18 +++++++++--------- .../aslam/implementation/aslamcv_helper.hpp | 13 ++++++------- .../src/example/apriltags_demo.cpp | 12 ++++++------ 7 files changed, 33 insertions(+), 34 deletions(-) diff --git a/aslam_cv/aslam_cameras/src/GridCalibrationTargetCheckerboard.cpp b/aslam_cv/aslam_cameras/src/GridCalibrationTargetCheckerboard.cpp index bf95aafee..229c11305 100644 --- a/aslam_cv/aslam_cameras/src/GridCalibrationTargetCheckerboard.cpp +++ b/aslam_cv/aslam_cameras/src/GridCalibrationTargetCheckerboard.cpp @@ -50,8 +50,8 @@ GridCalibrationTargetCheckerboard::GridCalibrationTargetCheckerboard( void GridCalibrationTargetCheckerboard::initialize() { if (_options.showExtractionVideo) { - cv::namedWindow("Checkerboard corners", CV_WINDOW_AUTOSIZE); - cvStartWindowThread(); + cv::namedWindow("Checkerboard corners", cv::WINDOW_AUTOSIZE); + cv::startWindowThread(); } } @@ -97,21 +97,21 @@ bool GridCalibrationTargetCheckerboard::computeObservation(const cv::Mat & image if (_options.doSubpixelRefinement && success) { cv::cornerSubPix( image, centers, cv::Size(_options.windowWidth, _options.windowWidth), cv::Size(-1, -1), - cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1)); + cv::TermCriteria(cv::TermCriteria::Type::EPS + cv::TermCriteria::Type::MAX_ITER, 30, 0.1)); } //draw corners if (_options.showExtractionVideo) { //image with refined (blue) and raw corners (red) cv::Mat imageCopy1 = image.clone(); - cv::cvtColor(imageCopy1, imageCopy1, CV_GRAY2RGB); + cv::cvtColor(imageCopy1, imageCopy1, cv::COLOR_GRAY2RGB); cv::drawChessboardCorners(imageCopy1, cv::Size(rows(), cols()), centers, true); // write error msg if (!success) cv::putText(imageCopy1, "Detection failed! (frame not used)", - cv::Point(50, 50), CV_FONT_HERSHEY_SIMPLEX, 0.8, + cv::Point(50, 50), cv::FONT_HERSHEY_SIMPLEX, 0.8, CV_RGB(255,0,0), 3, 8, false); cv::imshow("Checkerboard corners", imageCopy1); // OpenCV call diff --git a/aslam_cv/aslam_cameras/src/GridCalibrationTargetCirclegrid.cpp b/aslam_cv/aslam_cameras/src/GridCalibrationTargetCirclegrid.cpp index 85d18d4b2..6d2695936 100644 --- a/aslam_cv/aslam_cameras/src/GridCalibrationTargetCirclegrid.cpp +++ b/aslam_cv/aslam_cameras/src/GridCalibrationTargetCirclegrid.cpp @@ -30,8 +30,8 @@ GridCalibrationTargetCirclegrid::GridCalibrationTargetCirclegrid(size_t rows, si void GridCalibrationTargetCirclegrid::initialize() { if (_options.showExtractionVideo) { - cv::namedWindow("Circlegrid corners", CV_WINDOW_AUTOSIZE); - cvStartWindowThread(); + cv::namedWindow("Circlegrid corners", cv::WINDOW_AUTOSIZE); + cv::startWindowThread(); } } @@ -77,13 +77,13 @@ bool GridCalibrationTargetCirclegrid::computeObservation(const cv::Mat & image, if (_options.showExtractionVideo) { //image with refined (blue) and raw corners (red) cv::Mat imageCopy1 = image.clone(); - cv::cvtColor(imageCopy1, imageCopy1, CV_GRAY2RGB); + cv::cvtColor(imageCopy1, imageCopy1, cv::COLOR_GRAY2RGB); cv::drawChessboardCorners(imageCopy1, cv::Size(rows(), cols()), centers, true); // write error msg if (!success) cv::putText(imageCopy1, "Detection failed! (frame not used)", - cv::Point(50, 50), CV_FONT_HERSHEY_SIMPLEX, 0.8, + cv::Point(50, 50), cv::FONT_HERSHEY_SIMPLEX, 0.8, CV_RGB(255,0,0), 3, 8, false); cv::imshow("Circlegrid corners", imageCopy1); // OpenCV call diff --git a/aslam_cv/aslam_cameras/src/GridDetector.cpp b/aslam_cv/aslam_cameras/src/GridDetector.cpp index 91b716c04..85f63b4ef 100644 --- a/aslam_cv/aslam_cameras/src/GridDetector.cpp +++ b/aslam_cv/aslam_cameras/src/GridDetector.cpp @@ -185,7 +185,7 @@ bool GridDetector::findTarget(const cv::Mat & image, const aslam::Time & stamp, // show plot of reprojected corners if (_options.plotCornerReprojection) { cv::Mat imageCopy1 = image.clone(); - cv::cvtColor(imageCopy1, imageCopy1, CV_GRAY2RGB); + cv::cvtColor(imageCopy1, imageCopy1, cv::COLOR_GRAY2RGB); if (success) { //calculate reprojection @@ -198,7 +198,7 @@ bool GridDetector::findTarget(const cv::Mat & image, const aslam::Time & stamp, } else { cv::putText(imageCopy1, "Detection failed! (frame not used)", - cv::Point(50, 50), CV_FONT_HERSHEY_SIMPLEX, 0.8, + cv::Point(50, 50), cv::FONT_HERSHEY_SIMPLEX, 0.8, CV_RGB(255,0,0), 3, 8, false); } diff --git a/aslam_cv/aslam_cameras/src/ImageMask.cpp b/aslam_cv/aslam_cameras/src/ImageMask.cpp index 42f345405..742f1686c 100644 --- a/aslam_cv/aslam_cameras/src/ImageMask.cpp +++ b/aslam_cv/aslam_cameras/src/ImageMask.cpp @@ -19,7 +19,7 @@ ImageMask::ImageMask(const sm::PropertyTree & config) // Note: if this fails, _mask.data == NULL. // http://opencv.willowgarage.com/documentation/cpp/reading_and_writing_images_and_video.html#cv-imread // \todo Better error handling. - _mask = cv::imread(maskFile, CV_LOAD_IMAGE_GRAYSCALE); + _mask = cv::imread(maskFile, cv::IMREAD_GRAYSCALE); //TODO: BB: How to implement scale here? } diff --git a/aslam_cv/aslam_cameras_april/src/GridCalibrationTargetAprilgrid.cpp b/aslam_cv/aslam_cameras_april/src/GridCalibrationTargetAprilgrid.cpp index 44420e621..28b0dd0dc 100644 --- a/aslam_cv/aslam_cameras_april/src/GridCalibrationTargetAprilgrid.cpp +++ b/aslam_cv/aslam_cameras_april/src/GridCalibrationTargetAprilgrid.cpp @@ -164,10 +164,10 @@ bool GridCalibrationTargetAprilgrid::computeObservation( //show the duplicate tags in the image cv::destroyAllWindows(); cv::namedWindow("Wild Apriltag detected. Hide them!"); - cvStartWindowThread(); + cv::startWindowThread(); cv::Mat imageCopy = image.clone(); - cv::cvtColor(imageCopy, imageCopy, CV_GRAY2RGB); + cv::cvtColor(imageCopy, imageCopy, cv::COLOR_GRAY2RGB); //mark all duplicate tags in image for (int j = 0; i < detections.size() - 1; i++) { @@ -178,10 +178,10 @@ bool GridCalibrationTargetAprilgrid::computeObservation( } cv::putText(imageCopy, "Duplicate Apriltags detected. Hide them.", - cv::Point(50, 50), CV_FONT_HERSHEY_SIMPLEX, 0.8, + cv::Point(50, 50), cv::FONT_HERSHEY_SIMPLEX, 0.8, CV_RGB(255,0,0), 2, 8, false); cv::putText(imageCopy, "Press enter to exit...", cv::Point(50, 80), - CV_FONT_HERSHEY_SIMPLEX, 0.8, CV_RGB(255,0,0), 2, 8, false); + cv::FONT_HERSHEY_SIMPLEX, 0.8, CV_RGB(255,0,0), 2, 8, false); cv::imshow("Duplicate Apriltags detected. Hide them", imageCopy); // OpenCV call // and exit @@ -217,12 +217,12 @@ bool GridCalibrationTargetAprilgrid::computeObservation( if (_options.doSubpixRefinement && success) cv::cornerSubPix( image, tagCorners, cv::Size(2, 2), cv::Size(-1, -1), - cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1)); + cv::TermCriteria(cv::TermCriteria::Type::EPS + cv::TermCriteria::Type::MAX_ITER, 30, 0.1)); if (_options.showExtractionVideo) { //image with refined (blue) and raw corners (red) cv::Mat imageCopy1 = image.clone(); - cv::cvtColor(imageCopy1, imageCopy1, CV_GRAY2RGB); + cv::cvtColor(imageCopy1, imageCopy1, cv::COLOR_GRAY2RGB); for (unsigned i = 0; i < detections.size(); i++) for (unsigned j = 0; j < 4; j++) { //raw apriltag corners @@ -237,7 +237,7 @@ bool GridCalibrationTargetAprilgrid::computeObservation( if (!success) cv::putText(imageCopy1, "Detection failed! (frame not used)", - cv::Point(50, 50), CV_FONT_HERSHEY_SIMPLEX, 0.8, + cv::Point(50, 50), cv::FONT_HERSHEY_SIMPLEX, 0.8, CV_RGB(255,0,0), 3, 8, false); } @@ -246,14 +246,14 @@ bool GridCalibrationTargetAprilgrid::computeObservation( /* copy image for modification */ cv::Mat imageCopy2 = image.clone(); - cv::cvtColor(imageCopy2, imageCopy2, CV_GRAY2RGB); + cv::cvtColor(imageCopy2, imageCopy2, cv::COLOR_GRAY2RGB); /* highlight detected tags in image */ for (unsigned i = 0; i < detections.size(); i++) { detections[i].draw(imageCopy2); if (!success) cv::putText(imageCopy2, "Detection failed! (frame not used)", - cv::Point(50, 50), CV_FONT_HERSHEY_SIMPLEX, 0.8, + cv::Point(50, 50), cv::FONT_HERSHEY_SIMPLEX, 0.8, CV_RGB(255,0,0), 3, 8, false); } diff --git a/aslam_cv/aslam_imgproc/include/aslam/implementation/aslamcv_helper.hpp b/aslam_cv/aslam_imgproc/include/aslam/implementation/aslamcv_helper.hpp index 679326171..b4d6ce9ca 100644 --- a/aslam_cv/aslam_imgproc/include/aslam/implementation/aslamcv_helper.hpp +++ b/aslam_cv/aslam_imgproc/include/aslam/implementation/aslamcv_helper.hpp @@ -20,12 +20,11 @@ using namespace cv; /// template static void icvGetRectangles(boost::shared_ptr camera_geometry, - CvSize imgSize, cv::Rect_& inner, + cv::Size imgSize, cv::Rect_& inner, cv::Rect_& outer) { const int N = 9; int x, y, k; - cv::Ptr _pts(cvCreateMat(1, N * N, CV_32FC2)); - CvPoint2D32f* pts = (CvPoint2D32f*) (_pts->data.ptr); + std::vector _pts(N*N); for (y = k = 0; y < N; y++) { for (x = 0; x < N; x++) { @@ -43,7 +42,7 @@ static void icvGetRectangles(boost::shared_ptr camera_geometry, //undistort camera_geometry->projection().distortion().undistort(point); - pts[k++] = cvPoint2D32f((float) point[0], (float) point[1]); + _pts[k++] = cv::Point2f((float) point[0], (float) point[1]); } } @@ -53,7 +52,7 @@ static void icvGetRectangles(boost::shared_ptr camera_geometry, // the code will likely not work with extreme rotation matrices (R) (>45%) for (y = k = 0; y < N; y++) for (x = 0; x < N; x++) { - CvPoint2D32f p = pts[k++]; + cv::Point2f p = _pts[k++]; oX0 = MIN(oX0, p.x); oX1 = MAX(oX1, p.x); oY0 = MIN(oY0, p.y); @@ -85,8 +84,8 @@ static void icvGetRectangles(boost::shared_ptr camera_geometry, /// template Eigen::Matrix3d getOptimalNewCameraMatrix( - boost::shared_ptr camera_geometry, CvSize imgSize, double alpha, - CvSize newImgSize) { + boost::shared_ptr camera_geometry, cv::Size imgSize, double alpha, + cv::Size newImgSize) { cv::Rect_ inner, outer; newImgSize = newImgSize.width * newImgSize.height != 0 ? newImgSize : imgSize; diff --git a/aslam_offline_calibration/ethz_apriltag2/src/example/apriltags_demo.cpp b/aslam_offline_calibration/ethz_apriltag2/src/example/apriltags_demo.cpp index 919388a88..4ed23f2f3 100644 --- a/aslam_offline_calibration/ethz_apriltag2/src/example/apriltags_demo.cpp +++ b/aslam_offline_calibration/ethz_apriltag2/src/example/apriltags_demo.cpp @@ -294,7 +294,7 @@ class Demo { v4l2_set_control(device, V4L2_CID_BRIGHTNESS, m_brightness*256); } v4l2_close(device); -#endif +#endif // find and open a USB camera (built in laptop camera, web cam etc) m_cap = cv::VideoCapture(m_deviceId); @@ -302,12 +302,12 @@ class Demo { cerr << "ERROR: Can't find video device " << m_deviceId << "\n"; exit(1); } - m_cap.set(CV_CAP_PROP_FRAME_WIDTH, m_width); - m_cap.set(CV_CAP_PROP_FRAME_HEIGHT, m_height); + m_cap.set(cv::CAP_PROP_FRAME_WIDTH, m_width); + m_cap.set(cv::CAP_PROP_FRAME_HEIGHT, m_height); cout << "Camera successfully opened (ignore error messages above...)" << endl; cout << "Actual resolution: " - << m_cap.get(CV_CAP_PROP_FRAME_WIDTH) << "x" - << m_cap.get(CV_CAP_PROP_FRAME_HEIGHT) << endl; + << m_cap.get(cv::CAP_PROP_FRAME_WIDTH) << "x" + << m_cap.get(cv::CAP_PROP_FRAME_HEIGHT) << endl; // prepare window for drawing the camera images if (m_draw) { @@ -380,7 +380,7 @@ class Demo { // m_cap.retrieve(image); // detect April tags (requires a gray scale image) - cv::cvtColor(image, image_gray, CV_BGR2GRAY); + cv::cvtColor(image, image_gray, cv::COLOR_BGR2GRAY); vector detections = m_tagDetector->extractTags(image_gray); // print out each detection From 12ac18340877d17fef2b219e0b3ef479070611cd Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Tue, 15 Dec 2020 13:18:23 +0000 Subject: [PATCH 07/75] place 'gettid' in namespace to prevent name clash with definition in 'unistd_ext.h' --- .../incremental_calibration/src/base/Thread.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/aslam_incremental_calibration/incremental_calibration/src/base/Thread.cpp b/aslam_incremental_calibration/incremental_calibration/src/base/Thread.cpp index 52a20ba2f..9bd220558 100644 --- a/aslam_incremental_calibration/incremental_calibration/src/base/Thread.cpp +++ b/aslam_incremental_calibration/incremental_calibration/src/base/Thread.cpp @@ -24,6 +24,7 @@ #include "aslam/calibration/exceptions/SystemException.h" #include "aslam/calibration/exceptions/InvalidOperationException.h" +namespace aslam { #ifdef __NR_gettid static pid_t gettid (void) { return syscall(__NR_gettid); @@ -33,6 +34,7 @@ static pid_t gettid (void) { return -ENOSYS; } #endif +} namespace aslam { namespace calibration { From a738e0348aba34fc0a8f1e1a32950fa2936f3352 Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Tue, 15 Dec 2020 13:21:00 +0000 Subject: [PATCH 08/75] replace deprecated NPY_CHAR with NPY_STRING --- .../numpy_eigen/include/numpy_eigen/type_traits.hpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/Schweizer-Messer/numpy_eigen/include/numpy_eigen/type_traits.hpp b/Schweizer-Messer/numpy_eigen/include/numpy_eigen/type_traits.hpp index 9ea026b3e..7b183774b 100755 --- a/Schweizer-Messer/numpy_eigen/include/numpy_eigen/type_traits.hpp +++ b/Schweizer-Messer/numpy_eigen/include/numpy_eigen/type_traits.hpp @@ -46,7 +46,7 @@ template<> struct TypeToNumPy static const char * typeString() { return "unsigned char"; } static bool canConvert(int type) { - return type == NPY_UBYTE || type == NPY_BYTE || type == NPY_CHAR; + return type == NPY_UBYTE || type == NPY_BYTE || type == NPY_STRING; } }; @@ -57,7 +57,7 @@ template<> struct TypeToNumPy static const char * typeString() { return "char"; } static bool canConvert(int type) { - return type == NPY_UBYTE || type == NPY_BYTE || type == NPY_CHAR; + return type == NPY_UBYTE || type == NPY_BYTE || type == NPY_STRING; } }; @@ -136,8 +136,6 @@ inline const char * npyTypeToString(int npyType) return "NPY_NTYPES"; case NPY_NOTYPE: return "NPY_NOTYPE"; - case NPY_CHAR: - return "NPY_CHAR"; default: return "Unknown type"; } From 4d1cfd77187705073ca0990d829a6eb81d6790b9 Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Tue, 15 Dec 2020 13:31:34 +0000 Subject: [PATCH 09/75] ignore 'fpermissive' warnings --- Schweizer-Messer/numpy_eigen/CMakeLists.txt | 3 +++ 1 file changed, 3 insertions(+) diff --git a/Schweizer-Messer/numpy_eigen/CMakeLists.txt b/Schweizer-Messer/numpy_eigen/CMakeLists.txt index df9b9d0d3..0a76e06a4 100644 --- a/Schweizer-Messer/numpy_eigen/CMakeLists.txt +++ b/Schweizer-Messer/numpy_eigen/CMakeLists.txt @@ -19,6 +19,9 @@ catkin_package( ) add_definitions(-std=c++0x -D__STRICT_ANSI__) +# ignore "error: return-statement with a value, in function returning ‘void’" in "__multiarray_api.h" +add_compile_options(-fpermissive) + include_directories(include ${Boost_INCLUDE_DIRS}) IF(APPLE) From c681ade34283b66f0c42f41eebe2d876579705ce Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Tue, 15 Dec 2020 14:12:21 +0000 Subject: [PATCH 10/75] port to python3 --- Schweizer-Messer/sm_python/include/sm/python/Id.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/Schweizer-Messer/sm_python/include/sm/python/Id.hpp b/Schweizer-Messer/sm_python/include/sm/python/Id.hpp index 2a3b90245..651601d73 100755 --- a/Schweizer-Messer/sm_python/include/sm/python/Id.hpp +++ b/Schweizer-Messer/sm_python/include/sm/python/Id.hpp @@ -20,7 +20,7 @@ namespace sm { namespace python { // The "Convert from C to Python" API static PyObject * convert(id_t const & id){ - PyObject * i = PyInt_FromLong(id.getId()); + PyObject * i = PyLong_FromLong(id.getId()); // It seems that the call to "incref(.)" caused a memory leak! // I will check this in hoping it doesn't cause any instability. return i;//boost::python::incref(i); @@ -30,7 +30,7 @@ namespace sm { namespace python { // Two functions: convertible() and construct() static void * convertible(PyObject* obj_ptr) { - if (!(PyInt_Check(obj_ptr) || PyLong_Check(obj_ptr))) + if (!(PyLong_Check(obj_ptr) || PyLong_Check(obj_ptr))) return 0; return obj_ptr; @@ -43,8 +43,8 @@ namespace sm { namespace python { // Get the value. boost::uint64_t value; - if ( PyInt_Check(obj_ptr) ) { - value = PyInt_AsUnsignedLongLongMask(obj_ptr); + if ( PyLong_Check(obj_ptr) ) { + value = PyLong_AsUnsignedLongLongMask(obj_ptr); } else { value = PyLong_AsUnsignedLongLong(obj_ptr); } From 987538747158a9aaab19ebeed71af5de2d95182e Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Tue, 30 Mar 2021 13:36:20 +0100 Subject: [PATCH 11/75] Upgrade minimum CMake version to 3.0.2 to avoid author warning --- Schweizer-Messer/numpy_eigen/CMakeLists.txt | 2 +- Schweizer-Messer/python_module/CMakeLists.txt | 2 +- Schweizer-Messer/sm_boost/CMakeLists.txt | 2 +- Schweizer-Messer/sm_common/CMakeLists.txt | 2 +- Schweizer-Messer/sm_eigen/CMakeLists.txt | 2 +- Schweizer-Messer/sm_kinematics/CMakeLists.txt | 2 +- Schweizer-Messer/sm_logging/CMakeLists.txt | 2 +- Schweizer-Messer/sm_matrix_archive/CMakeLists.txt | 2 +- Schweizer-Messer/sm_opencv/CMakeLists.txt | 2 +- Schweizer-Messer/sm_property_tree/CMakeLists.txt | 2 +- Schweizer-Messer/sm_python/CMakeLists.txt | 2 +- Schweizer-Messer/sm_random/CMakeLists.txt | 2 +- Schweizer-Messer/sm_timing/CMakeLists.txt | 2 +- aslam_cv/aslam_cameras/CMakeLists.txt | 2 +- aslam_cv/aslam_cameras_april/CMakeLists.txt | 2 +- aslam_cv/aslam_cv_backend/CMakeLists.txt | 2 +- aslam_cv/aslam_cv_backend_python/CMakeLists.txt | 2 +- aslam_cv/aslam_cv_error_terms/CMakeLists.txt | 2 +- aslam_cv/aslam_cv_python/CMakeLists.txt | 2 +- aslam_cv/aslam_cv_serialization/CMakeLists.txt | 2 +- aslam_cv/aslam_imgproc/CMakeLists.txt | 2 +- aslam_cv/aslam_time/CMakeLists.txt | 2 +- .../incremental_calibration/CMakeLists.txt | 2 +- .../incremental_calibration_python/CMakeLists.txt | 2 +- aslam_nonparametric_estimation/aslam_splines/CMakeLists.txt | 2 +- .../aslam_splines_python/CMakeLists.txt | 2 +- aslam_nonparametric_estimation/bsplines/CMakeLists.txt | 2 +- aslam_nonparametric_estimation/bsplines_python/CMakeLists.txt | 2 +- aslam_offline_calibration/kalibr/CMakeLists.txt | 2 +- aslam_optimizer/aslam_backend/CMakeLists.txt | 2 +- aslam_optimizer/aslam_backend_expressions/CMakeLists.txt | 2 +- aslam_optimizer/aslam_backend_python/CMakeLists.txt | 2 +- aslam_optimizer/sparse_block_matrix/CMakeLists.txt | 2 +- catkin_simple/CMakeLists.txt | 2 +- catkin_simple/README.md | 4 ++-- opencv2_catkin/CMakeLists.txt | 2 +- suitesparse/CMakeLists.txt | 2 +- 37 files changed, 38 insertions(+), 38 deletions(-) diff --git a/Schweizer-Messer/numpy_eigen/CMakeLists.txt b/Schweizer-Messer/numpy_eigen/CMakeLists.txt index 0a76e06a4..26c51b2cb 100644 --- a/Schweizer-Messer/numpy_eigen/CMakeLists.txt +++ b/Schweizer-Messer/numpy_eigen/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(numpy_eigen) find_package(catkin REQUIRED COMPONENTS cmake_modules python_module) diff --git a/Schweizer-Messer/python_module/CMakeLists.txt b/Schweizer-Messer/python_module/CMakeLists.txt index e1258cadf..fb6a19bba 100644 --- a/Schweizer-Messer/python_module/CMakeLists.txt +++ b/Schweizer-Messer/python_module/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(python_module) find_package(catkin REQUIRED) diff --git a/Schweizer-Messer/sm_boost/CMakeLists.txt b/Schweizer-Messer/sm_boost/CMakeLists.txt index 59ae5dea6..977a87adc 100644 --- a/Schweizer-Messer/sm_boost/CMakeLists.txt +++ b/Schweizer-Messer/sm_boost/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(sm_boost) find_package(catkin REQUIRED COMPONENTS sm_common) diff --git a/Schweizer-Messer/sm_common/CMakeLists.txt b/Schweizer-Messer/sm_common/CMakeLists.txt index e70c97895..db7b0f4a3 100644 --- a/Schweizer-Messer/sm_common/CMakeLists.txt +++ b/Schweizer-Messer/sm_common/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(sm_common) find_package(catkin REQUIRED) diff --git a/Schweizer-Messer/sm_eigen/CMakeLists.txt b/Schweizer-Messer/sm_eigen/CMakeLists.txt index 7ed72c195..1e3ad8b3c 100644 --- a/Schweizer-Messer/sm_eigen/CMakeLists.txt +++ b/Schweizer-Messer/sm_eigen/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(sm_eigen) find_package(catkin REQUIRED COMPONENTS cmake_modules sm_common sm_random) diff --git a/Schweizer-Messer/sm_kinematics/CMakeLists.txt b/Schweizer-Messer/sm_kinematics/CMakeLists.txt index 4d060c346..a47787800 100644 --- a/Schweizer-Messer/sm_kinematics/CMakeLists.txt +++ b/Schweizer-Messer/sm_kinematics/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(sm_kinematics) find_package(catkin REQUIRED COMPONENTS cmake_modules sm_common sm_eigen sm_boost sm_random) diff --git a/Schweizer-Messer/sm_logging/CMakeLists.txt b/Schweizer-Messer/sm_logging/CMakeLists.txt index d70b60315..4025c0dee 100644 --- a/Schweizer-Messer/sm_logging/CMakeLists.txt +++ b/Schweizer-Messer/sm_logging/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(sm_logging) find_package(catkin REQUIRED) diff --git a/Schweizer-Messer/sm_matrix_archive/CMakeLists.txt b/Schweizer-Messer/sm_matrix_archive/CMakeLists.txt index bb294291b..b16e79766 100644 --- a/Schweizer-Messer/sm_matrix_archive/CMakeLists.txt +++ b/Schweizer-Messer/sm_matrix_archive/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(sm_matrix_archive) find_package(catkin REQUIRED COMPONENTS cmake_modules sm_common) diff --git a/Schweizer-Messer/sm_opencv/CMakeLists.txt b/Schweizer-Messer/sm_opencv/CMakeLists.txt index d9d224ed5..248c37190 100644 --- a/Schweizer-Messer/sm_opencv/CMakeLists.txt +++ b/Schweizer-Messer/sm_opencv/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(sm_opencv) find_package(catkin REQUIRED COMPONENTS sm_common) diff --git a/Schweizer-Messer/sm_property_tree/CMakeLists.txt b/Schweizer-Messer/sm_property_tree/CMakeLists.txt index 2b4bbd5ee..d036080c1 100644 --- a/Schweizer-Messer/sm_property_tree/CMakeLists.txt +++ b/Schweizer-Messer/sm_property_tree/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(sm_property_tree) find_package(catkin REQUIRED COMPONENTS sm_common) diff --git a/Schweizer-Messer/sm_python/CMakeLists.txt b/Schweizer-Messer/sm_python/CMakeLists.txt index b3f2ef832..82a1f3cb2 100644 --- a/Schweizer-Messer/sm_python/CMakeLists.txt +++ b/Schweizer-Messer/sm_python/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(sm_python) #catkin_python_setup() diff --git a/Schweizer-Messer/sm_random/CMakeLists.txt b/Schweizer-Messer/sm_random/CMakeLists.txt index a12f489c3..af9a8870f 100644 --- a/Schweizer-Messer/sm_random/CMakeLists.txt +++ b/Schweizer-Messer/sm_random/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(sm_random) find_package(catkin REQUIRED) diff --git a/Schweizer-Messer/sm_timing/CMakeLists.txt b/Schweizer-Messer/sm_timing/CMakeLists.txt index a673730e2..0c8dcf28c 100644 --- a/Schweizer-Messer/sm_timing/CMakeLists.txt +++ b/Schweizer-Messer/sm_timing/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(sm_timing) find_package(catkin REQUIRED COMPONENTS sm_common sm_random) diff --git a/aslam_cv/aslam_cameras/CMakeLists.txt b/aslam_cv/aslam_cameras/CMakeLists.txt index db751e38e..20cda664b 100644 --- a/aslam_cv/aslam_cameras/CMakeLists.txt +++ b/aslam_cv/aslam_cameras/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(aslam_cameras) find_package(catkin_simple REQUIRED) diff --git a/aslam_cv/aslam_cameras_april/CMakeLists.txt b/aslam_cv/aslam_cameras_april/CMakeLists.txt index 314f953d2..b960ddf95 100644 --- a/aslam_cv/aslam_cameras_april/CMakeLists.txt +++ b/aslam_cv/aslam_cameras_april/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(aslam_cameras_april) find_package(catkin_simple REQUIRED) diff --git a/aslam_cv/aslam_cv_backend/CMakeLists.txt b/aslam_cv/aslam_cv_backend/CMakeLists.txt index 160e0a435..98c978847 100644 --- a/aslam_cv/aslam_cv_backend/CMakeLists.txt +++ b/aslam_cv/aslam_cv_backend/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(aslam_cv_backend) find_package(catkin_simple REQUIRED) diff --git a/aslam_cv/aslam_cv_backend_python/CMakeLists.txt b/aslam_cv/aslam_cv_backend_python/CMakeLists.txt index e0edcce3f..a827da8c7 100644 --- a/aslam_cv/aslam_cv_backend_python/CMakeLists.txt +++ b/aslam_cv/aslam_cv_backend_python/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(aslam_cv_backend_python) find_package(catkin_simple REQUIRED) diff --git a/aslam_cv/aslam_cv_error_terms/CMakeLists.txt b/aslam_cv/aslam_cv_error_terms/CMakeLists.txt index 9e6e87c36..8a49f8b71 100644 --- a/aslam_cv/aslam_cv_error_terms/CMakeLists.txt +++ b/aslam_cv/aslam_cv_error_terms/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(aslam_cv_error_terms) find_package(catkin_simple REQUIRED) diff --git a/aslam_cv/aslam_cv_python/CMakeLists.txt b/aslam_cv/aslam_cv_python/CMakeLists.txt index 3a18b1821..20ac48801 100644 --- a/aslam_cv/aslam_cv_python/CMakeLists.txt +++ b/aslam_cv/aslam_cv_python/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(aslam_cv_python) find_package(catkin_simple REQUIRED) diff --git a/aslam_cv/aslam_cv_serialization/CMakeLists.txt b/aslam_cv/aslam_cv_serialization/CMakeLists.txt index f6befc888..2c08a2e94 100644 --- a/aslam_cv/aslam_cv_serialization/CMakeLists.txt +++ b/aslam_cv/aslam_cv_serialization/CMakeLists.txt @@ -1,5 +1,5 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(aslam_cv_serialization) find_package(catkin_simple REQUIRED) diff --git a/aslam_cv/aslam_imgproc/CMakeLists.txt b/aslam_cv/aslam_imgproc/CMakeLists.txt index d14277fc3..7d40138a6 100644 --- a/aslam_cv/aslam_imgproc/CMakeLists.txt +++ b/aslam_cv/aslam_imgproc/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(aslam_imgproc) find_package(catkin_simple REQUIRED) diff --git a/aslam_cv/aslam_time/CMakeLists.txt b/aslam_cv/aslam_time/CMakeLists.txt index cb75bb233..444738632 100644 --- a/aslam_cv/aslam_time/CMakeLists.txt +++ b/aslam_cv/aslam_time/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(aslam_time) find_package(catkin_simple REQUIRED) diff --git a/aslam_incremental_calibration/incremental_calibration/CMakeLists.txt b/aslam_incremental_calibration/incremental_calibration/CMakeLists.txt index 4fb6c70a8..cc768cbb4 100644 --- a/aslam_incremental_calibration/incremental_calibration/CMakeLists.txt +++ b/aslam_incremental_calibration/incremental_calibration/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(incremental_calibration) set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${PROJECT_SOURCE_DIR}/../cmake/) diff --git a/aslam_incremental_calibration/incremental_calibration_python/CMakeLists.txt b/aslam_incremental_calibration/incremental_calibration_python/CMakeLists.txt index 1c1a3791f..890777355 100644 --- a/aslam_incremental_calibration/incremental_calibration_python/CMakeLists.txt +++ b/aslam_incremental_calibration/incremental_calibration_python/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(incremental_calibration_python) find_package(catkin_simple REQUIRED) diff --git a/aslam_nonparametric_estimation/aslam_splines/CMakeLists.txt b/aslam_nonparametric_estimation/aslam_splines/CMakeLists.txt index 7c0db0873..d5a76b001 100644 --- a/aslam_nonparametric_estimation/aslam_splines/CMakeLists.txt +++ b/aslam_nonparametric_estimation/aslam_splines/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(aslam_splines) find_package(catkin_simple REQUIRED) diff --git a/aslam_nonparametric_estimation/aslam_splines_python/CMakeLists.txt b/aslam_nonparametric_estimation/aslam_splines_python/CMakeLists.txt index 0d993c510..2f861626c 100644 --- a/aslam_nonparametric_estimation/aslam_splines_python/CMakeLists.txt +++ b/aslam_nonparametric_estimation/aslam_splines_python/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(aslam_splines_python) find_package(catkin_simple REQUIRED) diff --git a/aslam_nonparametric_estimation/bsplines/CMakeLists.txt b/aslam_nonparametric_estimation/bsplines/CMakeLists.txt index 4e1e56857..a9338c52e 100644 --- a/aslam_nonparametric_estimation/bsplines/CMakeLists.txt +++ b/aslam_nonparametric_estimation/bsplines/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(bsplines) find_package(catkin_simple REQUIRED) diff --git a/aslam_nonparametric_estimation/bsplines_python/CMakeLists.txt b/aslam_nonparametric_estimation/bsplines_python/CMakeLists.txt index d06d9a172..ecac24a49 100644 --- a/aslam_nonparametric_estimation/bsplines_python/CMakeLists.txt +++ b/aslam_nonparametric_estimation/bsplines_python/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(bsplines_python) find_package(catkin_simple REQUIRED) diff --git a/aslam_offline_calibration/kalibr/CMakeLists.txt b/aslam_offline_calibration/kalibr/CMakeLists.txt index 3f823ae49..d08833109 100644 --- a/aslam_offline_calibration/kalibr/CMakeLists.txt +++ b/aslam_offline_calibration/kalibr/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(kalibr) find_package(catkin_simple REQUIRED) diff --git a/aslam_optimizer/aslam_backend/CMakeLists.txt b/aslam_optimizer/aslam_backend/CMakeLists.txt index ce269e696..9354c5db7 100644 --- a/aslam_optimizer/aslam_backend/CMakeLists.txt +++ b/aslam_optimizer/aslam_backend/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(aslam_backend) find_package(catkin_simple REQUIRED) diff --git a/aslam_optimizer/aslam_backend_expressions/CMakeLists.txt b/aslam_optimizer/aslam_backend_expressions/CMakeLists.txt index 7157aba30..da369eb73 100644 --- a/aslam_optimizer/aslam_backend_expressions/CMakeLists.txt +++ b/aslam_optimizer/aslam_backend_expressions/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(aslam_backend_expressions) include_directories(include ${catkin_INCLUDE_DIRS}) diff --git a/aslam_optimizer/aslam_backend_python/CMakeLists.txt b/aslam_optimizer/aslam_backend_python/CMakeLists.txt index baece975f..b54416c56 100644 --- a/aslam_optimizer/aslam_backend_python/CMakeLists.txt +++ b/aslam_optimizer/aslam_backend_python/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(aslam_backend_python) find_package(catkin_simple REQUIRED) diff --git a/aslam_optimizer/sparse_block_matrix/CMakeLists.txt b/aslam_optimizer/sparse_block_matrix/CMakeLists.txt index 7d0d26e46..66db51825 100644 --- a/aslam_optimizer/sparse_block_matrix/CMakeLists.txt +++ b/aslam_optimizer/sparse_block_matrix/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) # This helps find the TBB library set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/../cmake") diff --git a/catkin_simple/CMakeLists.txt b/catkin_simple/CMakeLists.txt index 2bb070212..66d418139 100644 --- a/catkin_simple/CMakeLists.txt +++ b/catkin_simple/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(catkin_simple) find_package(catkin REQUIRED) diff --git a/catkin_simple/README.md b/catkin_simple/README.md index 9cdf91742..7fb981d94 100644 --- a/catkin_simple/README.md +++ b/catkin_simple/README.md @@ -7,7 +7,7 @@ This `catkin` package is designed to make the `CMakeLists.txt` of other `catkin` Here is an example of a package `foo` which depends on other catkin packages: ```cmake -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(foo) find_package(catkin_simple REQUIRED) @@ -31,7 +31,7 @@ Lets break this down, line by line. First is the standard CMake header: ```cmake -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(foo) ``` diff --git a/opencv2_catkin/CMakeLists.txt b/opencv2_catkin/CMakeLists.txt index dc6cfe266..edb46fd1d 100644 --- a/opencv2_catkin/CMakeLists.txt +++ b/opencv2_catkin/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(opencv2_catkin) find_package(catkin_simple REQUIRED) diff --git a/suitesparse/CMakeLists.txt b/suitesparse/CMakeLists.txt index ad285a5f5..aa824ce05 100644 --- a/suitesparse/CMakeLists.txt +++ b/suitesparse/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(suitesparse) set(CMAKE_BUILD_TYPE Release) From 85918d4184fe2b97c4ddc40e45d3ab1c446cebd2 Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Tue, 30 Mar 2021 13:46:20 +0100 Subject: [PATCH 12/75] [python_module] Fix location of target file --- .../python_module/cmake/add_python_export_library.cmake | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/Schweizer-Messer/python_module/cmake/add_python_export_library.cmake b/Schweizer-Messer/python_module/cmake/add_python_export_library.cmake index dfd43d807..8bdc102df 100755 --- a/Schweizer-Messer/python_module/cmake/add_python_export_library.cmake +++ b/Schweizer-Messer/python_module/cmake/add_python_export_library.cmake @@ -128,8 +128,7 @@ ${SETUP_PY_TEXT} # On OSX and Linux, the python library must end in the extension .so. Build this # filename here. - get_property(PYLIB_OUTPUT_FILE TARGET ${TARGET_NAME} PROPERTY LOCATION) - get_filename_component(PYLIB_OUTPUT_NAME ${PYLIB_OUTPUT_FILE} NAME_WE) + get_filename_component(PYLIB_OUTPUT_NAME $ NAME_WE) set(PYLIB_SO_NAME ${PYLIB_OUTPUT_NAME}.so) if(APPLE) From e789d48917c3d8f6f32e2b7de5fbc370857cb210 Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Tue, 30 Mar 2021 13:46:43 +0100 Subject: [PATCH 13/75] [numpy_eigen] Upgrade to Eigen3 --- Schweizer-Messer/numpy_eigen/CMakeLists.txt | 6 +++--- Schweizer-Messer/numpy_eigen/package.xml | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/Schweizer-Messer/numpy_eigen/CMakeLists.txt b/Schweizer-Messer/numpy_eigen/CMakeLists.txt index 26c51b2cb..a1736b3b3 100644 --- a/Schweizer-Messer/numpy_eigen/CMakeLists.txt +++ b/Schweizer-Messer/numpy_eigen/CMakeLists.txt @@ -1,15 +1,15 @@ cmake_minimum_required(VERSION 3.0.2) project(numpy_eigen) -find_package(catkin REQUIRED COMPONENTS cmake_modules python_module) +find_package(catkin REQUIRED COMPONENTS python_module) include_directories(${catkin_INCLUDE_DIRS}) find_package(Boost REQUIRED COMPONENTS system) -find_package(Eigen REQUIRED) +find_package(Eigen3 REQUIRED) catkin_python_setup() add_definitions(${EIGEN_DEFINITIONS}) -include_directories(include ${Eigen_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) +include_directories(include ${EIGEN3_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) include_directories("${PROJECT_SOURCE_DIR}/include/numpy_eigen") catkin_package( diff --git a/Schweizer-Messer/numpy_eigen/package.xml b/Schweizer-Messer/numpy_eigen/package.xml index 41f8e2323..16a6e8a9d 100644 --- a/Schweizer-Messer/numpy_eigen/package.xml +++ b/Schweizer-Messer/numpy_eigen/package.xml @@ -7,7 +7,7 @@ Paul Furgale New BSD catkin - cmake_modules + eigen python_module python_module From 41bac10118ea0bc77ef59b665e45181fd509159c Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Tue, 30 Mar 2021 14:58:28 +0100 Subject: [PATCH 14/75] [numpy_eigen] Fix for new NumPy API & Python 3 --- .../numpy_eigen/NumpyEigenConverter.hpp | 17 +++++++------- .../include/numpy_eigen/copy_routines.hpp | 12 +++++----- .../include/numpy_eigen/type_traits.hpp | 6 ++--- .../numpy_eigen/src/numpy_eigen/__init__.py | 2 +- .../cmake/add_python_export_library.cmake | 22 ++++++------------- 5 files changed, 26 insertions(+), 33 deletions(-) diff --git a/Schweizer-Messer/numpy_eigen/include/numpy_eigen/NumpyEigenConverter.hpp b/Schweizer-Messer/numpy_eigen/include/numpy_eigen/NumpyEigenConverter.hpp index 31cef412a..1a4ebf01d 100755 --- a/Schweizer-Messer/numpy_eigen/include/numpy_eigen/NumpyEigenConverter.hpp +++ b/Schweizer-Messer/numpy_eigen/include/numpy_eigen/NumpyEigenConverter.hpp @@ -15,6 +15,7 @@ #include //#include +#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION #define PY_ARRAY_UNIQUE_SYMBOL NP_Eigen_AS #include @@ -134,8 +135,8 @@ struct NumpyEigenConverter static void checkMatrixSizes(PyObject * obj_ptr) { - int rows = PyArray_DIM(obj_ptr, 0); - int cols = PyArray_DIM(obj_ptr, 1); + int rows = PyArray_DIM((PyArrayObject*)obj_ptr, 0); + int cols = PyArray_DIM((PyArrayObject*)obj_ptr, 1); bool rowsValid = isDimensionValid(rows, RowsAtCompileTime, MaxRowsAtCompileTime); bool colsValid = isDimensionValid(cols, ColsAtCompileTime, MaxColsAtCompileTime); @@ -176,7 +177,7 @@ struct NumpyEigenConverter static void checkVectorSizes(PyObject * obj_ptr) { - int size = PyArray_DIM(obj_ptr, 0); + int size = PyArray_DIM((PyArrayObject*)obj_ptr, 0); // If the number of rows is fixed at 1, assume that is the sense of the vector. // Otherwise, assume it is a column. @@ -220,7 +221,7 @@ struct NumpyEigenConverter // Check the array dimensions. - int nd = PyArray_NDIM(obj_ptr); + int nd = PyArray_NDIM((PyArrayObject*)obj_ptr); if(nd != 1 && nd != 2) { @@ -266,10 +267,10 @@ struct NumpyEigenConverter matrix_t & M = *Mp; - int nd = PyArray_NDIM(obj_ptr); + int nd = PyArray_NDIM((PyArrayObject*)obj_ptr); if(nd == 1) { - int size = PyArray_DIM(obj_ptr, 0); + int size = PyArray_DIM((PyArrayObject*)obj_ptr, 0); // This is a vector type if(RowsAtCompileTime == 1) { @@ -285,8 +286,8 @@ struct NumpyEigenConverter } else { - int rows = PyArray_DIM(obj_ptr, 0); - int cols = PyArray_DIM(obj_ptr, 1); + int rows = PyArray_DIM((PyArrayObject*)obj_ptr, 0); + int cols = PyArray_DIM((PyArrayObject*)obj_ptr, 1); M.resize(rows,cols); numpyTypeDemuxer< CopyNumpyToEigenMatrix >(&M,obj_ptr); diff --git a/Schweizer-Messer/numpy_eigen/include/numpy_eigen/copy_routines.hpp b/Schweizer-Messer/numpy_eigen/include/numpy_eigen/copy_routines.hpp index 8ac94e8b7..a6b1271d3 100755 --- a/Schweizer-Messer/numpy_eigen/include/numpy_eigen/copy_routines.hpp +++ b/Schweizer-Messer/numpy_eigen/include/numpy_eigen/copy_routines.hpp @@ -16,7 +16,7 @@ struct CopyNumpyToEigenMatrix { for(int c = 0; c < M_->cols(); c++) { - T * p = static_cast(PyArray_GETPTR2(P_, r, c)); + T * p = static_cast(PyArray_GETPTR2((PyArrayObject*)P_, r, c)); (*M_)(r,c) = static_cast(*p); } } @@ -38,7 +38,7 @@ struct CopyEigenToNumpyMatrix { for(int c = 0; c < M_->cols(); c++) { - T * p = static_cast(PyArray_GETPTR2(P_, r, c)); + T * p = static_cast(PyArray_GETPTR2((PyArrayObject*)P_, r, c)); *p = static_cast((*M_)(r,c)); } } @@ -58,8 +58,8 @@ struct CopyEigenToNumpyVector // Assumes M is already initialized. for(int i = 0; i < M_->size(); i++) { - T * p = static_cast(PyArray_GETPTR1(P_, i)); - *p = static_cast((*M_)(i)); + // T * p = static_cast(PyArray_GETPTR1(P_, i)); + // *p = static_cast((*M_)(i)); } } @@ -78,8 +78,8 @@ struct CopyNumpyToEigenVector // Assumes M is already initialized. for(int i = 0; i < M_->size(); i++) { - T * p = static_cast(PyArray_GETPTR1(P_, i)); - (*M_)(i) = static_cast(*p); + // T * p = static_cast(PyArray_GETPTR1(P_, i)); + // (*M_)(i) = static_cast(*p); } } diff --git a/Schweizer-Messer/numpy_eigen/include/numpy_eigen/type_traits.hpp b/Schweizer-Messer/numpy_eigen/include/numpy_eigen/type_traits.hpp index 7b183774b..75686b3ae 100755 --- a/Schweizer-Messer/numpy_eigen/include/numpy_eigen/type_traits.hpp +++ b/Schweizer-Messer/numpy_eigen/include/numpy_eigen/type_traits.hpp @@ -144,14 +144,14 @@ inline const char * npyTypeToString(int npyType) inline std::string npyArrayTypeString(PyObject * obj_ptr) { std::stringstream ss; - int nd = PyArray_NDIM(obj_ptr); + int nd = PyArray_NDIM((PyArrayObject*)obj_ptr); ss << "numpy.array<" << npyTypeToString(PyArray_ObjectType(obj_ptr, 0)) << ">["; if(nd > 0) { - ss << PyArray_DIM(obj_ptr, 0); + ss << PyArray_DIM((PyArrayObject*)obj_ptr, 0); for(int i = 1; i < nd; i++) { - ss << ", " << PyArray_DIM(obj_ptr, i); + ss << ", " << PyArray_DIM((PyArrayObject*)obj_ptr, i); } } ss << "]"; diff --git a/Schweizer-Messer/numpy_eigen/src/numpy_eigen/__init__.py b/Schweizer-Messer/numpy_eigen/src/numpy_eigen/__init__.py index 758443372..9bfa5e5c4 100755 --- a/Schweizer-Messer/numpy_eigen/src/numpy_eigen/__init__.py +++ b/Schweizer-Messer/numpy_eigen/src/numpy_eigen/__init__.py @@ -1,3 +1,3 @@ import os -from libnumpy_eigen import * +from .libnumpy_eigen import * diff --git a/Schweizer-Messer/python_module/cmake/add_python_export_library.cmake b/Schweizer-Messer/python_module/cmake/add_python_export_library.cmake index 8bdc102df..ccd609372 100755 --- a/Schweizer-Messer/python_module/cmake/add_python_export_library.cmake +++ b/Schweizer-Messer/python_module/cmake/add_python_export_library.cmake @@ -63,7 +63,7 @@ ${SETUP_PY_TEXT} catkin_python_setup() # Find Python - FIND_PACKAGE(PythonLibs 2.7 REQUIRED) + FIND_PACKAGE(PythonLibs REQUIRED) INCLUDE_DIRECTORIES(${PYTHON_INCLUDE_DIRS}) if(APPLE) @@ -129,29 +129,21 @@ ${SETUP_PY_TEXT} # On OSX and Linux, the python library must end in the extension .so. Build this # filename here. get_filename_component(PYLIB_OUTPUT_NAME $ NAME_WE) - set(PYLIB_SO_NAME ${PYLIB_OUTPUT_NAME}.so) - - if(APPLE) - SET(DIST_DIR site-packages) - else() - SET(DIST_DIR dist-packages) - endif() install(TARGETS ${TARGET_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION}/python2.7/${DIST_DIR}/${TARGET_NAME} - LIBRARY DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION}/python2.7/${DIST_DIR}/${TARGET_NAME} + ARCHIVE DESTINATION ${CATKIN_GLOBAL_PYTHON_DESTINATION}/${TARGET_NAME} + LIBRARY DESTINATION ${CATKIN_GLOBAL_PYTHON_DESTINATION}/${TARGET_NAME} ) - # Cause the library to be output in the correct directory. - set(PYTHON_LIB_DIR ${CATKIN_DEVEL_PREFIX}/lib/python2.7/${DIST_DIR}/${PYTHON_PACKAGE_NAME}) + # Cause the library to be output in the correct directory. + set(PYTHON_LIB_DIR ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_PYTHON_DESTINATION}/${PYTHON_PACKAGE_NAME}) add_custom_command(TARGET ${TARGET_NAME} POST_BUILD - COMMAND mkdir -p ${PYTHON_LIB_DIR} && cp -v ${PYLIB_OUTPUT_FILE} ${PYTHON_LIB_DIR}/${PYLIB_SO_NAME} + COMMAND mkdir -p ${PYTHON_LIB_DIR} && cp -v ${PYLIB_OUTPUT_NAME} ${PYTHON_LIB_DIR}/ WORKING_DIRECTORY ${CATKIN_DEVEL_PREFIX} COMMENT "Copying library files to python directory" ) - get_directory_property(AMCF ADDITIONAL_MAKE_CLEAN_FILES) - list(APPEND AMCF ${PYTHON_LIB_DIR}/${PYLIB_SO_NAME}) + list(APPEND AMCF ${PYTHON_LIB_DIR}/${PYLIB_OUTPUT_NAME}) set_directory_properties(PROPERTIES ADDITIONAL_MAKE_CLEAN_FILES "${AMCF}") ENDFUNCTION() From 198d308ad95d45836b668cc04204814923917d38 Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Tue, 30 Mar 2021 15:07:27 +0100 Subject: [PATCH 15/75] Remove use of cmake_modules for Eigen --- Schweizer-Messer/sm_eigen/CMakeLists.txt | 8 ++++---- Schweizer-Messer/sm_eigen/package.xml | 2 +- Schweizer-Messer/sm_kinematics/CMakeLists.txt | 6 +++--- Schweizer-Messer/sm_kinematics/package.xml | 2 +- Schweizer-Messer/sm_matrix_archive/CMakeLists.txt | 6 +++--- Schweizer-Messer/sm_matrix_archive/package.xml | 2 +- Schweizer-Messer/sm_python/CMakeLists.txt | 6 +++--- Schweizer-Messer/sm_python/package.xml | 2 +- aslam_cv/aslam_cameras_april/CMakeLists.txt | 2 +- aslam_cv/aslam_imgproc/CMakeLists.txt | 2 +- .../ethz_apriltag2/CMakeLists.txt | 10 +++++----- aslam_offline_calibration/ethz_apriltag2/package.xml | 2 +- aslam_optimizer/aslam_backend/CMakeLists.txt | 2 +- .../aslam_backend_expressions/CMakeLists.txt | 2 +- aslam_optimizer/sparse_block_matrix/CMakeLists.txt | 2 +- 15 files changed, 28 insertions(+), 28 deletions(-) diff --git a/Schweizer-Messer/sm_eigen/CMakeLists.txt b/Schweizer-Messer/sm_eigen/CMakeLists.txt index 1e3ad8b3c..877bb3055 100644 --- a/Schweizer-Messer/sm_eigen/CMakeLists.txt +++ b/Schweizer-Messer/sm_eigen/CMakeLists.txt @@ -1,15 +1,15 @@ cmake_minimum_required(VERSION 3.0.2) project(sm_eigen) -find_package(catkin REQUIRED COMPONENTS cmake_modules sm_common sm_random) +find_package(catkin REQUIRED COMPONENTS sm_common sm_random) include_directories(${catkin_INCLUDE_DIRS}) find_package(Boost REQUIRED COMPONENTS system serialization) -find_package(Eigen REQUIRED) +find_package(Eigen3 REQUIRED) -include_directories(include ${Eigen_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) +include_directories(include ${EIGEN3_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) catkin_package( - INCLUDE_DIRS include ${Eigen_INCLUDE_DIRS} + INCLUDE_DIRS include ${EIGEN3_INCLUDE_DIRS} LIBRARIES ${PROJECT_NAME} CATKIN_DEPENDS DEPENDS diff --git a/Schweizer-Messer/sm_eigen/package.xml b/Schweizer-Messer/sm_eigen/package.xml index 83352977d..effa0f80b 100644 --- a/Schweizer-Messer/sm_eigen/package.xml +++ b/Schweizer-Messer/sm_eigen/package.xml @@ -7,7 +7,7 @@ Paul Furgale New BSD catkin - cmake_modules + eigen sm_common sm_random sm_common diff --git a/Schweizer-Messer/sm_kinematics/CMakeLists.txt b/Schweizer-Messer/sm_kinematics/CMakeLists.txt index a47787800..db6736a0d 100644 --- a/Schweizer-Messer/sm_kinematics/CMakeLists.txt +++ b/Schweizer-Messer/sm_kinematics/CMakeLists.txt @@ -1,13 +1,13 @@ cmake_minimum_required(VERSION 3.0.2) project(sm_kinematics) -find_package(catkin REQUIRED COMPONENTS cmake_modules sm_common sm_eigen sm_boost sm_random) +find_package(catkin REQUIRED COMPONENTS sm_common sm_eigen sm_boost sm_random) include_directories(${catkin_INCLUDE_DIRS}) find_package(Boost REQUIRED COMPONENTS system serialization filesystem) list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/../cmake) -find_package(Eigen REQUIRED) -include_directories(${EIGEN_INCLUDE_DIRS}) +find_package(Eigen3 REQUIRED) +include_directories(${EIGEN3_INCLUDE_DIRS}) add_definitions(${EIGEN_DEFINITIONS}) SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=c++0x") diff --git a/Schweizer-Messer/sm_kinematics/package.xml b/Schweizer-Messer/sm_kinematics/package.xml index 7b39b4a42..ef75ad87d 100644 --- a/Schweizer-Messer/sm_kinematics/package.xml +++ b/Schweizer-Messer/sm_kinematics/package.xml @@ -7,7 +7,7 @@ Paul Furgale New BSD catkin - cmake_modules + eigen sm_boost sm_common sm_eigen diff --git a/Schweizer-Messer/sm_matrix_archive/CMakeLists.txt b/Schweizer-Messer/sm_matrix_archive/CMakeLists.txt index b16e79766..4213be5e0 100644 --- a/Schweizer-Messer/sm_matrix_archive/CMakeLists.txt +++ b/Schweizer-Messer/sm_matrix_archive/CMakeLists.txt @@ -1,12 +1,12 @@ cmake_minimum_required(VERSION 3.0.2) project(sm_matrix_archive) -find_package(catkin REQUIRED COMPONENTS cmake_modules sm_common) +find_package(catkin REQUIRED COMPONENTS sm_common) include_directories(${catkin_INCLUDE_DIRS}) find_package(Boost REQUIRED COMPONENTS system filesystem) -find_package(Eigen REQUIRED) -include_directories(include ${Eigen_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) +find_package(Eigen3 REQUIRED) +include_directories(include ${EIGEN3_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) catkin_package( INCLUDE_DIRS include ${catkin_INCLUDE_DIRS} diff --git a/Schweizer-Messer/sm_matrix_archive/package.xml b/Schweizer-Messer/sm_matrix_archive/package.xml index 422074f0e..feef61494 100644 --- a/Schweizer-Messer/sm_matrix_archive/package.xml +++ b/Schweizer-Messer/sm_matrix_archive/package.xml @@ -7,7 +7,7 @@ Paul Furgale New BSD catkin - cmake_modules + eigen sm_common sm_common diff --git a/Schweizer-Messer/sm_python/CMakeLists.txt b/Schweizer-Messer/sm_python/CMakeLists.txt index 82a1f3cb2..9e260780b 100644 --- a/Schweizer-Messer/sm_python/CMakeLists.txt +++ b/Schweizer-Messer/sm_python/CMakeLists.txt @@ -3,13 +3,13 @@ project(sm_python) #catkin_python_setup() -find_package(catkin REQUIRED COMPONENTS cmake_modules sm_common numpy_eigen sm_kinematics +find_package(catkin REQUIRED COMPONENTS sm_common numpy_eigen sm_kinematics sm_timing sm_logging sm_matrix_archive sm_property_tree python_module) include_directories(${catkin_INCLUDE_DIRS}) -find_package(Eigen REQUIRED) +find_package(Eigen3 REQUIRED) -include_directories(include ${Eigen_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) +include_directories(include ${EIGEN3_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) catkin_package( diff --git a/Schweizer-Messer/sm_python/package.xml b/Schweizer-Messer/sm_python/package.xml index 25872da62..246c89d8d 100644 --- a/Schweizer-Messer/sm_python/package.xml +++ b/Schweizer-Messer/sm_python/package.xml @@ -7,7 +7,7 @@ Paul Furgale New BSD catkin - cmake_modules + eigen sm_common numpy_eigen python_module diff --git a/aslam_cv/aslam_cameras_april/CMakeLists.txt b/aslam_cv/aslam_cameras_april/CMakeLists.txt index b960ddf95..c8a14e924 100644 --- a/aslam_cv/aslam_cameras_april/CMakeLists.txt +++ b/aslam_cv/aslam_cameras_april/CMakeLists.txt @@ -7,7 +7,7 @@ catkin_simple() find_package(OpenCV REQUIRED) ADD_DEFINITIONS(-DASLAM_USE_ROS ) -include_directories(${Eigen_INCLUDE_DIRS}) +include_directories(${EIGEN3_INCLUDE_DIRS}) cs_add_library(${PROJECT_NAME} diff --git a/aslam_cv/aslam_imgproc/CMakeLists.txt b/aslam_cv/aslam_imgproc/CMakeLists.txt index 7d40138a6..da53b7282 100644 --- a/aslam_cv/aslam_imgproc/CMakeLists.txt +++ b/aslam_cv/aslam_imgproc/CMakeLists.txt @@ -7,7 +7,7 @@ catkin_simple() find_package(Boost REQUIRED COMPONENTS system) -include_directories(${Eigen_INCLUDE_DIRS}) +include_directories(${EIGEN3_INCLUDE_DIRS}) cs_add_library(${PROJECT_NAME} src/UndistorterBase.cpp) diff --git a/aslam_offline_calibration/ethz_apriltag2/CMakeLists.txt b/aslam_offline_calibration/ethz_apriltag2/CMakeLists.txt index 58f985c12..332e35930 100644 --- a/aslam_offline_calibration/ethz_apriltag2/CMakeLists.txt +++ b/aslam_offline_calibration/ethz_apriltag2/CMakeLists.txt @@ -2,24 +2,24 @@ cmake_minimum_required(VERSION 2.8) project(ethz_apriltag2) -find_package(catkin REQUIRED COMPONENTS cmake_modules) +find_package(catkin REQUIRED) include_directories(${catkin_INCLUDE_DIRS}) -find_package(Eigen REQUIRED) +find_package(Eigen3 REQUIRED) catkin_package( DEPENDS eigen opencv - INCLUDE_DIRS include ${Eigen_INCLUDE_DIRS} + INCLUDE_DIRS include ${EIGEN3_INCLUDE_DIRS} LIBRARIES ${PROJECT_NAME} ) set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${PROJECT_SOURCE_DIR}/cmake/) -find_package(Eigen REQUIRED) +find_package(Eigen3 REQUIRED) find_package(OpenCV REQUIRED) add_definitions(-fPIC -O3) -include_directories(include ${Eigen_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) +include_directories(include ${EIGEN3_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) #library file(GLOB SOURCE_FILES "src/*.cc") diff --git a/aslam_offline_calibration/ethz_apriltag2/package.xml b/aslam_offline_calibration/ethz_apriltag2/package.xml index 4fc8b1c73..508902ee4 100644 --- a/aslam_offline_calibration/ethz_apriltag2/package.xml +++ b/aslam_offline_calibration/ethz_apriltag2/package.xml @@ -7,7 +7,7 @@ Paul Furgale New BSD catkin - cmake_modules + eigen opencv eigen2 diff --git a/aslam_optimizer/aslam_backend/CMakeLists.txt b/aslam_optimizer/aslam_backend/CMakeLists.txt index 9354c5db7..269f4773b 100644 --- a/aslam_optimizer/aslam_backend/CMakeLists.txt +++ b/aslam_optimizer/aslam_backend/CMakeLists.txt @@ -6,7 +6,7 @@ catkin_simple() find_package(Boost REQUIRED COMPONENTS system thread) -include_directories(${Eigen_INCLUDE_DIRS}) +include_directories(${EIGEN3_INCLUDE_DIRS}) #add_definitions( -fPIC -msse2 -mssse3 -march=nocona -Wextra -Winit-self -Woverloaded-virtual -Wnon-virtual-dtor -Wsign-promo -Wno-long-long -std=c++0x -O3) diff --git a/aslam_optimizer/aslam_backend_expressions/CMakeLists.txt b/aslam_optimizer/aslam_backend_expressions/CMakeLists.txt index da369eb73..ca3a8e4d2 100644 --- a/aslam_optimizer/aslam_backend_expressions/CMakeLists.txt +++ b/aslam_optimizer/aslam_backend_expressions/CMakeLists.txt @@ -9,7 +9,7 @@ find_package(catkin_simple REQUIRED) catkin_simple() -include_directories(${EIGEN_INCLUDE_DIRS}) +include_directories(${EIGEN3_INCLUDE_DIRS}) find_package(Boost REQUIRED COMPONENTS system) diff --git a/aslam_optimizer/sparse_block_matrix/CMakeLists.txt b/aslam_optimizer/sparse_block_matrix/CMakeLists.txt index 66db51825..c8e3c200c 100644 --- a/aslam_optimizer/sparse_block_matrix/CMakeLists.txt +++ b/aslam_optimizer/sparse_block_matrix/CMakeLists.txt @@ -8,7 +8,7 @@ find_package(catkin_simple REQUIRED) catkin_simple() find_package(Eigen3) -include_directories(${Eigen_INCLUDE_DIRS}) +include_directories(${EIGEN3_INCLUDE_DIRS}) cs_add_library(${PROJECT_NAME} src/matrix_structure.cpp From 77ecd93a92e4441940f9dcc80b4ae4d3c0747275 Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Tue, 30 Mar 2021 15:29:42 +0100 Subject: [PATCH 16/75] [suitesparse] Use jobserver but do not define number of jobs --- suitesparse/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/suitesparse/CMakeLists.txt b/suitesparse/CMakeLists.txt index aa824ce05..295f37287 100644 --- a/suitesparse/CMakeLists.txt +++ b/suitesparse/CMakeLists.txt @@ -22,7 +22,7 @@ ExternalProject_Add(suitesparse_src DOWNLOAD_COMMAND rm -f SuiteSparse-${VERSION}.tar.gz && wget http://faculty.cse.tamu.edu/davis/SuiteSparse/SuiteSparse-${VERSION}.tar.gz PATCH_COMMAND tar -xzf ../SuiteSparse-${VERSION}.tar.gz && rm -rf ../suitesparse_src-build/SuiteSparse && sed -i.bu "s/\\/usr\\/local\\/lib/..\\/lib/g" SuiteSparse/SuiteSparse_config/SuiteSparse_config.mk && sed -i.bu "s/\\/usr\\/local\\/include/..\\/include/g" SuiteSparse/SuiteSparse_config/SuiteSparse_config.mk && mv SuiteSparse ../suitesparse_src-build/ CONFIGURE_COMMAND "" - BUILD_COMMAND cd SuiteSparse && make library -j8 -l8 + BUILD_COMMAND cd SuiteSparse && make library -j INSTALL_COMMAND cd SuiteSparse && mkdir -p lib && mkdir -p include && make install && cd lib && cp libamd.2.3.1.a libcamd.2.3.1.a libcholmod.2.1.2.a libcxsparse.3.1.2.a libldl.2.1.0.a libspqr.1.3.1.a libumfpack.5.6.2.a libamd.a libcamd.a libcholmod.a libcxsparse.a libldl.a libspqr.a libumfpack.a libbtf.1.2.0.a libccolamd.2.8.0.a libcolamd.2.8.0.a libklu.1.2.1.a librbio.2.1.1.a libsuitesparseconfig.4.2.1.a libbtf.a libccolamd.a libcolamd.a libklu.a librbio.a libsuitesparseconfig.a ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_DESTINATION}/ && cd .. && mkdir -p ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_INCLUDE_DESTINATION}/suitesparse && cd include && cp amd.h cholmod_matrixops.h SuiteSparseQR_definitions.h umfpack_load_symbolic.h umfpack_save_symbolic.h btf.h cholmod_modify.h SuiteSparseQR.hpp umfpack_numeric.h umfpack_scale.h camd.h cholmod_partition.h umfpack_col_to_triplet.h umfpack_qsymbolic.h umfpack_solve.h ccolamd.h cholmod_supernodal.h umfpack_defaults.h umfpack_report_control.h umfpack_symbolic.h cholmod_blas.h cholmod_template.h umfpack_free_numeric.h umfpack_report_info.h umfpack_tictoc.h cholmod_camd.h colamd.h umfpack_free_symbolic.h umfpack_report_matrix.h umfpack_timer.h cholmod_check.h cs.h umfpack_get_determinant.h umfpack_report_numeric.h umfpack_transpose.h cholmod_cholesky.h klu.h umfpack_get_lunz.h umfpack_report_perm.h umfpack_triplet_to_col.h cholmod_complexity.h ldl.h umfpack_get_numeric.h umfpack_report_status.h umfpack_wsolve.h cholmod_config.h RBio.h umfpack_get_symbolic.h umfpack_report_symbolic.h cholmod_core.h spqr.hpp umfpack_global.h umfpack_report_triplet.h cholmod.h SuiteSparse_config.h umfpack.h umfpack_report_vector.h cholmod_io64.h SuiteSparseQR_C.h umfpack_load_numeric.h umfpack_save_numeric.h ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_INCLUDE_DESTINATION}/suitesparse ) From e7dda6e398c1428158ce46f6f78178e540eb625f Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Tue, 30 Mar 2021 15:33:29 +0100 Subject: [PATCH 17/75] Set CMAKE_CXX_STANDARD to 14, remove add_definitions --- Schweizer-Messer/numpy_eigen/CMakeLists.txt | 4 +++- Schweizer-Messer/sm_common/CMakeLists.txt | 4 +++- Schweizer-Messer/sm_common/cmake/export_flags.cmake | 5 ++--- Schweizer-Messer/sm_kinematics/CMakeLists.txt | 4 +++- Schweizer-Messer/sm_logging/CMakeLists.txt | 4 ++-- Schweizer-Messer/sm_matrix_archive/CMakeLists.txt | 4 +++- Schweizer-Messer/sm_opencv/CMakeLists.txt | 4 +++- Schweizer-Messer/sm_property_tree/CMakeLists.txt | 3 ++- Schweizer-Messer/sm_python/CMakeLists.txt | 4 +++- Schweizer-Messer/sm_timing/CMakeLists.txt | 4 +++- aslam_cv/aslam_cv_python/CMakeLists.txt | 4 +++- .../incremental_calibration/CMakeLists.txt | 8 ++------ .../incremental_calibration_python/CMakeLists.txt | 8 ++------ aslam_optimizer/aslam_backend/CMakeLists.txt | 4 +++- aslam_optimizer/aslam_backend_expressions/CMakeLists.txt | 5 +++-- aslam_optimizer/aslam_backend_python/CMakeLists.txt | 4 +++- 16 files changed, 43 insertions(+), 30 deletions(-) diff --git a/Schweizer-Messer/numpy_eigen/CMakeLists.txt b/Schweizer-Messer/numpy_eigen/CMakeLists.txt index a1736b3b3..bb4cc3a94 100644 --- a/Schweizer-Messer/numpy_eigen/CMakeLists.txt +++ b/Schweizer-Messer/numpy_eigen/CMakeLists.txt @@ -1,6 +1,8 @@ cmake_minimum_required(VERSION 3.0.2) project(numpy_eigen) +set(CMAKE_CXX_STANDARD 14) + find_package(catkin REQUIRED COMPONENTS python_module) include_directories(${catkin_INCLUDE_DIRS}) find_package(Boost REQUIRED COMPONENTS system) @@ -17,7 +19,7 @@ catkin_package( LIBRARIES ${PROJECT_NAME} DEPENDS ) -add_definitions(-std=c++0x -D__STRICT_ANSI__) +add_definitions(-D__STRICT_ANSI__) # ignore "error: return-statement with a value, in function returning ‘void’" in "__multiarray_api.h" add_compile_options(-fpermissive) diff --git a/Schweizer-Messer/sm_common/CMakeLists.txt b/Schweizer-Messer/sm_common/CMakeLists.txt index db7b0f4a3..4ae7341b0 100644 --- a/Schweizer-Messer/sm_common/CMakeLists.txt +++ b/Schweizer-Messer/sm_common/CMakeLists.txt @@ -1,11 +1,13 @@ cmake_minimum_required(VERSION 3.0.2) project(sm_common) +set(CMAKE_CXX_STANDARD 14) + find_package(catkin REQUIRED) include_directories(${catkin_INCLUDE_DIRS}) find_package(Boost REQUIRED COMPONENTS system) -SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=c++0x -D__STRICT_ANSI__") +SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -D__STRICT_ANSI__") include_directories(include ${Boost_INCLUDE_DIRS}) diff --git a/Schweizer-Messer/sm_common/cmake/export_flags.cmake b/Schweizer-Messer/sm_common/cmake/export_flags.cmake index d0af91d3d..bde56c39f 100644 --- a/Schweizer-Messer/sm_common/cmake/export_flags.cmake +++ b/Schweizer-Messer/sm_common/cmake/export_flags.cmake @@ -1,3 +1,2 @@ -SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -std=c++0x") - - +SET(CMAKE_CXX_STANDARD 14) +SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra") diff --git a/Schweizer-Messer/sm_kinematics/CMakeLists.txt b/Schweizer-Messer/sm_kinematics/CMakeLists.txt index db6736a0d..c6a6b5e8a 100644 --- a/Schweizer-Messer/sm_kinematics/CMakeLists.txt +++ b/Schweizer-Messer/sm_kinematics/CMakeLists.txt @@ -1,6 +1,8 @@ cmake_minimum_required(VERSION 3.0.2) project(sm_kinematics) +set(CMAKE_CXX_STANDARD 14) + find_package(catkin REQUIRED COMPONENTS sm_common sm_eigen sm_boost sm_random) include_directories(${catkin_INCLUDE_DIRS}) find_package(Boost REQUIRED COMPONENTS system serialization filesystem) @@ -10,7 +12,7 @@ find_package(Eigen3 REQUIRED) include_directories(${EIGEN3_INCLUDE_DIRS}) add_definitions(${EIGEN_DEFINITIONS}) -SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=c++0x") +SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall") catkin_package( INCLUDE_DIRS include ${catkin_INCLUDE_DIRS} diff --git a/Schweizer-Messer/sm_logging/CMakeLists.txt b/Schweizer-Messer/sm_logging/CMakeLists.txt index 4025c0dee..7c5d0585d 100644 --- a/Schweizer-Messer/sm_logging/CMakeLists.txt +++ b/Schweizer-Messer/sm_logging/CMakeLists.txt @@ -1,6 +1,8 @@ cmake_minimum_required(VERSION 3.0.2) project(sm_logging) +set(CMAKE_CXX_STANDARD 14) + find_package(catkin REQUIRED) find_package(Boost REQUIRED COMPONENTS system thread regex) @@ -13,8 +15,6 @@ catkin_package( include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) -add_definitions(-std=c++0x) - ############## ## Building ## ############## diff --git a/Schweizer-Messer/sm_matrix_archive/CMakeLists.txt b/Schweizer-Messer/sm_matrix_archive/CMakeLists.txt index 4213be5e0..992900f0b 100644 --- a/Schweizer-Messer/sm_matrix_archive/CMakeLists.txt +++ b/Schweizer-Messer/sm_matrix_archive/CMakeLists.txt @@ -1,6 +1,8 @@ cmake_minimum_required(VERSION 3.0.2) project(sm_matrix_archive) +set(CMAKE_CXX_STANDARD 14) + find_package(catkin REQUIRED COMPONENTS sm_common) include_directories(${catkin_INCLUDE_DIRS}) find_package(Boost REQUIRED COMPONENTS system filesystem) @@ -14,7 +16,7 @@ catkin_package( CATKIN_DEPENDS sm_common DEPENDS ) -add_definitions(-std=c++0x -D__STRICT_ANSI__) +add_definitions(-D__STRICT_ANSI__) include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) diff --git a/Schweizer-Messer/sm_opencv/CMakeLists.txt b/Schweizer-Messer/sm_opencv/CMakeLists.txt index 248c37190..23c9a1707 100644 --- a/Schweizer-Messer/sm_opencv/CMakeLists.txt +++ b/Schweizer-Messer/sm_opencv/CMakeLists.txt @@ -1,6 +1,8 @@ cmake_minimum_required(VERSION 3.0.2) project(sm_opencv) +set(CMAKE_CXX_STANDARD 14) + find_package(catkin REQUIRED COMPONENTS sm_common) include_directories(${catkin_INCLUDE_DIRS}) find_package(Boost REQUIRED COMPONENTS system) @@ -10,6 +12,6 @@ catkin_package( CATKIN_DEPENDS sm_common DEPENDS ) -add_definitions(-std=c++0x -D__STRICT_ANSI__) +add_definitions(-D__STRICT_ANSI__) include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) diff --git a/Schweizer-Messer/sm_property_tree/CMakeLists.txt b/Schweizer-Messer/sm_property_tree/CMakeLists.txt index d036080c1..0fe7050f0 100644 --- a/Schweizer-Messer/sm_property_tree/CMakeLists.txt +++ b/Schweizer-Messer/sm_property_tree/CMakeLists.txt @@ -1,6 +1,8 @@ cmake_minimum_required(VERSION 3.0.2) project(sm_property_tree) +set(CMAKE_CXX_STANDARD 14) + find_package(catkin REQUIRED COMPONENTS sm_common) include_directories(${catkin_INCLUDE_DIRS}) find_package(Boost REQUIRED COMPONENTS system filesystem) @@ -12,7 +14,6 @@ catkin_package( DEPENDS ) add_definitions(-D__STRICT_ANSI__) -remove_definitions(-std=c++0x -std-c++11) include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) diff --git a/Schweizer-Messer/sm_python/CMakeLists.txt b/Schweizer-Messer/sm_python/CMakeLists.txt index 9e260780b..0f55f404f 100644 --- a/Schweizer-Messer/sm_python/CMakeLists.txt +++ b/Schweizer-Messer/sm_python/CMakeLists.txt @@ -1,6 +1,8 @@ cmake_minimum_required(VERSION 3.0.2) project(sm_python) +set(CMAKE_CXX_STANDARD 14) + #catkin_python_setup() find_package(catkin REQUIRED COMPONENTS sm_common numpy_eigen sm_kinematics @@ -19,7 +21,7 @@ catkin_package( sm_matrix_archive sm_property_tree python_module DEPENDS ) -add_definitions(-std=c++0x -D__STRICT_ANSI__) +add_definitions(-D__STRICT_ANSI__) include_directories(include) diff --git a/Schweizer-Messer/sm_timing/CMakeLists.txt b/Schweizer-Messer/sm_timing/CMakeLists.txt index 0c8dcf28c..834c7cb7a 100644 --- a/Schweizer-Messer/sm_timing/CMakeLists.txt +++ b/Schweizer-Messer/sm_timing/CMakeLists.txt @@ -1,6 +1,8 @@ cmake_minimum_required(VERSION 3.0.2) project(sm_timing) +set(CMAKE_CXX_STANDARD 14) + find_package(catkin REQUIRED COMPONENTS sm_common sm_random) include_directories(${catkin_INCLUDE_DIRS}) find_package(Boost REQUIRED COMPONENTS system) @@ -11,7 +13,7 @@ catkin_package( CATKIN_DEPENDS sm_common sm_random DEPENDS ) -add_definitions(-std=c++0x -D__STRICT_ANSI__) +add_definitions(-D__STRICT_ANSI__) include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) diff --git a/aslam_cv/aslam_cv_python/CMakeLists.txt b/aslam_cv/aslam_cv_python/CMakeLists.txt index 20ac48801..1a20a643e 100644 --- a/aslam_cv/aslam_cv_python/CMakeLists.txt +++ b/aslam_cv/aslam_cv_python/CMakeLists.txt @@ -1,10 +1,12 @@ cmake_minimum_required(VERSION 3.0.2) project(aslam_cv_python) +set(CMAKE_CXX_STANDARD 14) + find_package(catkin_simple REQUIRED) catkin_simple() -add_definitions( -fPIC -Wextra -Winit-self -Woverloaded-virtual -Wnon-virtual-dtor -Wsign-promo -Wno-long-long -std=c++0x) +add_definitions(-fPIC -Wextra -Winit-self -Woverloaded-virtual -Wnon-virtual-dtor -Wsign-promo -Wno-long-long) if(APPLE) add_definitions( -ftemplate-depth-1024) diff --git a/aslam_incremental_calibration/incremental_calibration/CMakeLists.txt b/aslam_incremental_calibration/incremental_calibration/CMakeLists.txt index cc768cbb4..64f5b582c 100644 --- a/aslam_incremental_calibration/incremental_calibration/CMakeLists.txt +++ b/aslam_incremental_calibration/incremental_calibration/CMakeLists.txt @@ -1,17 +1,13 @@ cmake_minimum_required(VERSION 3.0.2) project(incremental_calibration) +set(CMAKE_CXX_STANDARD 14) + set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${PROJECT_SOURCE_DIR}/../cmake/) find_package(catkin_simple REQUIRED) catkin_simple() -if(APPLE) - set(CMAKE_CXX_FLAGS "-std=c++11") -else() - set(CMAKE_CXX_FLAGS "-std=c++0x") -endif() - cs_add_library(${PROJECT_NAME} src/base/Serializable.cpp src/base/Timestamp.cpp diff --git a/aslam_incremental_calibration/incremental_calibration_python/CMakeLists.txt b/aslam_incremental_calibration/incremental_calibration_python/CMakeLists.txt index 890777355..fcdac39ca 100644 --- a/aslam_incremental_calibration/incremental_calibration_python/CMakeLists.txt +++ b/aslam_incremental_calibration/incremental_calibration_python/CMakeLists.txt @@ -1,15 +1,11 @@ cmake_minimum_required(VERSION 3.0.2) project(incremental_calibration_python) +set(CMAKE_CXX_STANDARD 14) + find_package(catkin_simple REQUIRED) catkin_simple() -if(APPLE) - -else() - add_definitions(-std=c++0x) -endif() - add_python_export_library(${PROJECT_NAME} src/incremental_calibration src/module.cpp src/OptimizationProblem.cpp diff --git a/aslam_optimizer/aslam_backend/CMakeLists.txt b/aslam_optimizer/aslam_backend/CMakeLists.txt index 269f4773b..3b8890f69 100644 --- a/aslam_optimizer/aslam_backend/CMakeLists.txt +++ b/aslam_optimizer/aslam_backend/CMakeLists.txt @@ -1,6 +1,8 @@ cmake_minimum_required(VERSION 3.0.2) project(aslam_backend) +set(CMAKE_CXX_STANDARD 14) + find_package(catkin_simple REQUIRED) catkin_simple() @@ -8,7 +10,7 @@ find_package(Boost REQUIRED COMPONENTS system thread) include_directories(${EIGEN3_INCLUDE_DIRS}) -#add_definitions( -fPIC -msse2 -mssse3 -march=nocona -Wextra -Winit-self -Woverloaded-virtual -Wnon-virtual-dtor -Wsign-promo -Wno-long-long -std=c++0x -O3) +#add_definitions( -fPIC -msse2 -mssse3 -march=nocona -Wextra -Winit-self -Woverloaded-virtual -Wnon-virtual-dtor -Wsign-promo -Wno-long-long -O3) #target_link_libraries(${PROJECT_NAME} ${CHOLMOD_LIBRARIES} ${EXTRA_LIBS} ${CSPARSE_LIBRARY}) #rosbuild_link_boost(${PROJECT_NAME} thread system) diff --git a/aslam_optimizer/aslam_backend_expressions/CMakeLists.txt b/aslam_optimizer/aslam_backend_expressions/CMakeLists.txt index ca3a8e4d2..a1d3d69f6 100644 --- a/aslam_optimizer/aslam_backend_expressions/CMakeLists.txt +++ b/aslam_optimizer/aslam_backend_expressions/CMakeLists.txt @@ -1,9 +1,8 @@ cmake_minimum_required(VERSION 3.0.2) project(aslam_backend_expressions) -include_directories(include ${catkin_INCLUDE_DIRS}) +set(CMAKE_CXX_STANDARD 14) -SET(CMAKE_CXX_FLAGS "-std=c++0x") find_package(catkin_simple REQUIRED) @@ -11,6 +10,8 @@ catkin_simple() include_directories(${EIGEN3_INCLUDE_DIRS}) +include_directories(include) + find_package(Boost REQUIRED COMPONENTS system) cs_add_library(${PROJECT_NAME} diff --git a/aslam_optimizer/aslam_backend_python/CMakeLists.txt b/aslam_optimizer/aslam_backend_python/CMakeLists.txt index b54416c56..c572a33b0 100644 --- a/aslam_optimizer/aslam_backend_python/CMakeLists.txt +++ b/aslam_optimizer/aslam_backend_python/CMakeLists.txt @@ -1,10 +1,12 @@ cmake_minimum_required(VERSION 3.0.2) project(aslam_backend_python) +set(CMAKE_CXX_STANDARD 14) + find_package(catkin_simple REQUIRED) catkin_simple() -add_definitions( -fPIC -Wextra -Winit-self -Woverloaded-virtual -Wnon-virtual-dtor -Wsign-promo -Wno-long-long -std=c++0x) +add_definitions( -fPIC -Wextra -Winit-self -Woverloaded-virtual -Wnon-virtual-dtor -Wsign-promo -Wno-long-long) # This functions take TARGET_NAME PYTHON_MODULE_DIRECTORY sourceFile1 [sourceFile2 ...] add_python_export_library(${PROJECT_NAME} python/aslam_backend From dcd156c25ce079b469a6157e9ca64e7e821ee488 Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Tue, 30 Mar 2021 15:34:26 +0100 Subject: [PATCH 18/75] Add -Wno-deprecated-copy for now --- Schweizer-Messer/sm_common/CMakeLists.txt | 2 +- aslam_cv/aslam_cameras/CMakeLists.txt | 1 + aslam_cv/aslam_cv_python/CMakeLists.txt | 1 + 3 files changed, 3 insertions(+), 1 deletion(-) diff --git a/Schweizer-Messer/sm_common/CMakeLists.txt b/Schweizer-Messer/sm_common/CMakeLists.txt index 4ae7341b0..d13890c74 100644 --- a/Schweizer-Messer/sm_common/CMakeLists.txt +++ b/Schweizer-Messer/sm_common/CMakeLists.txt @@ -24,7 +24,7 @@ add_library(${PROJECT_NAME} ) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) - +target_compile_options(${PROJECT_NAME} PUBLIC "-Wno-deprecated-copy") # TODO: Fix... ################## ## Installation ## diff --git a/aslam_cv/aslam_cameras/CMakeLists.txt b/aslam_cv/aslam_cameras/CMakeLists.txt index 20cda664b..277c15be6 100644 --- a/aslam_cv/aslam_cameras/CMakeLists.txt +++ b/aslam_cv/aslam_cameras/CMakeLists.txt @@ -35,6 +35,7 @@ cs_add_library(${PROJECT_NAME} ) target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES}) +target_compile_options(${PROJECT_NAME} PUBLIC "-Wno-deprecated-copy") # TODO: Fix... if(CATKIN_ENABLE_TESTING) diff --git a/aslam_cv/aslam_cv_python/CMakeLists.txt b/aslam_cv/aslam_cv_python/CMakeLists.txt index 1a20a643e..2e861e02a 100644 --- a/aslam_cv/aslam_cv_python/CMakeLists.txt +++ b/aslam_cv/aslam_cv_python/CMakeLists.txt @@ -37,6 +37,7 @@ add_python_export_library(${PROJECT_NAME} python/aslam_cv src/OmniUndistorter.cpp src/PinholeUndistorter.cpp ) +target_compile_options(${PROJECT_NAME} PUBLIC "-Wno-deprecated-copy") # TODO: Fix... find_package(Boost REQUIRED COMPONENTS serialization) target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES}) From a5e813e31a244bdbd70e80b43eae1c614de425fa Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Tue, 30 Mar 2021 15:34:41 +0100 Subject: [PATCH 19/75] Fix deprecation --- Schweizer-Messer/sm_boost/src/portable_binary_iarchive.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Schweizer-Messer/sm_boost/src/portable_binary_iarchive.cpp b/Schweizer-Messer/sm_boost/src/portable_binary_iarchive.cpp index 107276873..9c315974f 100755 --- a/Schweizer-Messer/sm_boost/src/portable_binary_iarchive.cpp +++ b/Schweizer-Messer/sm_boost/src/portable_binary_iarchive.cpp @@ -11,7 +11,7 @@ #include #include -#include +#include #include #include From aa92df71c6cf0b0427d07e957b0d7a76d26a8af0 Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Tue, 30 Mar 2021 15:34:53 +0100 Subject: [PATCH 20/75] Eigen include fixes --- aslam_optimizer/aslam_backend/CMakeLists.txt | 1 + aslam_optimizer/aslam_backend_expressions/CMakeLists.txt | 1 + 2 files changed, 2 insertions(+) diff --git a/aslam_optimizer/aslam_backend/CMakeLists.txt b/aslam_optimizer/aslam_backend/CMakeLists.txt index 3b8890f69..b5e5f7b91 100644 --- a/aslam_optimizer/aslam_backend/CMakeLists.txt +++ b/aslam_optimizer/aslam_backend/CMakeLists.txt @@ -8,6 +8,7 @@ catkin_simple() find_package(Boost REQUIRED COMPONENTS system thread) +find_package(Eigen3 REQUIRED) include_directories(${EIGEN3_INCLUDE_DIRS}) #add_definitions( -fPIC -msse2 -mssse3 -march=nocona -Wextra -Winit-self -Woverloaded-virtual -Wnon-virtual-dtor -Wsign-promo -Wno-long-long -O3) diff --git a/aslam_optimizer/aslam_backend_expressions/CMakeLists.txt b/aslam_optimizer/aslam_backend_expressions/CMakeLists.txt index a1d3d69f6..b7f00a9b4 100644 --- a/aslam_optimizer/aslam_backend_expressions/CMakeLists.txt +++ b/aslam_optimizer/aslam_backend_expressions/CMakeLists.txt @@ -8,6 +8,7 @@ find_package(catkin_simple REQUIRED) catkin_simple() +find_package(Eigen3 REQUIRED) include_directories(${EIGEN3_INCLUDE_DIRS}) include_directories(include) From 023d930974d77bb9b84bcf993dae1198a922aa42 Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Tue, 30 Mar 2021 15:36:09 +0100 Subject: [PATCH 21/75] Remove unused local typedefs, unused variables --- Schweizer-Messer/sm_python/src/exportHomogeneousPoint.cpp | 5 ----- .../aslam/cameras/implementation/DoubleSphereProjection.hpp | 2 +- .../aslam_cv_python/include/aslam/python/ExportFrame.hpp | 2 -- 3 files changed, 1 insertion(+), 8 deletions(-) diff --git a/Schweizer-Messer/sm_python/src/exportHomogeneousPoint.cpp b/Schweizer-Messer/sm_python/src/exportHomogeneousPoint.cpp index fe9483214..ef311e18a 100755 --- a/Schweizer-Messer/sm_python/src/exportHomogeneousPoint.cpp +++ b/Schweizer-Messer/sm_python/src/exportHomogeneousPoint.cpp @@ -7,10 +7,6 @@ using namespace sm::kinematics; void exportHomogeneousPoint() { - - typedef Eigen::Matrix3d euclidean_jacobian_t; - typedef Eigen::Matrix homogeneous_jacobian_t; - class_ >("HomogeneousPoint", init<>()) .def(init()) .def(init()) @@ -48,5 +44,4 @@ void exportHomogeneousPoint() .def("setUOplus", &UncertainHomogeneousPoint::setUOplus) .def("normalize", &UncertainHomogeneousPoint::normalize) ; - } diff --git a/aslam_cv/aslam_cameras/include/aslam/cameras/implementation/DoubleSphereProjection.hpp b/aslam_cv/aslam_cameras/include/aslam/cameras/implementation/DoubleSphereProjection.hpp index 90e123f87..701f52ba7 100644 --- a/aslam_cv/aslam_cameras/include/aslam/cameras/implementation/DoubleSphereProjection.hpp +++ b/aslam_cv/aslam_cameras/include/aslam/cameras/implementation/DoubleSphereProjection.hpp @@ -505,7 +505,7 @@ void DoubleSphereProjection::euclideanToKeypointIntrinsicsJacobian template template void DoubleSphereProjection::euclideanToKeypointDistortionJacobian( - const Eigen::MatrixBase & p, + const Eigen::MatrixBase & /*p*/, const Eigen::MatrixBase & outJd) const { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE_OR_DYNAMIC( diff --git a/aslam_cv/aslam_cv_python/include/aslam/python/ExportFrame.hpp b/aslam_cv/aslam_cv_python/include/aslam/python/ExportFrame.hpp index cc2ab3de1..f07a06619 100644 --- a/aslam_cv/aslam_cv_python/include/aslam/python/ExportFrame.hpp +++ b/aslam_cv/aslam_cv_python/include/aslam/python/ExportFrame.hpp @@ -19,7 +19,6 @@ void exportKeypoint() { using namespace aslam; typedef aslam::Keypoint keypoint_t; - typedef DescriptorBase descriptor_t; std::stringstream str; str << "Keypoint" << D; @@ -141,7 +140,6 @@ void exportFrame(const std::string & name) { using namespace boost::python; using namespace aslam; typedef CAMERA_GEOMETRY_T geometry_t; - typedef DescriptorBase descriptor_t; typedef Frame frame_t; typedef typename frame_t::keypoint_t keypoint_t; From 645d5407e425195b2bc68aa389689d565f195e92 Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Tue, 30 Mar 2021 16:33:50 +0100 Subject: [PATCH 22/75] [opencv2_catkin] Use STATUS message to avoid stderr out --- opencv2_catkin/cmake/opencv2-extras.cmake.in | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/opencv2_catkin/cmake/opencv2-extras.cmake.in b/opencv2_catkin/cmake/opencv2-extras.cmake.in index 45e7d7588..f808a4db5 100644 --- a/opencv2_catkin/cmake/opencv2-extras.cmake.in +++ b/opencv2_catkin/cmake/opencv2-extras.cmake.in @@ -7,6 +7,6 @@ find_package(OpenCV) set(opencv2_catkin_LIBRARY_DIRS ${OpenCV_LIB_DIR}) set(opencv2_catkin_LIBRARIES ${OpenCV_LIBS}) set(opencv2_catkin_INCLUDE_DIRS ${OpenCV_INCLUDE_DIRS}) -message("-- Using OpenCV library directory ${opencv2_catkin_LIBRARY_DIRS}") -message("-- Found OpenCV include directory ${opencv2_catkin_INCLUDE_DIRS}") -message("-- Found OpenCV libraries ${opencv2_catkin_LIBRARIES}") +message(STATUS "-- Using OpenCV library directory ${opencv2_catkin_LIBRARY_DIRS}") +message(STATUS "-- Found OpenCV include directory ${opencv2_catkin_INCLUDE_DIRS}") +message(STATUS "-- Found OpenCV libraries ${opencv2_catkin_LIBRARIES}") From 8fa6e009ad379bda45470c43a6f639a1394e7659 Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Tue, 30 Mar 2021 17:15:05 +0100 Subject: [PATCH 23/75] [ethz_apriltag2] Remove eigen2, add v4l dependency --- aslam_offline_calibration/ethz_apriltag2/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aslam_offline_calibration/ethz_apriltag2/package.xml b/aslam_offline_calibration/ethz_apriltag2/package.xml index 508902ee4..0e190d077 100644 --- a/aslam_offline_calibration/ethz_apriltag2/package.xml +++ b/aslam_offline_calibration/ethz_apriltag2/package.xml @@ -9,5 +9,5 @@ catkin eigen opencv - eigen2 + libv4l-dev From 8a4784c4dc22db059b7fa949a51d2a25d03f6461 Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Tue, 30 Mar 2021 17:20:29 +0100 Subject: [PATCH 24/75] Minor formatting + CMake fixes --- .../include/aslam/cameras/PinholeProjection.hpp | 1 - aslam_cv/aslam_cv_python/src/CameraProjections.cpp | 14 +++++--------- aslam_cv/aslam_imgproc/CMakeLists.txt | 3 +-- .../ethz_apriltag2/CMakeLists.txt | 3 +-- 4 files changed, 7 insertions(+), 14 deletions(-) diff --git a/aslam_cv/aslam_cameras/include/aslam/cameras/PinholeProjection.hpp b/aslam_cv/aslam_cameras/include/aslam/cameras/PinholeProjection.hpp index 79eb347bd..d3195f63e 100644 --- a/aslam_cv/aslam_cameras/include/aslam/cameras/PinholeProjection.hpp +++ b/aslam_cv/aslam_cameras/include/aslam/cameras/PinholeProjection.hpp @@ -264,7 +264,6 @@ class PinholeProjection { double _fu_over_fv; distortion_t _distortion; - }; } // namespace cameras diff --git a/aslam_cv/aslam_cv_python/src/CameraProjections.cpp b/aslam_cv/aslam_cv_python/src/CameraProjections.cpp index ea858a272..b9d4858e3 100644 --- a/aslam_cv/aslam_cv_python/src/CameraProjections.cpp +++ b/aslam_cv/aslam_cv_python/src/CameraProjections.cpp @@ -288,7 +288,6 @@ void exportOmniProjection(std::string name) { sm::python::pickle_suite >()); exportGenericProjectionFunctions >(omniProjection); //exportGenericProjectionDesignVariable< OmniProjection >(name); - } template @@ -334,7 +333,6 @@ void exportExtendedUnifiedProjection(std::string name) { sm::python::pickle_suite >()); exportGenericProjectionFunctions >(extendedUnifiedProjection); //exportGenericProjectionDesignVariable< ExtendedUnifiedProjection >(name); - } template @@ -380,7 +378,6 @@ void exportDoubleSphereProjection(std::string name) { sm::python::pickle_suite >()); exportGenericProjectionFunctions >(doubleSphereProjection); //exportGenericProjectionDesignVariable< DoubleSphereProjection >(name); - } template @@ -413,18 +410,17 @@ void exportPinholeProjection(std::string name) { /// \brief The horizontal resolution in pixels. .def("ru", &PinholeProjection::ru) /// \brief The vertical resolution in pixels. - .def("rv", &PinholeProjection::rv).def( - "focalLengthCol", &PinholeProjection::focalLengthCol).def( - "focalLengthRow", &PinholeProjection::focalLengthRow).def( - "opticalCenterCol", &PinholeProjection::opticalCenterCol).def( - "opticalCenterRow", &PinholeProjection::opticalCenterRow) + .def("rv", &PinholeProjection::rv) + .def("focalLengthCol", &PinholeProjection::focalLengthCol) + .def("focalLengthRow", &PinholeProjection::focalLengthRow) + .def("opticalCenterCol", &PinholeProjection::opticalCenterCol) + .def("opticalCenterRow", &PinholeProjection::opticalCenterRow) //.def("distortion",&PinholeProjection::distortion, return_internal_reference<>()) .def("distortion", distortion1, return_internal_reference<>()).def( "setDistortion", &PinholeProjection::setDistortion).def_pickle( sm::python::pickle_suite >()); exportGenericProjectionFunctions >(pinholeProjection); //exportGenericProjectionDesignVariable< PinholeProjection >(name); - } void exportCameraProjections() { diff --git a/aslam_cv/aslam_imgproc/CMakeLists.txt b/aslam_cv/aslam_imgproc/CMakeLists.txt index da53b7282..737bd315f 100644 --- a/aslam_cv/aslam_imgproc/CMakeLists.txt +++ b/aslam_cv/aslam_imgproc/CMakeLists.txt @@ -9,8 +9,7 @@ find_package(Boost REQUIRED COMPONENTS system) include_directories(${EIGEN3_INCLUDE_DIRS}) cs_add_library(${PROJECT_NAME} src/UndistorterBase.cpp) - - +target_compile_options(${PROJECT_NAME} PUBLIC -Wno-deprecated-copy -Wno-maybe-uninitialized) target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES}) cs_install() diff --git a/aslam_offline_calibration/ethz_apriltag2/CMakeLists.txt b/aslam_offline_calibration/ethz_apriltag2/CMakeLists.txt index 332e35930..8befbefaf 100644 --- a/aslam_offline_calibration/ethz_apriltag2/CMakeLists.txt +++ b/aslam_offline_calibration/ethz_apriltag2/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8) +cmake_minimum_required(VERSION 3.0.2) project(ethz_apriltag2) @@ -8,7 +8,6 @@ include_directories(${catkin_INCLUDE_DIRS}) find_package(Eigen3 REQUIRED) catkin_package( - DEPENDS eigen opencv INCLUDE_DIRS include ${EIGEN3_INCLUDE_DIRS} LIBRARIES ${PROJECT_NAME} ) From 9e5ca838eb7fe2ef933dc5c5f765064298ad24ed Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Tue, 30 Mar 2021 17:31:54 +0100 Subject: [PATCH 25/75] Fix rosdep keys --- aslam_cv/aslam_cameras/package.xml | 1 - aslam_cv/aslam_cv_backend/package.xml | 2 -- aslam_cv/aslam_cv_backend_python/package.xml | 3 --- aslam_cv/aslam_cv_python/package.xml | 7 ------- aslam_cv/aslam_cv_serialization/package.xml | 3 --- aslam_cv/aslam_imgproc/package.xml | 3 --- .../incremental_calibration/package.xml | 2 +- aslam_nonparametric_estimation/aslam_splines/package.xml | 1 - .../aslam_splines_python/package.xml | 1 - aslam_nonparametric_estimation/bsplines/package.xml | 1 - aslam_nonparametric_estimation/bsplines_python/package.xml | 1 - aslam_offline_calibration/ethz_apriltag2/package.xml | 2 +- aslam_offline_calibration/kalibr/package.xml | 5 +---- aslam_optimizer/aslam_backend/package.xml | 7 ++----- aslam_optimizer/aslam_backend_python/package.xml | 1 - aslam_optimizer/sparse_block_matrix/package.xml | 3 +-- opencv2_catkin/package.xml | 2 +- suitesparse/package.xml | 4 ++-- 18 files changed, 9 insertions(+), 40 deletions(-) diff --git a/aslam_cv/aslam_cameras/package.xml b/aslam_cv/aslam_cameras/package.xml index f58bc6942..954702f5c 100644 --- a/aslam_cv/aslam_cameras/package.xml +++ b/aslam_cv/aslam_cameras/package.xml @@ -20,7 +20,6 @@ email="paul.furgale@mavt.ethz.ch">Paul Furgale sm_opencv sm_property_tree opencv2_catkin - eigen_catkin sm_random diff --git a/aslam_cv/aslam_cv_backend/package.xml b/aslam_cv/aslam_cv_backend/package.xml index 3e2e87ed5..b43d28183 100644 --- a/aslam_cv/aslam_cv_backend/package.xml +++ b/aslam_cv/aslam_cv_backend/package.xml @@ -19,12 +19,10 @@ sm_timing aslam_backend - aslam_camera_system aslam_backend_expressions aslam_time aslam_cameras - eigen_catkin opencv2_catkin suitesparse sparse_block_matrix diff --git a/aslam_cv/aslam_cv_backend_python/package.xml b/aslam_cv/aslam_cv_backend_python/package.xml index 9b6df1b7a..4cd3a5cf0 100644 --- a/aslam_cv/aslam_cv_backend_python/package.xml +++ b/aslam_cv/aslam_cv_backend_python/package.xml @@ -21,11 +21,8 @@ aslam_backend aslam_backend_expressions aslam_backend_python - aslam_features2d opencv2_catkin aslam_cameras - aslam_camera_system - aslam_frames aslam_cv_error_terms aslam_splines aslam_cv_python diff --git a/aslam_cv/aslam_cv_python/package.xml b/aslam_cv/aslam_cv_python/package.xml index c178fbabe..af696cea4 100644 --- a/aslam_cv/aslam_cv_python/package.xml +++ b/aslam_cv/aslam_cv_python/package.xml @@ -17,16 +17,9 @@ python_module - brisk - aslam_cv_serialization aslam_imgproc aslam_cameras aslam_time - aslam_features2d - aslam_camera_system - aslam_matcher aslam_cv_serialization - - eigen_catkin diff --git a/aslam_cv/aslam_cv_serialization/package.xml b/aslam_cv/aslam_cv_serialization/package.xml index d56c81e1e..7b307a7bf 100644 --- a/aslam_cv/aslam_cv_serialization/package.xml +++ b/aslam_cv/aslam_cv_serialization/package.xml @@ -9,9 +9,6 @@ catkin_simple aslam_cameras - aslam_frames - aslam_features2d - aslam_camera_system aslam_time sm_common diff --git a/aslam_cv/aslam_imgproc/package.xml b/aslam_cv/aslam_imgproc/package.xml index c4d09a9c8..6a7d99e02 100644 --- a/aslam_cv/aslam_imgproc/package.xml +++ b/aslam_cv/aslam_imgproc/package.xml @@ -12,7 +12,6 @@ sm_common - sm_time sm_common sm_eigen sm_random @@ -23,9 +22,7 @@ sm_property_tree aslam_time aslam_cameras - aslam_frames opencv2_catkin - eigen_catkin numpy_eigen diff --git a/aslam_incremental_calibration/incremental_calibration/package.xml b/aslam_incremental_calibration/incremental_calibration/package.xml index 6042b702d..b873ba6a2 100644 --- a/aslam_incremental_calibration/incremental_calibration/package.xml +++ b/aslam_incremental_calibration/incremental_calibration/package.xml @@ -16,7 +16,7 @@ sm_eigen aslam_backend suitesparse - TBB + tbb sm_eigen aslam_backend diff --git a/aslam_nonparametric_estimation/aslam_splines/package.xml b/aslam_nonparametric_estimation/aslam_splines/package.xml index ff0d6f991..892e72d1a 100644 --- a/aslam_nonparametric_estimation/aslam_splines/package.xml +++ b/aslam_nonparametric_estimation/aslam_splines/package.xml @@ -15,7 +15,6 @@ numpy_eigen sparse_block_matrix suitesparse - eigen_catkin sm_common sm_kinematics sm_timing diff --git a/aslam_nonparametric_estimation/aslam_splines_python/package.xml b/aslam_nonparametric_estimation/aslam_splines_python/package.xml index 0196a7fbc..477eea23c 100644 --- a/aslam_nonparametric_estimation/aslam_splines_python/package.xml +++ b/aslam_nonparametric_estimation/aslam_splines_python/package.xml @@ -14,7 +14,6 @@ bsplines numpy_eigen python_module - eigen_catkin sparse_block_matrix sm_common suitesparse diff --git a/aslam_nonparametric_estimation/bsplines/package.xml b/aslam_nonparametric_estimation/bsplines/package.xml index ae11fd246..86f68d650 100644 --- a/aslam_nonparametric_estimation/bsplines/package.xml +++ b/aslam_nonparametric_estimation/bsplines/package.xml @@ -14,7 +14,6 @@ sm_eigen sm_kinematics sparse_block_matrix - eigen_catkin suitesparse diff --git a/aslam_nonparametric_estimation/bsplines_python/package.xml b/aslam_nonparametric_estimation/bsplines_python/package.xml index 24ccb9d70..5a629c134 100644 --- a/aslam_nonparametric_estimation/bsplines_python/package.xml +++ b/aslam_nonparametric_estimation/bsplines_python/package.xml @@ -12,7 +12,6 @@ bsplines numpy_eigen python_module - eigen_catkin sparse_block_matrix sm_common suitesparse diff --git a/aslam_offline_calibration/ethz_apriltag2/package.xml b/aslam_offline_calibration/ethz_apriltag2/package.xml index 0e190d077..780438a72 100644 --- a/aslam_offline_calibration/ethz_apriltag2/package.xml +++ b/aslam_offline_calibration/ethz_apriltag2/package.xml @@ -8,6 +8,6 @@ New BSD catkin eigen - opencv + libopencv-dev libv4l-dev diff --git a/aslam_offline_calibration/kalibr/package.xml b/aslam_offline_calibration/kalibr/package.xml index b0d7d74f0..ae9f15d36 100644 --- a/aslam_offline_calibration/kalibr/package.xml +++ b/aslam_offline_calibration/kalibr/package.xml @@ -16,16 +16,13 @@ aslam_backend_python incremental_calibration_python aslam_cameras_april - aslam_cameras_april_python aslam_splines_python - eigen_catkin numpy_eigen python_module sparse_block_matrix sm_python aslam_cv_backend_python - - aslam_cameras_april_python + aslam_cv_backend_python aslam_backend_python incremental_calibration_python diff --git a/aslam_optimizer/aslam_backend/package.xml b/aslam_optimizer/aslam_backend/package.xml index 4a3e36ea3..994721a40 100644 --- a/aslam_optimizer/aslam_backend/package.xml +++ b/aslam_optimizer/aslam_backend/package.xml @@ -16,8 +16,7 @@ sm_logging sm_property_tree suitesparse - TBB - asl_cmake_modules + tbb sparse_block_matrix sm_boost @@ -25,7 +24,5 @@ sm_timing sm_logging sm_property_tree - TBB - asl_cmake_modules - + tbb diff --git a/aslam_optimizer/aslam_backend_python/package.xml b/aslam_optimizer/aslam_backend_python/package.xml index 26d9d82f5..36bc3ad3e 100644 --- a/aslam_optimizer/aslam_backend_python/package.xml +++ b/aslam_optimizer/aslam_backend_python/package.xml @@ -13,7 +13,6 @@ aslam_backend_expressions numpy_eigen python_module - eigen_catkin sparse_block_matrix sm_common sm_kinematics diff --git a/aslam_optimizer/sparse_block_matrix/package.xml b/aslam_optimizer/sparse_block_matrix/package.xml index b46b9bd48..8c38f86a2 100644 --- a/aslam_optimizer/sparse_block_matrix/package.xml +++ b/aslam_optimizer/sparse_block_matrix/package.xml @@ -9,8 +9,7 @@ catkin catkin_simple - TBB - asl_cmake_modules + tbb sm_common sm_eigen suitesparse diff --git a/opencv2_catkin/package.xml b/opencv2_catkin/package.xml index f55657610..f00361734 100644 --- a/opencv2_catkin/package.xml +++ b/opencv2_catkin/package.xml @@ -10,5 +10,5 @@ catkin catkin_simple - opencv2 + libopencv-dev diff --git a/suitesparse/package.xml b/suitesparse/package.xml index be73627e3..b35832c2d 100644 --- a/suitesparse/package.xml +++ b/suitesparse/package.xml @@ -11,8 +11,8 @@ Dr. Tim Davis catkin - lapack - blas + liblapack-dev + libblas-dev gfortran From cb3e719ce72e70ab7aed0b67a6bdd47957b1a3ce Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Tue, 30 Mar 2021 17:43:35 +0100 Subject: [PATCH 26/75] [kalibr] Use catkin_install_python --- aslam_offline_calibration/kalibr/CMakeLists.txt | 15 +++++++++++++++ .../kalibr/python/kalibr_calibrate_rs_cameras | 0 aslam_offline_calibration/kalibr/setup.py | 16 +--------------- 3 files changed, 16 insertions(+), 15 deletions(-) mode change 100644 => 100755 aslam_offline_calibration/kalibr/python/kalibr_calibrate_rs_cameras diff --git a/aslam_offline_calibration/kalibr/CMakeLists.txt b/aslam_offline_calibration/kalibr/CMakeLists.txt index d08833109..dbdd098f4 100644 --- a/aslam_offline_calibration/kalibr/CMakeLists.txt +++ b/aslam_offline_calibration/kalibr/CMakeLists.txt @@ -39,3 +39,18 @@ endif() catkin_python_setup() cs_install() cs_export() + +catkin_install_python( + PROGRAMS + python/kalibr_bagcreater + python/kalibr_camera_focus + python/kalibr_bagextractor + python/kalibr_camera_validator + python/kalibr_visualize_calibration + python/kalibr_calibrate_cameras + python/kalibr_visualize_distortion + python/kalibr_calibrate_imu_camera + python/kalibr_create_target_pdf + python/kalibr_calibrate_rs_cameras + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_calibrate_rs_cameras b/aslam_offline_calibration/kalibr/python/kalibr_calibrate_rs_cameras old mode 100644 new mode 100755 diff --git a/aslam_offline_calibration/kalibr/setup.py b/aslam_offline_calibration/kalibr/setup.py index 5d6e71bf3..bb1ca662e 100644 --- a/aslam_offline_calibration/kalibr/setup.py +++ b/aslam_offline_calibration/kalibr/setup.py @@ -9,21 +9,7 @@ 'kalibr_common', 'kalibr_camera_calibration', 'kalibr_imu_camera_calibration'], - package_dir={'':'python'}, - scripts=['python/kalibr_bagcreater', - 'python/kalibr_bagextractor', - 'python/kalibr_calibrate_cameras', - 'python/kalibr_calibrate_rs_cameras', - 'python/kalibr_calibrate_imu_camera', - 'python/kalibr_camera_focus', - 'python/kalibr_camera_validator', - 'python/kalibr_create_target_pdf', - 'python/kalibr_visualize_calibration', - 'python/kalibr_visualize_distortion', - 'python/exporters/kalibr_maplab_config', - 'python/exporters/kalibr_msf_config', - 'python/exporters/kalibr_okvis_config', - 'python/exporters/kalibr_rovio_config'] + package_dir={'':'python'} ) setup(**setup_args) From e18cfcd73c85f3ffbae19768c9a6771e7999a905 Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Tue, 30 Mar 2021 17:43:54 +0100 Subject: [PATCH 27/75] Fix issues detected by CI --- Schweizer-Messer/sm_common/CMakeLists.txt | 5 ++--- Schweizer-Messer/sm_common/package.xml | 1 + suitesparse/package.xml | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/Schweizer-Messer/sm_common/CMakeLists.txt b/Schweizer-Messer/sm_common/CMakeLists.txt index d13890c74..030c5978d 100644 --- a/Schweizer-Messer/sm_common/CMakeLists.txt +++ b/Schweizer-Messer/sm_common/CMakeLists.txt @@ -9,7 +9,6 @@ find_package(Boost REQUIRED COMPONENTS system) SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -D__STRICT_ANSI__") -include_directories(include ${Boost_INCLUDE_DIRS}) catkin_package( INCLUDE_DIRS include ${catkin_INCLUDE_DIRS} @@ -18,7 +17,8 @@ catkin_package( DEPENDS CFG_EXTRAS export_flags.cmake ) - + +include_directories(include ${Boost_INCLUDE_DIRS}) add_library(${PROJECT_NAME} src/progress_info.cpp ) @@ -41,7 +41,6 @@ install(TARGETS ${PROJECT_NAME} install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} FILES_MATCHING PATTERN "*.hpp" - PATTERN ".svn" EXCLUDE ) ############# diff --git a/Schweizer-Messer/sm_common/package.xml b/Schweizer-Messer/sm_common/package.xml index 26e5eb1f6..2b5454885 100644 --- a/Schweizer-Messer/sm_common/package.xml +++ b/Schweizer-Messer/sm_common/package.xml @@ -7,5 +7,6 @@ Paul Furgale New BSD catkin + boost gtest diff --git a/suitesparse/package.xml b/suitesparse/package.xml index b35832c2d..c6ef0c8bb 100644 --- a/suitesparse/package.xml +++ b/suitesparse/package.xml @@ -14,5 +14,5 @@ liblapack-dev libblas-dev gfortran - + wget From 25ddca63182e4655235baf25bf28b92c2696619f Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Tue, 30 Mar 2021 17:54:16 +0100 Subject: [PATCH 28/75] [kalibr] First set of 2to3 conversion --- .../kalibr/python/kalibr_bagcreater | 2 +- .../kalibr/python/kalibr_bagextractor | 18 +- .../kalibr/python/kalibr_calibrate_cameras | 100 ++++++------ .../kalibr/python/kalibr_calibrate_imu_camera | 46 +++--- .../kalibr/python/kalibr_calibrate_rs_cameras | 12 +- .../CameraCalibrator.py | 82 +++++----- .../MulticamGraph.py | 2 +- .../python/kalibr_camera_calibration/ObsDb.py | 22 +-- .../kalibr_camera_calibration/__init__.py | 8 +- .../kalibr/python/kalibr_camera_focus | 8 +- .../kalibr/python/kalibr_camera_validator | 12 +- .../python/kalibr_common/ConfigReader.py | 76 ++++----- .../kalibr_common/ImageDatasetReader.py | 6 +- .../python/kalibr_common/ImuDatasetReader.py | 10 +- .../python/kalibr_common/TargetExtractor.py | 12 +- .../kalibr/python/kalibr_common/__init__.py | 8 +- .../kalibr/python/kalibr_create_target_pdf | 12 +- .../IccCalibrator.py | 32 ++-- .../kalibr_imu_camera_calibration/IccPlots.py | 8 +- .../IccSensors.py | 154 +++++++++--------- .../kalibr_imu_camera_calibration/IccUtil.py | 144 ++++++++-------- .../kalibr_imu_camera_calibration/__init__.py | 8 +- ...ojectionErrorKnotSequenceUpdateStrategy.py | 2 +- .../RsCalibrator.py | 26 +-- .../kalibr_rs_camera_calibration/__init__.py | 2 +- .../python/kalibr_visualize_calibration | 2 +- .../kalibr/python/kalibr_visualize_distortion | 2 +- 27 files changed, 408 insertions(+), 408 deletions(-) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_bagcreater b/aslam_offline_calibration/kalibr/python/kalibr_bagcreater index 6bb0091c4..8598f2d03 100755 --- a/aslam_offline_calibration/kalibr/python/kalibr_bagcreater +++ b/aslam_offline_calibration/kalibr/python/kalibr_bagcreater @@ -1,5 +1,5 @@ #!/usr/bin/env python -print "importing libraries" +print("importing libraries") import rosbag import rospy diff --git a/aslam_offline_calibration/kalibr/python/kalibr_bagextractor b/aslam_offline_calibration/kalibr/python/kalibr_bagextractor index b295e0c02..6c4017bc8 100755 --- a/aslam_offline_calibration/kalibr/python/kalibr_bagextractor +++ b/aslam_offline_calibration/kalibr/python/kalibr_bagextractor @@ -1,5 +1,5 @@ #!/usr/bin/env python -print "importing libraries" +print("importing libraries") import kalibr_common as kc @@ -35,7 +35,7 @@ if len(sys.argv)<2: parsed = parser.parse_args() if parsed.image_topics is None and parsed.imu_topics is None: - print "ERROR: Need at least one camera or IMU topic." + print("ERROR: Need at least one camera or IMU topic.") sys.exit(-1) #create output folder @@ -55,7 +55,7 @@ if parsed.image_topics is not None: numImages = dataset.numImages() #progress bar - print "Extracting {0} images from topic {1}".format(numImages, dataset.topic) + print("Extracting {0} images from topic {1}".format(numImages, dataset.topic)) iProgress.reset(numImages) iProgress.sample() @@ -68,9 +68,9 @@ if parsed.image_topics is not None: iProgress.sample() - print "\r done. " - print -print + print("\r done. ") + print() +print() #extract imu data if parsed.imu_topics is not None: @@ -80,7 +80,7 @@ if parsed.imu_topics is not None: #progress bar numMsg = dataset.numMessages() - print "Extracting {0} IMU messages from topic {1}".format(numMsg, dataset.topic) + print("Extracting {0} IMU messages from topic {1}".format(numMsg, dataset.topic)) iProgress.reset(numMsg) iProgress.sample() @@ -92,5 +92,5 @@ if parsed.imu_topics is not None: timestamp_int = int(timestamp.toSec()*1e9) spamwriter.writerow([timestamp_int, omega[0],omega[1],omega[2], alpha[0],alpha[1],alpha[2] ]) iProgress.sample() - print "\r done. " - print + print("\r done. ") + print() diff --git a/aslam_offline_calibration/kalibr/python/kalibr_calibrate_cameras b/aslam_offline_calibration/kalibr/python/kalibr_calibrate_cameras index aa957e224..3c8a022b6 100755 --- a/aslam_offline_calibration/kalibr/python/kalibr_calibrate_cameras +++ b/aslam_offline_calibration/kalibr/python/kalibr_calibrate_cameras @@ -1,5 +1,5 @@ #!/usr/bin/env python -print "importing libraries" +print("importing libraries") import rosbag import cv_bridge @@ -23,10 +23,10 @@ import signal np.set_printoptions(suppress=True) def initBagDataset(bagfile, topic, from_to): - print "\tDataset: {0}".format(bagfile) - print "\tTopic: {0}".format(topic) + print("\tDataset: {0}".format(bagfile)) + print("\tTopic: {0}".format(topic)) reader = kc.BagImageDatasetReader(bagfile, topic, bag_from_to=from_to) - print "\tNumber of images: {0}".format(reader.numImages()) + print("\tNumber of images: {0}".format(reader.numImages())) return reader #available models @@ -78,7 +78,7 @@ def parseArgs(): tagSpacing: 0.3 #percent of tagSize""" parser = KalibrArgParser(description='Calibrate the intrinsics and extrinsics of a camera system with non-shared overlapping field of view.', usage=usage) - parser.add_argument('--models', nargs='+', dest='models', help='The camera model {0} to estimate'.format(cameraModels.keys()), required=True) + parser.add_argument('--models', nargs='+', dest='models', help='The camera model {0} to estimate'.format(list(cameraModels.keys())), required=True) groupSource = parser.add_argument_group('Data source') groupSource.add_argument('--bag', dest='bagfile', help='The bag file with the data') @@ -160,8 +160,8 @@ def main(): for cam_id in range(0, numCams): topic = parsed.topics[cam_id] modelName = parsed.models[cam_id] - print "Initializing cam{0}:".format(cam_id) - print "\tCamera model:\t {0}".format(modelName) + print("Initializing cam{0}:".format(cam_id)) + print("\tCamera model:\t {0}".format(modelName)) if modelName in cameraModels: #open dataset @@ -185,12 +185,12 @@ def main(): if not cam.initGeometryFromObservations(observations): raise RuntimeError("Could not initialize the intrinsics for camera with topic: {0}. Try to use --verbose and check whether the calibration target extraction is successful.".format(topic)) - print "\tProjection initialized to: %s" % cam.geometry.projection().getParameters().flatten() - print "\tDistortion initialized to: %s" % cam.geometry.projection().distortion().getParameters().flatten() + print("\tProjection initialized to: %s" % cam.geometry.projection().getParameters().flatten()) + print("\tDistortion initialized to: %s" % cam.geometry.projection().distortion().getParameters().flatten()) cameraList.append(cam) else: - raise RuntimeError( "Unknown camera model: {0}. Try {1}.".format(modelName, cameraModels.keys()) ) + raise RuntimeError( "Unknown camera model: {0}. Try {1}.".format(modelName, list(cameraModels.keys())) ) if parsed.verbose: obsdb.printTable() @@ -200,7 +200,7 @@ def main(): if not graph.isGraphConnected(): obsdb.printTable() - print "Cameras are not connected through mutual observations, please check the dataset. Maybe adjust the approx. sync. tolerance." + print("Cameras are not connected through mutual observations, please check the dataset. Maybe adjust the approx. sync. tolerance.") graph.plotGraph() sys.exit(-1) @@ -211,7 +211,7 @@ def main(): while True: try: #compute initial guesses for the baselines, intrinsics - print "initializing initial guesses" + print("initializing initial guesses") if len(cameraList)>1: baseline_guesses = graph.getInitialGuesses(cameraList) else: @@ -221,16 +221,16 @@ def main(): graph.plotGraph() for baseline_idx, baseline in enumerate(baseline_guesses): - print "initialized baseline between cam{0} and cam{1} to:".format(baseline_idx, baseline_idx+1) - print baseline.T() + print("initialized baseline between cam{0} and cam{1} to:".format(baseline_idx, baseline_idx+1)) + print(baseline.T()) for cam_idx, cam in enumerate(cameraList): - print "initialized cam{0} to:".format(cam_idx) - print "\t projection cam{0}: {1}".format(cam_idx, cam.geometry.projection().getParameters().flatten()) - print "\t distortion cam{0}: {1}".format(cam_idx, cam.geometry.projection().distortion().getParameters().flatten()) + print("initialized cam{0} to:".format(cam_idx)) + print("\t projection cam{0}: {1}".format(cam_idx, cam.geometry.projection().getParameters().flatten())) + print("\t distortion cam{0}: {1}".format(cam_idx, cam.geometry.projection().distortion().getParameters().flatten())) - print "initializing calibrator" + print("initializing calibrator") calibrator = kcc.CameraCalibration(cameraList, baseline_guesses, verbose=parsed.verbose, useBlakeZissermanMest=parsed.doBlakeZisserman) options = calibrator.estimator.getOptions() options.infoGainDelta = parsed.miTol @@ -250,7 +250,7 @@ def main(): doPlot = parsed.plot if doPlot: - print "Plotting during calibration. Things may be very slow (but you might learn something)." + print("Plotting during calibration. Things may be very slow (but you might learn something).") #shuffle the views timestamps = obsdb.getAllViewTimestamps() @@ -258,7 +258,7 @@ def main(): random.shuffle(timestamps) #process all target views - print "starting calibration..." + print("starting calibration...") numViews = len(timestamps) progress = sm.Progress2(numViews); progress.sample() for view_id, timestamp in enumerate(timestamps): @@ -274,14 +274,14 @@ def main(): #display process if (verbose or (view_id % 25) == 0) and calibrator.estimator.getNumBatches()>0 and view_id>1: - print - print "------------------------------------------------------------------" - print - print "Processed {0} of {1} views with {2} views used".format(view_id+1, numViews, calibrator.estimator.getNumBatches()) - print + print() + print("------------------------------------------------------------------") + print() + print("Processed {0} of {1} views with {2} views used".format(view_id+1, numViews, calibrator.estimator.getNumBatches())) + print() kcc.printParameters(calibrator) - print - print "------------------------------------------------------------------" + print() + print("------------------------------------------------------------------") #calibration progress progress.sample() @@ -304,16 +304,16 @@ def main(): #create the list of the batches to check if initOutlierRejection: #check all views after the min. number of batches has been reached - batches_to_check=range(0, calibrator.estimator.getNumBatches()) - print;print - print "Filtering outliers in all batches..." + batches_to_check=list(range(0, calibrator.estimator.getNumBatches())) + print();print() + print("Filtering outliers in all batches...") initOutlierRejection=False progress_filter = sm.Progress2(len(batches_to_check)); progress_filter.sample() elif runEndFiltering: #check all batches again after all views have been processed - print;print - print "All views have been processed.\n\nStarting final outlier filtering..." - batches_to_check=range(0, calibrator.estimator.getNumBatches()) + print();print() + print("All views have been processed.\n\nStarting final outlier filtering...") + batches_to_check=list(range(0, calibrator.estimator.getNumBatches())) progress_filter = sm.Progress2(len(batches_to_check)); progress_filter.sample() else: #only check most recent view @@ -326,7 +326,7 @@ def main(): #check all cameras in this batch cornerRemovalList_allCams=list() - camerasInBatch = calibrator.views[batch_id].rerrs.keys() + camerasInBatch = list(calibrator.views[batch_id].rerrs.keys()) for cidx in camerasInBatch: #calculate the reprojection errors statistics @@ -385,33 +385,33 @@ def main(): progress_filter.sample() #final output - print - print - print ".................................................................." - print - print "Calibration complete." - print + print() + print() + print("..................................................................") + print() + print("Calibration complete.") + print() if parsed.removeOutliers: sm.logWarn("Removed {0} outlier corners.".format(len(removedOutlierCorners)) ) - print - print "Processed {0} images with {1} images used".format(numViews, calibrator.estimator.getNumBatches()) + print() + print("Processed {0} images with {1} images used".format(numViews, calibrator.estimator.getNumBatches())) kcc.printParameters(calibrator) if parsed.verbose and len(calibrator.baselines)>1: - f=pl.figure(100006) - kcc.plotCameraRig(calibrator.baselines, fno=f.number, clearFigure=False) - pl.show() + f=pl.figure(100006) + kcc.plotCameraRig(calibrator.baselines, fno=f.number, clearFigure=False) + pl.show() #write to file bagtag = parsed.bagfile.translate(None, "<>:/\|?*").replace('.bag', '', 1) resultFile = "camchain-" + bagtag + ".yaml" kcc.saveChainParametersYaml(calibrator, resultFile, graph) - print "Results written to file: {0}".format(resultFile) + print("Results written to file: {0}".format(resultFile)) #save results to file resultFileTxt = "results-cam-" + bagtag + ".txt" kcc.saveResultTxt(calibrator, filename=resultFileTxt) - print " Detailed results written to file: {0}".format(resultFileTxt) + print(" Detailed results written to file: {0}".format(resultFileTxt)) #generate report reportFile = "report-cam-" + bagtag + ".pdf" @@ -432,13 +432,13 @@ def main(): #reinitialize the intrinsics for cam_id, cam in enumerate(cameraList): - print "Reinitialize the intrinsics for camera {0}".format(cam_id) + print("Reinitialize the intrinsics for camera {0}".format(cam_id)) observations = obsdb.getAllObsCam(cam_id) if not cam.initGeometryFromObservations(observations): raise RuntimeError("Could not re-initialize the intrinsics for camera with topic: {0}".format(topic)) - print "\tProjection initialized to: %s" % cam.geometry.projection().getParameters().flatten() - print "\tDistortion initialized to: %s" % cam.geometry.projection().distortion().getParameters().flatten() + print("\tProjection initialized to: %s" % cam.geometry.projection().getParameters().flatten()) + print("\tDistortion initialized to: %s" % cam.geometry.projection().distortion().getParameters().flatten()) else: break #normal exit diff --git a/aslam_offline_calibration/kalibr/python/kalibr_calibrate_imu_camera b/aslam_offline_calibration/kalibr/python/kalibr_calibrate_imu_camera index 69ae426b8..3417318f8 100755 --- a/aslam_offline_calibration/kalibr/python/kalibr_calibrate_imu_camera +++ b/aslam_offline_calibration/kalibr/python/kalibr_calibrate_imu_camera @@ -1,5 +1,5 @@ #!/usr/bin/env python -print "importing libraries" +print("importing libraries") import sm import kalibr_common as kc from kalibr_imu_camera_calibration import * @@ -13,7 +13,7 @@ import sys np.set_printoptions(suppress=True) def signal_exit(signal, frame): - print + print() sm.logWarn("Shutting down! (CTRL+C)") sys.exit(1) @@ -134,7 +134,7 @@ def main(): signal.signal(signal.SIGINT, signal_exit) - print "Initializing IMUs:" + print("Initializing IMUs:") if len(parsed.imu_yamls) == 1 and not parsed.imu_models: #Maintain backwards compatibility with previous interface. parsed.imu_models = ['calibrated'] @@ -162,10 +162,10 @@ def main(): #load calibration target configuration targetConfig = kc.CalibrationTargetParameters(parsed.target_yaml) - print "Initializing calibration target:" + print("Initializing calibration target:") targetConfig.printDetails() - print "Initializing camera chain:" + print("Initializing camera chain:") chain = kc.CameraChainParameters(parsed.chain_yaml) chain.printDetails() camChain = sens.IccCameraChain(chain, targetConfig, parsed) @@ -180,8 +180,8 @@ def main(): if imu is not imus[0]: imu.findOrientationPrior(imus[0]) - print - print "Building the problem" + print() + print("Building the problem") iCal.buildProblem(splineOrder=6, poseKnotsPerSecond=100, biasKnotsPerSecond=50, @@ -196,41 +196,41 @@ def main(): timeOffsetPadding=parsed.timeoffset_padding, verbose = parsed.verbose) - print - print "Before Optimization" - print "===================" + print() + print("Before Optimization") + print("===================") util.printErrorStatistics(iCal) - print - print "Optimizing..." + print() + print("Optimizing...") iCal.optimize(maxIterations=parsed.max_iter, recoverCov=parsed.recover_cov) - print - print "After Optimization (Results)" - print "==================" + print() + print("After Optimization (Results)") + print("==================") util.printErrorStatistics(iCal) util.printResults(iCal, withCov=parsed.recover_cov) - print + print() bagtag = parsed.bagfile[0].translate(None, "<>:/\|?*").replace('.bag', '', 1) yamlFilename = "camchain-imucam-" + bagtag + ".yaml" iCal.saveCamChainParametersYaml(yamlFilename) - print " Saving camera chain calibration to file: {0}".format(yamlFilename) + print(" Saving camera chain calibration to file: {0}".format(yamlFilename)) - print + print() yamlFilename = "imu-" + bagtag + ".yaml" iCal.saveImuSetParametersYaml(yamlFilename) - print " Saving imu calibration to file: {0}".format(yamlFilename) + print(" Saving imu calibration to file: {0}".format(yamlFilename)) resultFileTxt = "results-imucam-" + bagtag + ".txt" util.saveResultTxt(iCal, filename=resultFileTxt) - print " Detailed results written to file: {0}".format(resultFileTxt) + print(" Detailed results written to file: {0}".format(resultFileTxt)) - print "Generating result report..." + print("Generating result report...") reportFile = "report-imucam-" + bagtag + ".pdf" util.generateReport(iCal, filename=reportFile, showOnScreen=not parsed.dontShowReport) - print " Report written to {0}".format(reportFile) - print + print(" Report written to {0}".format(reportFile)) + print() if __name__ == "__main__": main() diff --git a/aslam_offline_calibration/kalibr/python/kalibr_calibrate_rs_cameras b/aslam_offline_calibration/kalibr/python/kalibr_calibrate_rs_cameras index ccf9aa980..e8fa19156 100755 --- a/aslam_offline_calibration/kalibr/python/kalibr_calibrate_rs_cameras +++ b/aslam_offline_calibration/kalibr/python/kalibr_calibrate_rs_cameras @@ -1,5 +1,5 @@ #!/usr/bin/env python -print "importing libraries" +print("importing libraries") import rosbag import cv_bridge @@ -25,10 +25,10 @@ import signal np.set_printoptions(suppress=True) def __initBagDataset(bagfile, topic, from_to): - print "\tDataset: {0}".format(bagfile) - print "\tTopic: {0}".format(topic) + print("\tDataset: {0}".format(bagfile)) + print("\tTopic: {0}".format(topic)) reader = kc.BagImageDatasetReader(bagfile, topic, bag_from_to=from_to) - print "\tNumber of images: {0}".format(reader.numImages()) + print("\tNumber of images: {0}".format(reader.numImages())) return reader #available models @@ -84,7 +84,7 @@ def __parseArgs(): parser = KalibrArgParser(description='Calibrate the intrinsics of a single rolling shutter camera.', usage=usage) parser.add_argument('--model', dest='model', help='The camera model to estimate. ' - 'Currently supported models are {0}, where the suffix \'-rs\' identifies rolling shutter models.'.format(', '.join(cameraModels.keys())), required=True) + 'Currently supported models are {0}, where the suffix \'-rs\' identifies rolling shutter models.'.format(', '.join(list(cameraModels.keys()))), required=True) parser.add_argument('--frame-rate', dest='framerate', type=int, help='Approximate framerate of the camera.', required=True) groupSource = parser.add_argument_group('Data source') @@ -177,7 +177,7 @@ def main(): if __name__ == "__main__": try: main() - except Exception,e: + except Exception as e: sm.logError("Exception: {0}".format(e)) sys.exit(-1) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/CameraCalibrator.py b/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/CameraCalibrator.py index cba2c186c..64e3f0652 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/CameraCalibrator.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/CameraCalibrator.py @@ -314,7 +314,7 @@ def getPointStatistics(cself,view_id,cam_id,pidx): z = np.array([0,0,1]) dz = np.dot(z,vc) if np.abs(dz - 1.0) > 1e-3: - print("The point statistics are only valid if the camera points down the z axis. This camera has the image center: [%f, %f, %f]" % (z[0],z[1],z[2])) + print(("The point statistics are only valid if the camera points down the z axis. This camera has the image center: [%f, %f, %f]" % (z[0],z[1],z[2]))) valid, y = obs.imagePoint(pidx) geometry = cself.cameras[cam_id].geometry rerr = None @@ -385,7 +385,7 @@ def getReprojectionErrors(cself, cam_id): gc.disable() #append speed up for view_id, view in enumerate(cself.views): - if cam_id in view.rerrs.keys(): + if cam_id in list(view.rerrs.keys()): view_corners=list(); view_reprojections=list(); view_reprojection_errs=list() for rerr in view.rerrs[cam_id]: #add if the corners was observed @@ -583,77 +583,77 @@ def printParameters(cself, dest=sys.stdout): std_baselines, std_cameras = recoverCovariance(cself) #print cameras - print >> dest, "Camera-system parameters:" + print("Camera-system parameters:", file=dest) for cidx, cam in enumerate(cself.cameras): d = cam.geometry.projection().distortion().getParameters().flatten(1) p = cam.geometry.projection().getParameters().flatten(1) dd = std_cameras[cidx][0:d.shape[0]] dp = std_cameras[cidx][d.shape[0]:] - print >> dest, "\tcam{0} ({1}):".format(cidx, cam.dataset.topic) - print >> dest, "\t type: %s" % ( type(cam.geometry) ) - print >> dest, "\t distortion: %s +- %s" % (d, np.array(dd)) - print >> dest, "\t projection: %s +- %s" % (p, np.array(dp)) + print("\tcam{0} ({1}):".format(cidx, cam.dataset.topic), file=dest) + print("\t type: %s" % ( type(cam.geometry) ), file=dest) + print("\t distortion: %s +- %s" % (d, np.array(dd)), file=dest) + print("\t projection: %s +- %s" % (p, np.array(dp)), file=dest) #reproj error statistics corners, reprojs, rerrs = getReprojectionErrors(cself, cidx) if len(rerrs)>0: me, se = getReprojectionErrorStatistics(rerrs) try: - print >> dest, "\t reprojection error: [%f, %f] +- [%f, %f]" % (me[0], me[1], se[0], se[1]) + print("\t reprojection error: [%f, %f] +- [%f, %f]" % (me[0], me[1], se[0], se[1]), file=dest) except: - print >> dest, "\t Failed printing the reprojection error." - print >> dest + print("\t Failed printing the reprojection error.", file=dest) + print(file=dest) #print baselines for bidx, baseline in enumerate(cself.baselines): T = sm.Transformation( baseline.T() ) dq = std_baselines[bidx][0:3] dt = std_baselines[bidx][3:6] - print >> dest, "\tbaseline T_{1}_{0}:".format(bidx, bidx+1) - print >> dest, "\t q: %s +- %s" % (T.q(), np.array(dq)) - print >> dest, "\t t: %s +- %s" % (T.t(), np.array(dt)) - print >> dest + print("\tbaseline T_{1}_{0}:".format(bidx, bidx+1), file=dest) + print("\t q: %s +- %s" % (T.q(), np.array(dq)), file=dest) + print("\t t: %s +- %s" % (T.t(), np.array(dt)), file=dest) + print(file=dest) def printDebugEnd(cself): - print - print + print() + print() for cidx, cam in enumerate(cself.cameras): - print "cam{0}".format(cidx) - print "----------" - print - print + print("cam{0}".format(cidx)) + print("----------") + print() + print() corners, reprojs, rerrs = getReprojectionErrors(cself, cidx) if len(rerrs)>0: me, se = getReprojectionErrorStatistics(rerrs) - print me[0] - print me[1] - print se[0] - print se[1] + print(me[0]) + print(me[1]) + print(se[0]) + print(se[1]) - print + print() p = cam.geometry.projection().getParameters().flatten(1) for temp in p: - print temp + print(temp) - print + print() d = cam.geometry.projection().distortion().getParameters().flatten(1) for temp in d: - print temp + print(temp) if cidx>0: bidx=cidx-1 T = sm.Transformation( cself.baselines[bidx].T() ) for temp in T.t(): - print temp + print(temp) for temp in T.q(): - print temp + print(temp) - print - print + print() + print() def saveChainParametersYaml(cself, resultFile, graph): cameraModels = {acvb.DistortedPinhole: 'pinhole', @@ -795,7 +795,7 @@ def generateReport(cself, filename="report.pdf", showOnScreen=True, graph=None, for fig in figs: pdf.savefig(fig) pdf.close() - print "Report written to: {0}".format(filename) + print("Report written to: {0}".format(filename)) if showOnScreen: plotter.show() @@ -821,7 +821,7 @@ def plotCorners(gridobs, fno=1, cornerlist=None, clearFigure=True, plotImage=Tru if valid: P.append(y) else: - print "Tried to plot unobserved corner" + print("Tried to plot unobserved corner") P=np.array(P) else: #get all points @@ -870,15 +870,15 @@ def plotCameraRig(baselines, fno=1, clearFigure=True, title=""): def saveResultTxt(cself, filename="camera_calibration_result.txt"): f1=open(filename, 'w') - print >> f1, "Calibration results " - print >> f1, "====================" + print("Calibration results ", file=f1) + print("====================", file=f1) printParameters(cself, f1) - print >> f1, "" - print >> f1, "" - print >> f1, "Target configuration" - print >> f1, "====================" - print >> f1, "" + print("", file=f1) + print("", file=f1) + print("Target configuration", file=f1) + print("====================", file=f1) + print("", file=f1) cself.cameras[0].ctarget.targetConfig.printDetails(f1) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/MulticamGraph.py b/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/MulticamGraph.py index d70e665ff..041a33d61 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/MulticamGraph.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/MulticamGraph.py @@ -161,7 +161,7 @@ def getInitialGuesses(self, cameras): camL_nr = vertices[1] camH_nr = vertices[0] - print "\t initializing camera pair ({0},{1})... ".format(camL_nr, camH_nr) + print(("\t initializing camera pair ({0},{1})... ".format(camL_nr, camH_nr))) #run the pair extrinsic calibration obs_list = self.obs_db.getAllObsTwoCams(camL_nr, camH_nr) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/ObsDb.py b/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/ObsDb.py index 7f0f83637..0fdd06dd2 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/ObsDb.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/ObsDb.py @@ -38,7 +38,7 @@ def addObservation(self, cam_id, obs): obs_idx = len(self.observations[cam_id])-1 #find the nearest timestamp in the table - timestamps_table = self.targetViews.keys() + timestamps_table = list(self.targetViews.keys()) timestamp_obs = obs.time().toSec() #check if the table is still empty (initialization) @@ -83,15 +83,15 @@ def getAllObsAtTimestamp(self, timestamp): #get the number of cameras that see at least one target def numCameras(self): - return len(self.observations.keys()) + return len(list(self.observations.keys())) #get a list of all timestamps at which we see a target def getAllViewTimestamps(self): - return self.targetViews.keys() + return list(self.targetViews.keys()) #get a list of all cam_ids that see the target at the given time def getCamIdsAtTimestamp(self, timestamp): - return self.targetViews[timestamp].keys() + return list(self.targetViews[timestamp].keys()) #get the observation for given cam_id and timestamp def getObservationAtTime(self, timestamp, cam_id): @@ -143,22 +143,22 @@ def getAllObsCam(self, cam_id): ############################################################# def printTable(self): #header - print "timestamp \t", + print("timestamp \t", end=' ') for cam_id in range(0, self.numCameras()): - print "cam{0} \t".format(cam_id), - print + print("cam{0} \t".format(cam_id), end=' ') + print() #sort for time - times_sorted = np.sort(self.targetViews.keys()) + times_sorted = np.sort(list(self.targetViews.keys())) #data lines for time in times_sorted: - print time, + print(time, end=' ') for cam_id in range(0, self.numCameras()): try: numCorners = len(self.targetViews[time][cam_id]['observed_corners']) except KeyError: numCorners = "-" - print "\t", numCorners, - print + print("\t", numCorners, end=' ') + print() diff --git a/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/__init__.py b/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/__init__.py index 69f754904..ac58863bf 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/__init__.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/__init__.py @@ -1,6 +1,6 @@ # Import the numpy to Eigen type conversion. import numpy_eigen -from CameraCalibrator import * -from ObsDb import * -from MulticamGraph import * -from CameraIntializers import * \ No newline at end of file +from .CameraCalibrator import * +from .ObsDb import * +from .MulticamGraph import * +from .CameraIntializers import * \ No newline at end of file diff --git a/aslam_offline_calibration/kalibr/python/kalibr_camera_focus b/aslam_offline_calibration/kalibr/python/kalibr_camera_focus index 626349ea0..bf6895ca3 100755 --- a/aslam_offline_calibration/kalibr/python/kalibr_camera_focus +++ b/aslam_offline_calibration/kalibr/python/kalibr_camera_focus @@ -1,5 +1,5 @@ #!/usr/bin/env python -print "importing libraries" +print("importing libraries") import rospy import cv2 import cv @@ -21,8 +21,8 @@ class CameraFocus: try: cv_image = self.bridge.imgmsg_to_cv2(msg) np_image= np.array(cv_image) - except CvBridgeError, e: - print "Could not convert ros message to opencv image: ", e + except CvBridgeError as e: + print("Could not convert ros message to opencv image: ", e) return #calculate the fft magnitude @@ -56,6 +56,6 @@ if __name__ == "__main__": try: rospy.spin() except KeyboardInterrupt: - print "Shutting down" + print("Shutting down") cv.DestroyAllWindows() diff --git a/aslam_offline_calibration/kalibr/python/kalibr_camera_validator b/aslam_offline_calibration/kalibr/python/kalibr_camera_validator index 759154eee..bb493b7ef 100755 --- a/aslam_offline_calibration/kalibr/python/kalibr_camera_validator +++ b/aslam_offline_calibration/kalibr/python/kalibr_camera_validator @@ -125,8 +125,8 @@ class CameraChainValidator(object): else: cv_image = self.bridge.imgmsg_to_cv2(msg) np_image = np.array(cv_image) - except CvBridgeError, e: - print e + except CvBridgeError as e: + print(e) #get the corresponding monovalidator instance validator = self.monovalidators[cam_nr] @@ -368,12 +368,12 @@ class CameraChainValidator(object): class MonoCameraValidator(object): def __init__(self, camConfig, targetParams): - print "initializing camera geometry" + print("initializing camera geometry") self.camera = kc.ConfigReader.AslamCamera.fromParameters(camConfig) self.target = CalibrationTargetDetector(self.camera, targetConfig) #print details - print "Camera {0}:".format(camConfig.getRosTopic()) + print("Camera {0}:".format(camConfig.getRosTopic())) camConfig.printDetails(); self.topic = camConfig.getRosTopic() @@ -472,7 +472,7 @@ if __name__ == "__main__": #load the configuration yamls targetConfig = kc.ConfigReader.CalibrationTargetParameters(parsed.targetYaml) - print "Initializing calibration target:".format(targetConfig.getTargetType()) + print("Initializing calibration target:".format(targetConfig.getTargetType())) targetConfig.printDetails() camchain = kc.ConfigReader.CameraChainParameters(parsed.chainYaml) @@ -484,6 +484,6 @@ if __name__ == "__main__": try: rospy.spin() except KeyboardInterrupt: - print "Shutting down" + print("Shutting down") cv2.destroyAllWindows() diff --git a/aslam_offline_calibration/kalibr/python/kalibr_common/ConfigReader.py b/aslam_offline_calibration/kalibr/python/kalibr_common/ConfigReader.py index 5d764ec71..89b31de95 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_common/ConfigReader.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_common/ConfigReader.py @@ -343,7 +343,7 @@ def checkDistortion(self, model, coeffs): if model not in distortionModelsAndNumParams: self.raiseError("Unknown distortion model '{}'. Supported models: {}. )".format( - model, ", ".join(distortionModelsAndNumParams.keys()))) + model, ", ".join(list(distortionModelsAndNumParams.keys())))) if len(coeffs) != distortionModelsAndNumParams[model]: self.raiseError("distortion model '{}' requires {} coefficients; {} given".format( @@ -405,19 +405,19 @@ def printDetails(self, dest=sys.stdout): else: self.raiseError("Unknown camera model '{}'.".format(camera_model)) - print >> dest, " Camera model: {0}".format(camera_model) - print >> dest, " Focal length: {0}".format(focalLength) - print >> dest, " Principal point: {0}".format(principalPoint) + print(" Camera model: {0}".format(camera_model), file=dest) + print(" Focal length: {0}".format(focalLength), file=dest) + print(" Principal point: {0}".format(principalPoint), file=dest) if camera_model == 'omni': - print >> dest, " Omni xi: {0}".format(xi_omni) + print(" Omni xi: {0}".format(xi_omni), file=dest) if camera_model == 'eucm': - print >> dest, " EUCM alpha: {0}".format(alpha_eucm) - print >> dest, " EUCM beta: {0}".format(beta_eucm) + print(" EUCM alpha: {0}".format(alpha_eucm), file=dest) + print(" EUCM beta: {0}".format(beta_eucm), file=dest) if camera_model == 'ds': - print >> dest, " DS xi: {0}".format(xi_ds) - print >> dest, " DS alpha: {0}".format(alpha_ds) - print >> dest, " Distortion model: {0}".format(dist_model) - print >> dest, " Distortion coefficients: {0}".format(dist_coeff) + print(" DS xi: {0}".format(xi_ds), file=dest) + print(" DS alpha: {0}".format(alpha_ds), file=dest) + print(" Distortion model: {0}".format(dist_model), file=dest) + print(" Distortion coefficients: {0}".format(dist_coeff), file=dest) @@ -502,15 +502,15 @@ def printDetails(self, dest=sys.stdout): accelUncertaintyDiscrete, accelRandomWalk, accelUncertainty = self.getAccelerometerStatistics() gyroUncertaintyDiscrete, gyroRandomWalk, gyroUncertainty = self.getGyroStatistics() - print >> dest, " Update rate: {0}".format(update_rate) - print >> dest, " Accelerometer:" - print >> dest, " Noise density: {0} ".format(accelUncertainty) - print >> dest, " Noise density (discrete): {0} ".format(accelUncertaintyDiscrete) - print >> dest, " Random walk: {0}".format(accelRandomWalk) - print >> dest, " Gyroscope:" - print >> dest, " Noise density: {0}".format(gyroUncertainty) - print >> dest, " Noise density (discrete): {0} ".format(gyroUncertaintyDiscrete) - print >> dest, " Random walk: {0}".format(gyroRandomWalk) + print(" Update rate: {0}".format(update_rate), file=dest) + print(" Accelerometer:", file=dest) + print(" Noise density: {0} ".format(accelUncertainty), file=dest) + print(" Noise density (discrete): {0} ".format(accelUncertaintyDiscrete), file=dest) + print(" Random walk: {0}".format(accelRandomWalk), file=dest) + print(" Gyroscope:", file=dest) + print(" Noise density: {0}".format(gyroUncertainty), file=dest) + print(" Noise density (discrete): {0} ".format(gyroUncertaintyDiscrete), file=dest) + print(" Random walk: {0}".format(gyroRandomWalk), file=dest) class ImuSetParameters(ParametersBase): def __init__(self, yamlFile, createYaml=False): @@ -555,7 +555,7 @@ def getTargetParams(self): targetCols = self.data["targetCols"] rowSpacingMeters = self.data["rowSpacingMeters"] colSpacingMeters = self.data["colSpacingMeters"] - except KeyError, e: + except KeyError as e: self.raiseError("Calibration target configuration in {0} is missing the field: {1}".format(self.yamlFile, str(e)) ) if not isinstance(targetRows,int) or targetRows < 3: @@ -579,7 +579,7 @@ def getTargetParams(self): targetCols = self.data["targetCols"] spacingMeters = self.data["spacingMeters"] asymmetricGrid = self.data["asymmetricGrid"] - except KeyError, e: + except KeyError as e: self.raiseError("Calibration target configuration in {0} is missing the field: {1}".format(self.yamlFile, str(e)) ) if not isinstance(targetRows,int) or targetRows < 3: @@ -603,7 +603,7 @@ def getTargetParams(self): tagCols = self.data["tagCols"] tagSize = self.data["tagSize"] tagSpacing = self.data["tagSpacing"] - except KeyError, e: + except KeyError as e: self.raiseError("Calibration target configuration in {0} is missing the field: {1}".format(self.yamlFile, str(e)) ) if not isinstance(tagRows,int) or tagRows < 3: @@ -631,21 +631,21 @@ def printDetails(self, dest=sys.stdout): targetType = self.getTargetType() targetParams = self.getTargetParams() - print >> dest, " Type: {0}".format(targetType) + print(" Type: {0}".format(targetType), file=dest) if targetType == 'checkerboard': - print >> dest, " Rows" - print >> dest, " Count: {0}".format(targetParams['targetRows']) - print >> dest, " Distance: {0} [m]".format(targetParams['rowSpacingMeters']) - print >> dest, " Cols" - print >> dest, " Count: {0}".format(targetParams['targetCols']) - print >> dest, " Distance: {0} [m]".format(targetParams['colSpacingMeters']) + print(" Rows", file=dest) + print(" Count: {0}".format(targetParams['targetRows']), file=dest) + print(" Distance: {0} [m]".format(targetParams['rowSpacingMeters']), file=dest) + print(" Cols", file=dest) + print(" Count: {0}".format(targetParams['targetCols']), file=dest) + print(" Distance: {0} [m]".format(targetParams['colSpacingMeters']), file=dest) elif targetType == 'aprilgrid': - print >> dest, " Tags: " - print >> dest, " Rows: {0}".format(targetParams['tagRows']) - print >> dest, " Cols: {0}".format(targetParams['tagCols']) - print >> dest, " Size: {0} [m]".format(targetParams['tagSize']) - print >> dest, " Spacing {0} [m]".format( targetParams['tagSize']*targetParams['tagSpacing'] ) + print(" Tags: ", file=dest) + print(" Rows: {0}".format(targetParams['tagRows']), file=dest) + print(" Cols: {0}".format(targetParams['tagCols']), file=dest) + print(" Size: {0} [m]".format(targetParams['tagSize']), file=dest) + print(" Spacing {0} [m]".format( targetParams['tagSize']*targetParams['tagSpacing'] ), file=dest) @@ -751,14 +751,14 @@ def numCameras(self): def printDetails(self, dest=sys.stdout): for camNr in range(0, self.numCameras()): - print "Camera chain - cam{0}:".format(camNr) + print("Camera chain - cam{0}:".format(camNr)) camConfig = self.getCameraParameters(camNr) camConfig.printDetails(dest) #print baseline if available try: T = self.getExtrinsicsLastCamToHere(camNr) - print >> dest, " baseline:", T.T() + print(" baseline:", T.T(), file=dest) except: - print >> dest, " baseline: no data available" + print(" baseline: no data available", file=dest) pass diff --git a/aslam_offline_calibration/kalibr/python/kalibr_common/ImageDatasetReader.py b/aslam_offline_calibration/kalibr/python/kalibr_common/ImageDatasetReader.py index a9037bfd2..13688ba29 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_common/ImageDatasetReader.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_common/ImageDatasetReader.py @@ -20,8 +20,8 @@ def __init__(self, dataset, indices=None): def __iter__(self): return self - def next(self): - idx = self.iter.next() + def __next__(self): + idx = next(self.iter) return self.dataset.getImage(idx) @@ -42,7 +42,7 @@ def __init__(self, bagfile, imagetopic, bag_from_to=None, perform_synchronizatio indices = self.bag._get_indexes(conx) try: - self.index = indices.next() + self.index = next(indices) except: raise RuntimeError("Could not find topic {0} in {1}.".format(imagetopic, self.bagfile)) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_common/ImuDatasetReader.py b/aslam_offline_calibration/kalibr/python/kalibr_common/ImuDatasetReader.py index 49eb201cd..61fb0af5b 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_common/ImuDatasetReader.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_common/ImuDatasetReader.py @@ -15,8 +15,8 @@ def __init__(self,dataset,indices=None): self.iter = self.indices.__iter__() def __iter__(self): return self - def next(self): - idx = self.iter.next() + def __next__(self): + idx = next(self.iter) return self.dataset.getMessage(idx) class BagImuDatasetReader(object): @@ -34,7 +34,7 @@ def __init__(self, bagfile, imutopic, bag_from_to=None, perform_synchronization= indices = self.bag._get_indexes(conx) try: - self.index = indices.next() + self.index = next(indices) except: raise RuntimeError("Could not find topic {0} in {1}.".format(imutopic, self.bagfile)) @@ -73,8 +73,8 @@ def truncateIndicesFromTime(self, indices, bag_from_to): bagstart = min(timestamps) baglength = max(timestamps)-bagstart - print "bagstart",bagstart - print "baglength",baglength + print(("bagstart",bagstart)) + print(("baglength",baglength)) #some value checking if bag_from_to[0]>=bag_from_to[1]: raise RuntimeError("Bag start time must be bigger than end time.".format(bag_from_to[0])) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_common/TargetExtractor.py b/aslam_offline_calibration/kalibr/python/kalibr_common/TargetExtractor.py index 2b7ca395d..5be4ad9f9 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_common/TargetExtractor.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_common/TargetExtractor.py @@ -3,7 +3,7 @@ import numpy as np import sys import multiprocessing -import Queue +import queue import time import copy import cv2 @@ -12,7 +12,7 @@ def multicoreExtractionWrapper(detector, taskq, resultq, clearImages, noTransfor while 1: try: task = taskq.get_nowait() - except Queue.Empty: + except queue.Empty: return idx = task[0] stamp = task[1] @@ -29,7 +29,7 @@ def multicoreExtractionWrapper(detector, taskq, resultq, clearImages, noTransfor resultq.put( (obs, idx) ) def extractCornersFromDataset(dataset, detector, multithreading=False, numProcesses=None, clearImages=True, noTransformation=False): - print "Extracting calibration target corners" + print("Extracting calibration target corners") targetObservations = [] numImages = dataset.numImages() @@ -69,7 +69,7 @@ def extractCornersFromDataset(dataset, detector, multithreading=False, numProces last_done = done time.sleep(0.5) resultq.put('STOP') - except Exception, e: + except Exception as e: raise RuntimeError("Exception during multithreaded extraction: {0}".format(e)) #get result sorted by time (=idx) @@ -96,10 +96,10 @@ def extractCornersFromDataset(dataset, detector, multithreading=False, numProces iProgress.sample() if len(targetObservations) == 0: - print "\r" + print("\r") sm.logFatal("No corners could be extracted for camera {0}! Check the calibration target configuration and dataset.".format(dataset.topic)) else: - print "\r Extracted corners for %d images (of %d images) " % (len(targetObservations), numImages) + print(("\r Extracted corners for %d images (of %d images) " % (len(targetObservations), numImages))) #close all opencv windows that might be open cv2.destroyAllWindows() diff --git a/aslam_offline_calibration/kalibr/python/kalibr_common/__init__.py b/aslam_offline_calibration/kalibr/python/kalibr_common/__init__.py index df660f7d3..b95209320 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_common/__init__.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_common/__init__.py @@ -1,6 +1,6 @@ # Import the numpy to Eigen type conversion. import numpy_eigen -from ConfigReader import * -from ImageDatasetReader import * -from ImuDatasetReader import * -from TargetExtractor import * +from .ConfigReader import * +from .ImageDatasetReader import * +from .ImuDatasetReader import * +from .TargetExtractor import * diff --git a/aslam_offline_calibration/kalibr/python/kalibr_create_target_pdf b/aslam_offline_calibration/kalibr/python/kalibr_create_target_pdf index 0378adadd..27e31dcc0 100755 --- a/aslam_offline_calibration/kalibr/python/kalibr_create_target_pdf +++ b/aslam_offline_calibration/kalibr/python/kalibr_create_target_pdf @@ -28,7 +28,7 @@ class AprilTagCodes: self.tagCodes = AprilTagCodes.TagFamilies[chosenTagFamiliy][0] self.totalBits = AprilTagCodes.TagFamilies[chosenTagFamiliy][1] except: - print "[ERROR]: Unknown tag familiy." + print("[ERROR]: Unknown tag familiy.") sys.exit(0) #borderBits must be consitent with the variable "blackBorder" in the detector code in file ethz_apriltag2/src/TagFamily.cc @@ -37,7 +37,7 @@ def generateAprilTag(canvas, position, metricSize, tagSpacing, tagID, tagFamilil try: tagCode=tagFamililyData.tagCodes[tagID] except: - print "[ERROR]: Requested tag ID of {0} not available in the {1} TagFamiliy".format(tagID, tagFamililyData.chosenTagFamiliy) + print("[ERROR]: Requested tag ID of {0} not available in the {1} TagFamiliy".format(tagID, tagFamililyData.chosenTagFamiliy)) #calculate the bit size of the tag sqrtBits = (math.sqrt(tagFamililyData.totalBits)) @@ -89,7 +89,7 @@ def generateAprilTag(canvas, position, metricSize, tagSpacing, tagID, tagFamilil def generateAprilBoard(canvas, n_cols, n_rows, tagSize, tagSpacing=0.25, tagFamilily="t36h11", skipIds=None): if(tagSpacing<0 or tagSpacing>1.0): - print "[ERROR]: Invalid tagSpacing specified. [0-1.0] of tagSize" + print("[ERROR]: Invalid tagSpacing specified. [0-1.0] of tagSize") sys.exit(0) if skipIds is None: @@ -138,7 +138,7 @@ def generateCheckerboard(canvas, n_cols, n_rows, size_cols, size_rows): size_rows = size_rows*100.0 #message - print "Generating a checkerboard with {0}x{1} corners and a box size of {2}x{3} cm".format(n_cols,n_rows,size_cols,size_rows) + print("Generating a checkerboard with {0}x{1} corners and a box size of {2}x{3} cm".format(n_cols,n_rows,size_cols,size_rows)) #draw boxes for x in range(0,n_cols+1): @@ -178,7 +178,7 @@ if __name__ == "__main__": aprilOptions = parser.add_argument_group('Apriltag arguments') aprilOptions.add_argument('--tsize', type=float, default=0.08, dest='tsize', help='The size of one tag [m] (default: %(default)s)') aprilOptions.add_argument('--tspace', type=float, default=0.3, dest='tagspacing', help='The space between the tags in fraction of the edge size [0..1] (default: %(default)s)') - aprilOptions.add_argument('--tfam', default='t36h11', dest='tagfamiliy', help='Familiy of April tags {0} (default: %(default)s)'.format(AprilTagCodes.TagFamilies.keys())) + aprilOptions.add_argument('--tfam', default='t36h11', dest='tagfamiliy', help='Familiy of April tags {0} (default: %(default)s)'.format(list(AprilTagCodes.TagFamilies.keys()))) aprilOptions.add_argument('--skip-ids', default='', dest='skipIds', help='Space-separated list of tag ids to leave blank (default: none)') checkerOptions = parser.add_argument_group('Checkerboard arguments') @@ -207,7 +207,7 @@ if __name__ == "__main__": elif parsed.gridType == "checkerboard": generateCheckerboard(c, parsed.n_cols, parsed.n_rows, parsed.chessSzX, parsed.chessSzY) else: - print "[ERROR]: Unknown grid pattern" + print("[ERROR]: Unknown grid pattern") sys.exit(0) #write to file diff --git a/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccCalibrator.py b/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccCalibrator.py index ac8d10984..6387b40e0 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccCalibrator.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccCalibrator.py @@ -1,6 +1,6 @@ import aslam_backend as aopt import aslam_splines as asp -import IccUtil as util +from . import IccUtil as util import incremental_calibration as inc import kalibr_common as kc import sm @@ -80,19 +80,19 @@ def buildProblem( self, timeOffsetPadding=0.02, verbose=False ): - print "\tSpline order: %d" % (splineOrder) - print "\tPose knots per second: %d" % (poseKnotsPerSecond) - print "\tDo pose motion regularization: %s" % (doPoseMotionError) - print "\t\txddot translation variance: %f" % (mrTranslationVariance) - print "\t\txddot rotation variance: %f" % (mrRotationVariance) - print "\tBias knots per second: %d" % (biasKnotsPerSecond) - print "\tDo bias motion regularization: %s" % (doBiasMotionError) - print "\tBlake-Zisserman on reprojection errors %s" % blakeZisserCam - print "\tAcceleration Huber width (sigma): %f" % (huberAccel) - print "\tGyroscope Huber width (sigma): %f" % (huberGyro) - print "\tDo time calibration: %s" % (not noTimeCalibration) - print "\tMax iterations: %d" % (maxIterations) - print "\tTime offset padding: %f" % (timeOffsetPadding) + print(("\tSpline order: %d" % (splineOrder))) + print(("\tPose knots per second: %d" % (poseKnotsPerSecond))) + print(("\tDo pose motion regularization: %s" % (doPoseMotionError))) + print(("\t\txddot translation variance: %f" % (mrTranslationVariance))) + print(("\t\txddot rotation variance: %f" % (mrRotationVariance))) + print(("\tBias knots per second: %d" % (biasKnotsPerSecond))) + print(("\tDo bias motion regularization: %s" % (doBiasMotionError))) + print(("\tBlake-Zisserman on reprojection errors %s" % blakeZisserCam)) + print(("\tAcceleration Huber width (sigma): %f" % (huberAccel))) + print(("\tGyroscope Huber width (sigma): %f" % (huberGyro))) + print(("\tDo time calibration: %s" % (not noTimeCalibration))) + print(("\tMax iterations: %d" % (maxIterations))) + print(("\tTime offset padding: %f" % (timeOffsetPadding))) ############################################ @@ -191,7 +191,7 @@ def recoverCovariance(self): # 1. transformation imu-cam0 --> 6 # 2. camera time2imu --> 1*numCams (only if enabled) - print "Recovering covariance..." + print("Recovering covariance...") estimator = inc.IncrementalEstimator(CALIBRATION_GROUP_ID) rval = estimator.addBatch(self.problem, True) est_stds = np.sqrt(estimator.getSigma2Theta().diagonal()) @@ -230,5 +230,5 @@ def saveCamChainParametersYaml(self, resultFile): try: chain.writeYaml(resultFile) except: - print "ERROR: Could not write parameters to file: {0}\n".format(resultFile) + print(("ERROR: Could not write parameters to file: {0}\n".format(resultFile))) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccPlots.py b/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccPlots.py index e1a94ea71..dc23d4d13 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccPlots.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccPlots.py @@ -33,7 +33,7 @@ def plotGyroErrorPerAxis(cself, iidx, fno=1, clearFigure=True, noShow=False): f.clf() f.suptitle("imu{0}: angular velocities error".format(iidx)) - for i in xrange(3): + for i in range(3): pl.subplot(3, 1, i+1) pl.plot(errors[:,i]) pl.xlabel('error index') @@ -76,7 +76,7 @@ def plotAccelErrorPerAxis(cself, iidx, fno=1, clearFigure=True, noShow=False): f.clf() f.suptitle("imu{0}: acceleration error".format(iidx)) - for i in xrange(3): + for i in range(3): pl.subplot(3, 1, i+1) pl.plot(errors[:,i]) pl.xlabel('error index') @@ -102,7 +102,7 @@ def plotAccelBias(cself, imu_idx, fno=1, clearFigure=True, noShow=False): sigma_rw = cself.ImuList[imu_idx].getImuConfig().getAccelerometerStatistics()[1] bounds = 3. * sigma_rw * np.sqrt(times) - for i in xrange(3): + for i in range(3): pl.subplot(3, 1, i+1) pl.plot(times, acc_bias_spline[i,0] + bounds, 'r--') pl.plot(times, acc_bias_spline[i,0] - bounds, 'r--') @@ -122,7 +122,7 @@ def plotAngularVelocityBias(cself, imu_idx, fno=1, clearFigure=True, noShow=Fals sigma_rw = cself.ImuList[imu_idx].getImuConfig().getGyroStatistics()[1] bounds = 3. * sigma_rw * np.sqrt(times) - for i in xrange(3): + for i in range(3): pl.subplot(3, 1, i+1) pl.plot(times, gyro_bias_spline[i,0] + bounds, 'r--') pl.plot(times, gyro_bias_spline[i,0] - bounds, 'r--') diff --git a/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccSensors.py b/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccSensors.py index c81d77fff..8a1aab571 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccSensors.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccSensors.py @@ -6,7 +6,7 @@ import bsplines import kalibr_common as kc import kalibr_errorterms as ket -import IccCalibrator as ic +from . import IccCalibrator as ic import cv2 import sys @@ -17,21 +17,21 @@ def initCameraBagDataset(bagfile, topic, from_to=None, perform_synchronization=False): - print "Initializing camera rosbag dataset reader:" - print "\tDataset: {0}".format(bagfile) - print "\tTopic: {0}".format(topic) + print("Initializing camera rosbag dataset reader:") + print("\tDataset: {0}".format(bagfile)) + print("\tTopic: {0}".format(topic)) reader = kc.BagImageDatasetReader(bagfile, topic, bag_from_to=from_to, \ perform_synchronization=perform_synchronization) - print "\tNumber of images: {0}".format(len(reader.index)) + print("\tNumber of images: {0}".format(len(reader.index))) return reader def initImuBagDataset(bagfile, topic, from_to=None, perform_synchronization=False): - print "Initializing imu rosbag dataset reader:" - print "\tDataset: {0}".format(bagfile) - print "\tTopic: {0}".format(topic) + print("Initializing imu rosbag dataset reader:") + print("\tDataset: {0}".format(bagfile)) + print("\tTopic: {0}".format(topic)) reader = kc.BagImuDatasetReader(bagfile, topic, bag_from_to=from_to, \ perform_synchronization=perform_synchronization) - print "\tNumber of messages: {0}".format(len(reader.index)) + print("\tNumber of messages: {0}".format(len(reader.index))) return reader @@ -114,8 +114,8 @@ def setupCalibrationTarget(self, targetConfig, showExtraction=False, showReproj= self.detector = acv.GridDetector(self.camera.geometry, grid, options) def findOrientationPriorCameraToImu(self, imu): - print - print "Estimating imu-camera rotation prior" + print() + print("Estimating imu-camera rotation prior") # build the problem problem = aopt.OptimizationProblem() @@ -186,7 +186,7 @@ def findOrientationPriorCameraToImu(self, imu): a_w.append(np.dot(poseSpline.orientation(tk), np.dot(R_i_c, - im.alpha))) mean_a_w = np.mean(np.asarray(a_w).T, axis=1) self.gravity_w = mean_a_w / np.linalg.norm(mean_a_w) * 9.80655 - print "Gravity was intialized to", self.gravity_w, "[m/s^2]" + print("Gravity was intialized to", self.gravity_w, "[m/s^2]") #set the gyro bias prior (if we have more than 1 cameras use recursive average) b_gyro = bias.toEuclidean() @@ -194,10 +194,10 @@ def findOrientationPriorCameraToImu(self, imu): imu.GyroBiasPrior = (imu.GyroBiasPriorCount-1.0)/imu.GyroBiasPriorCount * imu.GyroBiasPrior + 1.0/imu.GyroBiasPriorCount*b_gyro #print result - print " Orientation prior camera-imu found as: (T_i_c)" - print R_i_c - print " Gyro bias prior found as: (b_gyro)" - print b_gyro + print(" Orientation prior camera-imu found as: (T_i_c)") + print(R_i_c) + print(" Gyro bias prior found as: (b_gyro)") + print(b_gyro) #return an etimate of gravity in the world coordinate frame as perceived by this camera def getEstimatedGravity(self): @@ -213,7 +213,7 @@ def getEstimatedGravity(self): # and imu, the maximum corresponds to the timeshift... # in a next step we can use the time shift to estimate the rotation between camera and imu def findTimeshiftCameraImuPrior(self, imu, verbose=False): - print "Estimating time shift camera to imu:" + print("Estimating time shift camera to imu:") #fit a spline to the camera observations poseSpline = self.initPoseSplineFromCamera( timeOffsetPadding=0.0 ) @@ -268,8 +268,8 @@ def findTimeshiftCameraImuPrior(self, imu, verbose=False): #store the timeshift (t_imu = t_cam + timeshiftCamToImuPrior) self.timeshiftCamToImuPrior = shift - print " Time shift camera to imu (t_imu = t_cam + shift):" - print self.timeshiftCamToImuPrior + print(" Time shift camera to imu (t_imu = t_cam + shift):") + print(self.timeshiftCamToImuPrior) #initialize a pose spline using camera poses (pose spline = T_wb) def initPoseSplineFromCamera(self, splineOrder=6, poseKnotsPerSecond=100, timeOffsetPadding=0.02): @@ -308,8 +308,8 @@ def initPoseSplineFromCamera(self, splineOrder=6, poseKnotsPerSecond=100, timeOf seconds = times[-1] - times[0] knots = int(round(seconds * poseKnotsPerSecond)) - print - print "Initializing a pose spline with %d knots (%f knots per second over %f seconds)" % ( knots, poseKnotsPerSecond, seconds) + print() + print("Initializing a pose spline with %d knots (%f knots per second over %f seconds)" % ( knots, poseKnotsPerSecond, seconds)) pose.initPoseSplineSparse(times, curve, knots, 1e-4) return pose @@ -326,8 +326,8 @@ def addDesignVariables(self, problem, noExtrinsics=True, noTimeCalibration=True, problem.addDesignVariable(self.cameraTimeToImuTimeDv, ic.CALIBRATION_GROUP_ID) def addCameraErrorTerms(self, problem, poseSplineDv, T_cN_b, blakeZissermanDf=0.0, timeOffsetPadding=0.0): - print - print "Adding camera error terms ({0})".format(self.dataset.topic) + print() + print("Adding camera error terms ({0})".format(self.dataset.topic)) #progress bar iProgress = sm.Progress2( len(self.targetObservations) ) @@ -395,7 +395,7 @@ def addCameraErrorTerms(self, problem, poseSplineDv, T_cN_b, blakeZissermanDf=0. #update progress bar iProgress.sample() - print "\r Added {0} camera error terms ".format( len(self.targetObservations) ) + print("\r Added {0} camera error terms ".format( len(self.targetObservations) )) self.allReprojectionErrors = allReprojectionErrors #pair of cameras with overlapping field of view (perfectly synced cams required!!) @@ -444,9 +444,9 @@ def initializeBaselines(self): for camNr in range(1, len(self.camList)): self.camList[camNr].T_extrinsic = self.chainConfig.getExtrinsicsLastCamToHere(camNr) - print "Baseline between cam{0} and cam{1} set to:".format(camNr-1,camNr) - print "T= ", self.camList[camNr].T_extrinsic.T() - print "Baseline: ", np.linalg.norm(self.camList[camNr].T_extrinsic.t()), " [m]" + print("Baseline between cam{0} and cam{1} set to:".format(camNr-1,camNr)) + print("T= ", self.camList[camNr].T_extrinsic.T()) + print("Baseline: ", np.linalg.norm(self.camList[camNr].T_extrinsic.t()), " [m]") #initialize a pose spline for the chain def initializePoseSplineFromCameraChain(self, splineOrder=6, poseKnotsPerSecond=100, timeOffsetPadding=0.02): @@ -563,11 +563,11 @@ def formatIndented(self, indent, np_array): return indent + str(np.array_str(np_array)).replace('\n',"\n"+indent) def printDetails(self, dest=sys.stdout): - print >> dest, " Model: {0}".format(self.data["model"]) + print(" Model: {0}".format(self.data["model"]), file=dest) kc.ImuParameters.printDetails(self, dest) - print >> dest, " T_i_b" - print >> dest, self.formatIndented(" ", np.array(self.data["T_i_b"])) - print >> dest, " time offset with respect to IMU0: {0} [s]".format(self.data["time_offset"]) + print(" T_i_b", file=dest) + print(self.formatIndented(" ", np.array(self.data["T_i_b"])), file=dest) + print(" time offset with respect to IMU0: {0} [s]".format(self.data["time_offset"]), file=dest) def getImuConfig(self): self.updateImuConfig() @@ -617,7 +617,7 @@ def __init__(self, stamp, omega, alpha, Rgyro, Raccel): self.stamp = stamp def loadImuData(self): - print "Reading IMU data ({0})".format(self.dataset.topic) + print("Reading IMU data ({0})".format(self.dataset.topic)) # prepare progess bar iProgress = sm.Progress2( self.dataset.numMessages() ) @@ -636,8 +636,8 @@ def loadImuData(self): self.imuData = imu if len(self.imuData)>1: - print "\r Read %d imu readings over %.1f seconds " \ - % (len(imu), imu[-1].stamp.toSec() - imu[0].stamp.toSec()) + print("\r Read %d imu readings over %.1f seconds " \ + % (len(imu), imu[-1].stamp.toSec() - imu[0].stamp.toSec())) else: sm.logFatal("Could not find any IMU messages. Please check the dataset.") sys.exit(-1) @@ -666,8 +666,8 @@ def addDesignVariables(self, problem): def addAccelerometerErrorTerms(self, problem, poseSplineDv, g_w, mSigma=0.0, \ accelNoiseScale=1.0): - print - print "Adding accelerometer error terms ({0})".format(self.dataset.topic) + print() + print("Adding accelerometer error terms ({0})".format(self.dataset.topic)) #progress bar iProgress = sm.Progress2( len(self.imuData) ) @@ -705,13 +705,13 @@ def addAccelerometerErrorTerms(self, problem, poseSplineDv, g_w, mSigma=0.0, \ #update progress bar iProgress.sample() - print "\r Added {0} of {1} accelerometer error terms (skipped {2} out-of-bounds measurements)".format( len(self.imuData)-num_skipped, len(self.imuData), num_skipped ) + print("\r Added {0} of {1} accelerometer error terms (skipped {2} out-of-bounds measurements)".format( len(self.imuData)-num_skipped, len(self.imuData), num_skipped )) self.accelErrors = accelErrors def addGyroscopeErrorTerms(self, problem, poseSplineDv, mSigma=0.0, gyroNoiseScale=1.0, \ g_w=None): - print - print "Adding gyroscope error terms ({0})".format(self.dataset.topic) + print() + print("Adding gyroscope error terms ({0})".format(self.dataset.topic)) #progress bar iProgress = sm.Progress2( len(self.imuData) ) @@ -743,7 +743,7 @@ def addGyroscopeErrorTerms(self, problem, poseSplineDv, mSigma=0.0, gyroNoiseSca #update progress bar iProgress.sample() - print "\r Added {0} of {1} gyroscope error terms (skipped {2} out-of-bounds measurements)".format( len(self.imuData)-num_skipped, len(self.imuData), num_skipped ) + print("\r Added {0} of {1} gyroscope error terms (skipped {2} out-of-bounds measurements)".format( len(self.imuData)-num_skipped, len(self.imuData), num_skipped )) self.gyroErrors = gyroErrors def initBiasSplines(self, poseSpline, splineOrder, biasKnotsPerSecond): @@ -752,8 +752,8 @@ def initBiasSplines(self, poseSpline, splineOrder, biasKnotsPerSecond): seconds = end - start; knots = int(round(seconds * biasKnotsPerSecond)) - print - print "Initializing the bias splines with %d knots" % (knots) + print() + print("Initializing the bias splines with %d knots" % (knots)) #initialize the bias splines self.gyroBias = bsplines.BSpline(splineOrder) @@ -778,8 +778,8 @@ def getTransformationFromBodyToImu(self): self.r_b_Dv.toEuclidean())) def findOrientationPrior(self, referenceImu): - print - print "Estimating imu-imu rotation initial guess." + print() + print("Estimating imu-imu rotation initial guess.") # build the problem problem = aopt.OptimizationProblem() @@ -871,9 +871,9 @@ def findOrientationPrior(self, referenceImu): refined_shift = scipy.optimize.fmin(objectiveFunction, np.array([shift]), maxiter=100)[0] self.timeOffset = float(refined_shift) - print "Temporal correction with respect to reference IMU " - print self.timeOffset, "[s]", ("" if self.estimateTimedelay else \ - " (this offset is not accounted for in the calibration)") + print("Temporal correction with respect to reference IMU ") + print(self.timeOffset, "[s]", ("" if self.estimateTimedelay else \ + " (this offset is not accounted for in the calibration)")) # Add constant gyro bias as design variable gyroBiasDv = aopt.EuclideanPointDv( np.zeros(3) ) @@ -901,8 +901,8 @@ def findOrientationPrior(self, referenceImu): sm.logFatal("Failed to obtain initial guess for the relative orientation!") sys.exit(-1) - print "Estimated imu to reference imu Rotation: " - print q_i_b_Dv.toRotationMatrix() + print("Estimated imu to reference imu Rotation: ") + print(q_i_b_Dv.toRotationMatrix()) self.q_i_b_prior = sm.r2quat(q_i_b_Dv.toRotationMatrix()) @@ -917,16 +917,16 @@ def __init__(self, imuConfig): def printDetails(self, dest=sys.stdout): IccImu.ImuParameters.printDetails(self, dest) - print >> dest, " Gyroscope: " - print >> dest, " M:" - print >> dest, self.formatIndented(" ", np.array(self.data["gyroscopes"]["M"])) - print >> dest, " A [(rad/s)/(m/s^2)]:" - print >> dest, self.formatIndented(" ", np.array(self.data["gyroscopes"]["A"])) - print >> dest, " C_gyro_i:" - print >> dest, self.formatIndented(" ", np.array(self.data["gyroscopes"]["C_gyro_i"])) - print >> dest, " Accelerometer: " - print >> dest, " M:" - print >> dest, self.formatIndented(" ", np.array(self.data["accelerometers"]["M"])) + print(" Gyroscope: ", file=dest) + print(" M:", file=dest) + print(self.formatIndented(" ", np.array(self.data["gyroscopes"]["M"])), file=dest) + print(" A [(rad/s)/(m/s^2)]:", file=dest) + print(self.formatIndented(" ", np.array(self.data["gyroscopes"]["A"])), file=dest) + print(" C_gyro_i:", file=dest) + print(self.formatIndented(" ", np.array(self.data["gyroscopes"]["C_gyro_i"])), file=dest) + print(" Accelerometer: ", file=dest) + print(" M:", file=dest) + print(self.formatIndented(" ", np.array(self.data["accelerometers"]["M"])), file=dest) def setIntrisicsMatrices(self, M_accel, C_gyro_i, M_gyro, Ma_gyro): self.data["accelerometers"] = dict() @@ -966,8 +966,8 @@ def addDesignVariables(self, problem): def addAccelerometerErrorTerms(self, problem, poseSplineDv, g_w, mSigma=0.0, \ accelNoiseScale=1.0): - print - print "Adding accelerometer error terms ({0})".format(self.dataset.topic) + print() + print("Adding accelerometer error terms ({0})".format(self.dataset.topic)) #progress bar iProgress = sm.Progress2( len(self.imuData) ) @@ -1007,12 +1007,12 @@ def addAccelerometerErrorTerms(self, problem, poseSplineDv, g_w, mSigma=0.0, \ #update progress bar iProgress.sample() - print "\r Added {0} of {1} accelerometer error terms (skipped {2} out-of-bounds measurements)".format( len(self.imuData)-num_skipped, len(self.imuData), num_skipped ) + print("\r Added {0} of {1} accelerometer error terms (skipped {2} out-of-bounds measurements)".format( len(self.imuData)-num_skipped, len(self.imuData), num_skipped )) self.accelErrors = accelErrors def addGyroscopeErrorTerms(self, problem, poseSplineDv, mSigma=0.0, gyroNoiseScale=1.0, g_w=None): - print - print "Adding gyroscope error terms ({0})".format(self.dataset.topic) + print() + print("Adding gyroscope error terms ({0})".format(self.dataset.topic)) #progress bar iProgress = sm.Progress2( len(self.imuData) ) @@ -1056,7 +1056,7 @@ def addGyroscopeErrorTerms(self, problem, poseSplineDv, mSigma=0.0, gyroNoiseSca #update progress bar iProgress.sample() - print "\r Added {0} of {1} gyroscope error terms (skipped {2} out-of-bounds measurements)".format( len(self.imuData)-num_skipped, len(self.imuData), num_skipped ) + print("\r Added {0} of {1} gyroscope error terms (skipped {2} out-of-bounds measurements)".format( len(self.imuData)-num_skipped, len(self.imuData), num_skipped )) self.gyroErrors = gyroErrors class IccScaledMisalignedSizeEffectImu(IccScaledMisalignedImu): @@ -1069,15 +1069,15 @@ def __init__(self, imuConfig): def printDetails(self, dest=sys.stdout): IccScaledMisalignedImu.ImuParameters.printDetails(self, dest) - print >> dest, " rx_i [m]:" - print >> dest, self.formatIndented(" ", \ - np.array(self.data["accelerometers"]["rx_i"])) - print >> dest, " ry_i [m]:" - print >> dest, self.formatIndented(" ", \ - np.array(self.data["accelerometers"]["ry_i"])) - print >> dest, " rz_i [m]:" - print >> dest, self.formatIndented(" ", \ - np.array(self.data["accelerometers"]["rz_i"])) + print(" rx_i [m]:", file=dest) + print(self.formatIndented(" ", \ + np.array(self.data["accelerometers"]["rx_i"])), file=dest) + print(" ry_i [m]:", file=dest) + print(self.formatIndented(" ", \ + np.array(self.data["accelerometers"]["ry_i"])), file=dest) + print(" rz_i [m]:", file=dest) + print(self.formatIndented(" ", \ + np.array(self.data["accelerometers"]["rz_i"])), file=dest) def setAccelerometerLeverArms(self, rx_i, ry_i, rz_i): self.data["accelerometers"]["rx_i"] = rx_i.tolist() @@ -1117,8 +1117,8 @@ def addDesignVariables(self, problem): def addAccelerometerErrorTerms(self, problem, poseSplineDv, g_w, mSigma=0.0, \ accelNoiseScale=1.0): - print - print "Adding accelerometer error terms ({0})".format(self.dataset.topic) + print() + print("Adding accelerometer error terms ({0})".format(self.dataset.topic)) #progress bar iProgress = sm.Progress2( len(self.imuData) ) @@ -1166,5 +1166,5 @@ def addAccelerometerErrorTerms(self, problem, poseSplineDv, g_w, mSigma=0.0, \ #update progress bar iProgress.sample() - print "\r Added {0} of {1} accelerometer error terms (skipped {2} out-of-bounds measurements)".format( len(self.imuData)-num_skipped, len(self.imuData), num_skipped ) + print("\r Added {0} of {1} accelerometer error terms (skipped {2} out-of-bounds measurements)".format( len(self.imuData)-num_skipped, len(self.imuData), num_skipped )) self.accelErrors = accelErrors \ No newline at end of file diff --git a/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccUtil.py b/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccUtil.py index 46034220e..07717eef0 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccUtil.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccUtil.py @@ -1,5 +1,5 @@ from sm import PlotCollection -import IccPlots as plots +from . import IccPlots as plots import numpy as np import pylab as pl import sys @@ -7,74 +7,74 @@ import yaml import time from matplotlib.backends.backend_pdf import PdfPages -import StringIO +import io import matplotlib.patches as patches def printErrorStatistics(cself, dest=sys.stdout): # Reprojection errors - print >> dest, "Normalized Residuals\n----------------------------" + print("Normalized Residuals\n----------------------------", file=dest) for cidx, cam in enumerate(cself.CameraChain.camList): if len(cam.allReprojectionErrors)>0: e2 = np.array([ np.sqrt(rerr.evaluateError()) for reprojectionErrors in cam.allReprojectionErrors for rerr in reprojectionErrors]) - print >> dest, "Reprojection error (cam{0}): mean {1}, median {2}, std: {3}".format(cidx, np.mean(e2), np.median(e2), np.std(e2) ) + print("Reprojection error (cam{0}): mean {1}, median {2}, std: {3}".format(cidx, np.mean(e2), np.median(e2), np.std(e2) ), file=dest) else: - print >> dest, "Reprojection error (cam{0}): no corners".format(cidx) + print("Reprojection error (cam{0}): no corners".format(cidx), file=dest) for iidx, imu in enumerate(cself.ImuList): # Gyro errors e2 = np.array([ np.sqrt(e.evaluateError()) for e in imu.gyroErrors ]) - print >> dest, "Gyroscope error (imu{0}): mean {1}, median {2}, std: {3}".format(iidx, np.mean(e2), np.median(e2), np.std(e2)) + print("Gyroscope error (imu{0}): mean {1}, median {2}, std: {3}".format(iidx, np.mean(e2), np.median(e2), np.std(e2)), file=dest) # Accelerometer errors e2 = np.array([ np.sqrt(e.evaluateError()) for e in imu.accelErrors ]) - print >> dest, "Accelerometer error (imu{0}): mean {1}, median {2}, std: {3}".format(iidx, np.mean(e2), np.median(e2), np.std(e2)) + print("Accelerometer error (imu{0}): mean {1}, median {2}, std: {3}".format(iidx, np.mean(e2), np.median(e2), np.std(e2)), file=dest) - print >> dest, "" - print >> dest, "Residuals\n----------------------------" + print("", file=dest) + print("Residuals\n----------------------------", file=dest) for cidx, cam in enumerate(cself.CameraChain.camList): if len(cam.allReprojectionErrors)>0: e2 = np.array([ np.linalg.norm(rerr.error()) for reprojectionErrors in cam.allReprojectionErrors for rerr in reprojectionErrors]) - print >> dest, "Reprojection error (cam{0}) [px]: mean {1}, median {2}, std: {3}".format(cidx, np.mean(e2), np.median(e2), np.std(e2) ) + print("Reprojection error (cam{0}) [px]: mean {1}, median {2}, std: {3}".format(cidx, np.mean(e2), np.median(e2), np.std(e2) ), file=dest) else: - print >> dest, "Reprojection error (cam{0}) [px]: no corners".format(cidx) + print("Reprojection error (cam{0}) [px]: no corners".format(cidx), file=dest) for iidx, imu in enumerate(cself.ImuList): # Gyro errors e2 = np.array([ np.linalg.norm(e.error()) for e in imu.gyroErrors ]) - print >> dest, "Gyroscope error (imu{0}) [rad/s]: mean {1}, median {2}, std: {3}".format(iidx, np.mean(e2), np.median(e2), np.std(e2)) + print("Gyroscope error (imu{0}) [rad/s]: mean {1}, median {2}, std: {3}".format(iidx, np.mean(e2), np.median(e2), np.std(e2)), file=dest) # Accelerometer errors e2 = np.array([ np.linalg.norm(e.error()) for e in imu.accelErrors ]) - print >> dest, "Accelerometer error (imu{0}) [m/s^2]: mean {1}, median {2}, std: {3}".format(iidx, np.mean(e2), np.median(e2), np.std(e2)) + print("Accelerometer error (imu{0}) [m/s^2]: mean {1}, median {2}, std: {3}".format(iidx, np.mean(e2), np.median(e2), np.std(e2)), file=dest) def printGravity(cself): - print - print "Gravity vector: (in target coordinates): [m/s^2]" - print cself.gravityDv.toEuclidean() + print() + print("Gravity vector: (in target coordinates): [m/s^2]") + print(cself.gravityDv.toEuclidean()) def printResults(cself, withCov=False): nCams = len(cself.CameraChain.camList) for camNr in range(0,nCams): T_cam_b = cself.CameraChain.getResultTrafoImuToCam(camNr) - print - print "Transformation T_cam{0}_imu0 (imu0 to cam{0}, T_ci): ".format(camNr) + print() + print("Transformation T_cam{0}_imu0 (imu0 to cam{0}, T_ci): ".format(camNr)) if withCov and camNr==0: - print "\t quaternion: ", T_cam_b.q(), " +- ", cself.std_trafo_ic[0:3] - print "\t translation: ", T_cam_b.t(), " +- ", cself.std_trafo_ic[3:] - print T_cam_b.T() + print("\t quaternion: ", T_cam_b.q(), " +- ", cself.std_trafo_ic[0:3]) + print("\t translation: ", T_cam_b.t(), " +- ", cself.std_trafo_ic[3:]) + print(T_cam_b.T()) if not cself.noTimeCalibration: - print - print "cam{0} to imu0 time: [s] (t_imu = t_cam + shift)".format(camNr) - print cself.CameraChain.getResultTimeShift(camNr), + print() + print("cam{0} to imu0 time: [s] (t_imu = t_cam + shift)".format(camNr)) + print(cself.CameraChain.getResultTimeShift(camNr), end=' ') if withCov: - print " +- ", cself.std_times[camNr] + print(" +- ", cself.std_times[camNr]) else: - print + print() - print + print() for (imuNr, imu) in enumerate(cself.ImuList): - print "IMU{0}:\n".format(imuNr), "----------------------------" + print("IMU{0}:\n".format(imuNr), "----------------------------") imu.getImuConfig().printDetails() def printBaselines(self): @@ -88,10 +88,10 @@ def printBaselines(self): else: isFixed = "" - print - print "Baseline (cam{0} to cam{1}): [m] {2}".format(camNr, camNr+1, isFixed) - print T.T() - print baseline, "[m]" + print() + print("Baseline (cam{0} to cam{1}): [m] {2}".format(camNr, camNr+1, isFixed)) + print(T.T()) + print(baseline, "[m]") def generateReport(cself, filename="report.pdf", showOnScreen=True): @@ -100,10 +100,10 @@ def generateReport(cself, filename="report.pdf", showOnScreen=True): offset = 3010 #Output calibration results in text form. - sstream = StringIO.StringIO() + sstream = io.StringIO() printResultTxt(cself, sstream) - text = [line for line in StringIO.StringIO(sstream.getvalue())] + text = [line for line in io.StringIO(sstream.getvalue())] linesPerPage = 40 while True: @@ -198,65 +198,65 @@ def saveResultTxt(cself, filename='cam_imu_result.txt'): def printResultTxt(cself, stream=sys.stdout): - print >> stream, "Calibration results" - print >> stream, "===================" + print("Calibration results", file=stream) + print("===================", file=stream) printErrorStatistics(cself, stream) # Calibration results nCams = len(cself.CameraChain.camList) for camNr in range(0,nCams): T = cself.CameraChain.getResultTrafoImuToCam(camNr) - print >> stream, "" - print >> stream, "Transformation (cam{0}):".format(camNr) - print >> stream, "-----------------------" - print >> stream, "T_ci: (imu0 to cam{0}): ".format(camNr) - print >> stream, T.T() - print >> stream, "" - print >> stream, "T_ic: (cam{0} to imu0): ".format(camNr) - print >> stream, T.inverse().T() + print("", file=stream) + print("Transformation (cam{0}):".format(camNr), file=stream) + print("-----------------------", file=stream) + print("T_ci: (imu0 to cam{0}): ".format(camNr), file=stream) + print(T.T(), file=stream) + print("", file=stream) + print("T_ic: (cam{0} to imu0): ".format(camNr), file=stream) + print(T.inverse().T(), file=stream) # Time - print >> stream, "" - print >> stream, "timeshift cam{0} to imu0: [s] (t_imu = t_cam + shift)".format(camNr) - print >> stream, cself.CameraChain.getResultTimeShift(camNr) - print >> stream, "" + print("", file=stream) + print("timeshift cam{0} to imu0: [s] (t_imu = t_cam + shift)".format(camNr), file=stream) + print(cself.CameraChain.getResultTimeShift(camNr), file=stream) + print("", file=stream) #print all baselines in the camera chain if nCams > 1: - print >> stream, "Baselines:" - print >> stream, "----------" + print("Baselines:", file=stream) + print("----------", file=stream) for camNr in range(0,nCams-1): T, baseline = cself.CameraChain.getResultBaseline(camNr, camNr+1) - print >> stream, "Baseline (cam{0} to cam{1}): ".format(camNr, camNr+1) - print >> stream, T.T() - print >> stream, "baseline norm: ", baseline, "[m]" - print >> stream, "" + print("Baseline (cam{0} to cam{1}): ".format(camNr, camNr+1), file=stream) + print(T.T(), file=stream) + print("baseline norm: ", baseline, "[m]", file=stream) + print("", file=stream) # Gravity - print >> stream, "" - print >> stream, "Gravity vector in target coords: [m/s^2]" - print >> stream, cself.gravityDv.toEuclidean() + print("", file=stream) + print("Gravity vector in target coords: [m/s^2]", file=stream) + print(cself.gravityDv.toEuclidean(), file=stream) - print >> stream, "" - print >> stream, "" - print >> stream, "Calibration configuration" - print >> stream, "=========================" - print >> stream, "" + print("", file=stream) + print("", file=stream) + print("Calibration configuration", file=stream) + print("=========================", file=stream) + print("", file=stream) for camNr, cam in enumerate( cself.CameraChain.camList ): - print >> stream, "cam{0}".format(camNr) - print >> stream, "-----" + print("cam{0}".format(camNr), file=stream) + print("-----", file=stream) cam.camConfig.printDetails(stream) cam.targetConfig.printDetails(stream) - print >> stream, "" + print("", file=stream) - print >> stream, "" - print >> stream, "" - print >> stream, "IMU configuration" - print >> stream, "=================" - print >> stream, "" + print("", file=stream) + print("", file=stream) + print("IMU configuration", file=stream) + print("=================", file=stream) + print("", file=stream) for (imuNr, imu) in enumerate(cself.ImuList): - print >> stream, "IMU{0}:\n".format(imuNr), "----------------------------" + print("IMU{0}:\n".format(imuNr), "----------------------------", file=stream) imu.getImuConfig().printDetails(stream) - print >> stream, "" + print("", file=stream) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/__init__.py b/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/__init__.py index 3ae141e34..1c4c3ecef 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/__init__.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/__init__.py @@ -1,4 +1,4 @@ -from IccCalibrator import * -import IccUtil as util -import IccPlots as plots -import IccSensors as sens +from .IccCalibrator import * +from . import IccUtil as util +from . import IccPlots as plots +from . import IccSensors as sens diff --git a/aslam_offline_calibration/kalibr/python/kalibr_rs_camera_calibration/ReprojectionErrorKnotSequenceUpdateStrategy.py b/aslam_offline_calibration/kalibr/python/kalibr_rs_camera_calibration/ReprojectionErrorKnotSequenceUpdateStrategy.py index 4180ba131..05baec7bc 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_rs_camera_calibration/ReprojectionErrorKnotSequenceUpdateStrategy.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_rs_camera_calibration/ReprojectionErrorKnotSequenceUpdateStrategy.py @@ -90,7 +90,7 @@ def __getErrorAndTimestamp(self,reprojection_errors): errors.append(reprojection_error.evaluateError()) # it is not guaranteed that the errors are sorted in tiem - newIdx = sorted(range(len(times)),key=times.__getitem__) + newIdx = sorted(list(range(len(times))),key=times.__getitem__) times = [ times[i] for i in newIdx] errors = [ errors[i] for i in newIdx] diff --git a/aslam_offline_calibration/kalibr/python/kalibr_rs_camera_calibration/RsCalibrator.py b/aslam_offline_calibration/kalibr/python/kalibr_rs_camera_calibration/RsCalibrator.py index 791a11b30..b5dc1c147 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_rs_camera_calibration/RsCalibrator.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_rs_camera_calibration/RsCalibrator.py @@ -11,9 +11,9 @@ import sys import gc import math -from ReprojectionErrorKnotSequenceUpdateStrategy import * -from RsPlot import plotSpline -from RsPlot import plotSplineValues +from .ReprojectionErrorKnotSequenceUpdateStrategy import * +from .RsPlot import plotSpline +from .RsPlot import plotSplineValues import pylab as pl import pdb @@ -253,8 +253,8 @@ def __generateInitialSpline(self, splineOrder, timeOffsetPadding, numberOfKnots else: knots = int(round(seconds * framerate/3)) - print - print "Initializing a pose spline with %d knots (%f knots per second over %f seconds)" % ( knots, 100, seconds) + print() + print(("Initializing a pose spline with %d knots (%f knots per second over %f seconds)" % ( knots, 100, seconds))) poseSpline.initPoseSplineSparse(times, curve, knots, 1e-4) return poseSpline @@ -415,7 +415,7 @@ def __initPoseDesignVariables(self, problem): def __runOptimization(self, problem ,deltaJ, deltaX, maxIt): """Run the given optimization problem problem""" - print "run new optimisation with initial values:" + print("run new optimisation with initial values:") self.__printResults() # verbose and choldmod solving with schur complement trick @@ -446,13 +446,13 @@ def __printResults(self): shutter = self.__camera_dv.shutterDesignVariable().value() proj = self.__camera_dv.projectionDesignVariable().value() dist = self.__camera_dv.distortionDesignVariable().value() - print + print() if (self.__isRollingShutter()): - print "LineDelay:" - print shutter.lineDelay() - print "Intrinsics:" - print proj.getParameters().flatten() + print("LineDelay:") + print((shutter.lineDelay())) + print("Intrinsics:") + print((proj.getParameters().flatten())) #print "(",proj.fu(),", ",proj.fv(),") (",proj.cu(),", ",proj.cv(),")" #in the future, not all projection models might support these parameters - print "Distortion:" - print dist.getParameters().flatten() + print("Distortion:") + print((dist.getParameters().flatten())) #print "(",dist.p1(),", ",dist.p2(),") (",dist.k1(),", ",dist.k2(),")" #not all distortion models implement these parameters diff --git a/aslam_offline_calibration/kalibr/python/kalibr_rs_camera_calibration/__init__.py b/aslam_offline_calibration/kalibr/python/kalibr_rs_camera_calibration/__init__.py index 0940f8237..200df6ad7 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_rs_camera_calibration/__init__.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_rs_camera_calibration/__init__.py @@ -1 +1 @@ -from RsCalibrator import * +from .RsCalibrator import * diff --git a/aslam_offline_calibration/kalibr/python/kalibr_visualize_calibration b/aslam_offline_calibration/kalibr/python/kalibr_visualize_calibration index 8e0f298c3..b8ad47281 100755 --- a/aslam_offline_calibration/kalibr/python/kalibr_visualize_calibration +++ b/aslam_offline_calibration/kalibr/python/kalibr_visualize_calibration @@ -154,7 +154,7 @@ def main(): for camchain_yaml in parsed_args.chain_yaml: camchain = kc.ConfigReader.CameraChainParameters(camchain_yaml) num_cameras = camchain.numCameras() - print("Plotting configuration with " + str(num_cameras) + " cameras.") + print(("Plotting configuration with " + str(num_cameras) + " cameras.")) base_transform = np.array([[1, 0, 0, 0], [0, 0, 1, 0], [0, -1, 0, 0], [0, 0, 0, 1]]) T_c0_cn = sm.Transformation(base_transform) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_visualize_distortion b/aslam_offline_calibration/kalibr/python/kalibr_visualize_distortion index 2fd34617f..6cb1a4146 100755 --- a/aslam_offline_calibration/kalibr/python/kalibr_visualize_distortion +++ b/aslam_offline_calibration/kalibr/python/kalibr_visualize_distortion @@ -131,7 +131,7 @@ def view_distortions(): parsed_args = parse_arguments() camchain = kc.ConfigReader.CameraChainParameters(parsed_args.chain_yaml) num_cameras = camchain.numCameras() - print("Plotting distortion with " + str(num_cameras) + " cameras.") + print(("Plotting distortion with " + str(num_cameras) + " cameras.")) for cam_index in range(num_cameras): cam = 'cam{0}'.format(cam_index) intrinsics = np.asarray(camchain.data[cam]['intrinsics']) From f01b7788c46b03a926bd3a5788d1fde212aa107d Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Tue, 30 Mar 2021 17:57:10 +0100 Subject: [PATCH 29/75] Fix local imports --- Schweizer-Messer/sm_python/python/sm/__init__.py | 10 +++++----- .../python/aslam_cameras_april/__init__.py | 2 +- .../python/aslam_cv_backend/__init__.py | 2 +- aslam_cv/aslam_cv_python/python/aslam_cv/__init__.py | 2 +- .../src/incremental_calibration/__init__.py | 2 +- .../python/aslam_splines/__init__.py | 2 +- .../bsplines_python/python/bsplines/__init__.py | 2 +- .../kalibr/python/kalibr_errorterms/__init__.py | 2 +- .../python/aslam_backend/__init__.py | 2 +- .../src/sparse_block_matrix/__init__.py | 2 +- 10 files changed, 14 insertions(+), 14 deletions(-) diff --git a/Schweizer-Messer/sm_python/python/sm/__init__.py b/Schweizer-Messer/sm_python/python/sm/__init__.py index 152259d34..20ee5af7f 100755 --- a/Schweizer-Messer/sm_python/python/sm/__init__.py +++ b/Schweizer-Messer/sm_python/python/sm/__init__.py @@ -1,11 +1,11 @@ import numpy_eigen import inspect -from libsm_python import * -from plotCoordinateFrame import plotCoordinateFrame -from Progress import Progress -from Progress import Progress2 -from saveFigTight import saveFigTight +from .libsm_python import * +from .plotCoordinateFrame import plotCoordinateFrame +from .Progress import Progress +from .Progress import Progress2 +from .saveFigTight import saveFigTight def logInfo(message): diff --git a/aslam_cv/aslam_cameras_april/python/aslam_cameras_april/__init__.py b/aslam_cv/aslam_cameras_april/python/aslam_cameras_april/__init__.py index 713e1d763..b15561191 100755 --- a/aslam_cv/aslam_cameras_april/python/aslam_cameras_april/__init__.py +++ b/aslam_cv/aslam_cameras_april/python/aslam_cameras_april/__init__.py @@ -7,7 +7,7 @@ pathToSo = os.path.dirname(os.path.realpath(__file__)) if os.path.isfile(os.path.join(pathToSo,"libaslam_cameras_april_python.so")): # Import the the C++ exports from your package library. - from libaslam_cameras_april_python import * + from .libaslam_cameras_april_python import * # Import other files in the directory # from mypyfile import * isCompiled = True diff --git a/aslam_cv/aslam_cv_backend_python/python/aslam_cv_backend/__init__.py b/aslam_cv/aslam_cv_backend_python/python/aslam_cv_backend/__init__.py index 908683f2d..a144e1fc8 100644 --- a/aslam_cv/aslam_cv_backend_python/python/aslam_cv_backend/__init__.py +++ b/aslam_cv/aslam_cv_backend_python/python/aslam_cv_backend/__init__.py @@ -6,7 +6,7 @@ import roslib; roslib.load_manifest('aslam_backend'); import aslam_backend import roslib; roslib.load_manifest('aslam_cv_python'); import aslam_cv # Import the the C++ exports from your package library. -from libaslam_cv_backend_python import * +from .libaslam_cv_backend_python import * # Import other files in the directory # from mypyfile import * diff --git a/aslam_cv/aslam_cv_python/python/aslam_cv/__init__.py b/aslam_cv/aslam_cv_python/python/aslam_cv/__init__.py index 47c601d61..34c65adfb 100755 --- a/aslam_cv/aslam_cv_python/python/aslam_cv/__init__.py +++ b/aslam_cv/aslam_cv_python/python/aslam_cv/__init__.py @@ -6,7 +6,7 @@ pathToSo = os.path.dirname(os.path.realpath(__file__)) if os.path.isfile(os.path.join(pathToSo,"libaslam_cv_python.so")): # Import the the C++ exports from your package library. - from libaslam_cv_python import * + from .libaslam_cv_python import * # Import other files in the directory # from mypyfile import * isCompiled = True diff --git a/aslam_incremental_calibration/incremental_calibration_python/src/incremental_calibration/__init__.py b/aslam_incremental_calibration/incremental_calibration_python/src/incremental_calibration/__init__.py index 7365d2217..21c6cecb9 100644 --- a/aslam_incremental_calibration/incremental_calibration_python/src/incremental_calibration/__init__.py +++ b/aslam_incremental_calibration/incremental_calibration_python/src/incremental_calibration/__init__.py @@ -7,7 +7,7 @@ if os.path.isfile(os.path.join(pathToSo,"libincremental_calibration_python.so")): roslib.load_manifest('aslam_backend_python'); import aslam_backend # Import the the C++ exports from your package library. - from libincremental_calibration_python import * + from .libincremental_calibration_python import * # Import other files in the directory # from mypyfile import * isCompiled = True diff --git a/aslam_nonparametric_estimation/aslam_splines_python/python/aslam_splines/__init__.py b/aslam_nonparametric_estimation/aslam_splines_python/python/aslam_splines/__init__.py index a756b9c8d..330bb4a5e 100755 --- a/aslam_nonparametric_estimation/aslam_splines_python/python/aslam_splines/__init__.py +++ b/aslam_nonparametric_estimation/aslam_splines_python/python/aslam_splines/__init__.py @@ -3,6 +3,6 @@ import os import aslam_backend import bsplines -from libaslam_splines_python import * +from .libaslam_splines_python import * diff --git a/aslam_nonparametric_estimation/bsplines_python/python/bsplines/__init__.py b/aslam_nonparametric_estimation/bsplines_python/python/bsplines/__init__.py index 3302488c9..e8b573988 100755 --- a/aslam_nonparametric_estimation/bsplines_python/python/bsplines/__init__.py +++ b/aslam_nonparametric_estimation/bsplines_python/python/bsplines/__init__.py @@ -1,5 +1,5 @@ # Import the numpy to Eigen type conversion. import numpy_eigen # Import the Spline C++ library. -from libbsplines_python import * +from .libbsplines_python import * from plotPoseSpline import * diff --git a/aslam_offline_calibration/kalibr/python/kalibr_errorterms/__init__.py b/aslam_offline_calibration/kalibr/python/kalibr_errorterms/__init__.py index e9a8ea558..d6d6e61e0 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_errorterms/__init__.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_errorterms/__init__.py @@ -1,2 +1,2 @@ import aslam_cv_backend #needed for errorterm base class wrapper -from libkalibr_errorterms_python import * +from .libkalibr_errorterms_python import * diff --git a/aslam_optimizer/aslam_backend_python/python/aslam_backend/__init__.py b/aslam_optimizer/aslam_backend_python/python/aslam_backend/__init__.py index 62a59a79f..afb466d13 100755 --- a/aslam_optimizer/aslam_backend_python/python/aslam_backend/__init__.py +++ b/aslam_optimizer/aslam_backend_python/python/aslam_backend/__init__.py @@ -2,7 +2,7 @@ import numpy_eigen import sm # Import the the C++ exports from your package library. -from libaslam_backend_python import * +from .libaslam_backend_python import * # Import other files in the directory # from mypyfile import * diff --git a/aslam_optimizer/sparse_block_matrix/src/sparse_block_matrix/__init__.py b/aslam_optimizer/sparse_block_matrix/src/sparse_block_matrix/__init__.py index 9cf56f55f..78e11f607 100755 --- a/aslam_optimizer/sparse_block_matrix/src/sparse_block_matrix/__init__.py +++ b/aslam_optimizer/sparse_block_matrix/src/sparse_block_matrix/__init__.py @@ -1,6 +1,6 @@ # Import the numpy to Eigen type conversion. import numpy_eigen # Import the the C++ exports from your package library. -from libsparse_block_matrix_python import * +from .libsparse_block_matrix_python import * # Import other files in the directory # from mypyfile import * From f36b58b4a4e4ef591f82e0b76c1bbc4b02ba9551 Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Tue, 30 Mar 2021 18:08:59 +0100 Subject: [PATCH 30/75] More 2to3 fixes --- .../src/numpy_eigen/test/__init__.py | 2 +- .../sm_python/python/sm/PlotCollection.py | 4 +- .../sm_python/python/sm/Progress.py | 6 +-- .../python/aslam_cameras_april/__init__.py | 2 +- .../src/createTargetPDF.py | 12 +++--- aslam_cv/aslam_cv_python/example/prototype.py | 24 ++++++------ .../python/aslam_cv/__init__.py | 2 +- .../src/incremental_calibration/__init__.py | 2 +- .../aslam_splines_python/test/OptBSpline.py | 2 +- .../test/QuadraticIntegralError.py | 2 +- .../interp_rotation/cummulativeTestPlots.py | 2 +- .../bsplines/interp_rotation/cumulative.py | 4 +- .../bsplines/interp_rotation/invariance2.py | 2 +- .../interp_rotation/testManifoldBSplines.py | 4 +- .../interp_rotation/testThreeManifold.py | 8 ++-- .../threeManifoldVisual/__init__.py | 10 ++--- .../python/bsplines/__init__.py | 2 +- .../bsplines_python/test/BSplineTests.py | 18 ++++----- .../DiffManifoldBSplineFittingExperiments.py | 38 +++++++++---------- .../bsplines_python/test/SplineTests.py | 6 +-- 20 files changed, 76 insertions(+), 76 deletions(-) diff --git a/Schweizer-Messer/numpy_eigen/src/numpy_eigen/test/__init__.py b/Schweizer-Messer/numpy_eigen/src/numpy_eigen/test/__init__.py index ef04d32c5..56bec9619 100755 --- a/Schweizer-Messer/numpy_eigen/src/numpy_eigen/test/__init__.py +++ b/Schweizer-Messer/numpy_eigen/src/numpy_eigen/test/__init__.py @@ -1,3 +1,3 @@ import os -print 'Importing numpy_test ', os.path.realpath(__file__) +print('Importing numpy_test ', os.path.realpath(__file__)) from ..libnumpy_eigen_test import * diff --git a/Schweizer-Messer/sm_python/python/sm/PlotCollection.py b/Schweizer-Messer/sm_python/python/sm/PlotCollection.py index 155aa2cb2..4cbe7abf3 100755 --- a/Schweizer-Messer/sm_python/python/sm/PlotCollection.py +++ b/Schweizer-Messer/sm_python/python/sm/PlotCollection.py @@ -52,12 +52,12 @@ def show(self): """ Show the window on screen """ - if len(self.figureList.keys()) == 0: + if len(list(self.figureList.keys())) == 0: return app = wx.PySimpleApp() frame = wx.Frame(None,-1,self.frame_name, size=self.window_size) plotter = self.PlotNotebook(frame) - for name in self.figureList.keys(): + for name in list(self.figureList.keys()): plotter.add(name, self.figureList[name]) frame.Show() app.MainLoop() diff --git a/Schweizer-Messer/sm_python/python/sm/Progress.py b/Schweizer-Messer/sm_python/python/sm/Progress.py index 7b9beece6..99e92d0f4 100644 --- a/Schweizer-Messer/sm_python/python/sm/Progress.py +++ b/Schweizer-Messer/sm_python/python/sm/Progress.py @@ -17,8 +17,8 @@ def sample(self): timePerRun = self.elapsed / self.iteration totalTime = self.numIterations * timePerRun - print "Progress %d / %d" % (self.iteration, self.numIterations) - print "Time %s / %s (%s * %d) " % (datetime.timedelta(seconds=self.elapsed), datetime.timedelta(seconds=totalTime), datetime.timedelta(seconds=timePerRun), self.numIterations) + print("Progress %d / %d" % (self.iteration, self.numIterations)) + print("Time %s / %s (%s * %d) " % (datetime.timedelta(seconds=self.elapsed), datetime.timedelta(seconds=totalTime), datetime.timedelta(seconds=timePerRun), self.numIterations)) else: self.startTime = time.time() @@ -63,7 +63,7 @@ def sample(self, steps=1): if h > 0: t_remaining_str = "%d h " % h if m > 0: t_remaining_str = t_remaining_str + "%dm " % m if s > 0: t_remaining_str = t_remaining_str + "%ds" % s - print "\r Progress {0} / {1} \t Time remaining: {2} ".format(self.iteration, self.numIterations, t_remaining_str), + print("\r Progress {0} / {1} \t Time remaining: {2} ".format(self.iteration, self.numIterations, t_remaining_str), end=' ') sys.stdout.flush() else: self.startTime = time.time() diff --git a/aslam_cv/aslam_cameras_april/python/aslam_cameras_april/__init__.py b/aslam_cv/aslam_cameras_april/python/aslam_cameras_april/__init__.py index b15561191..3ae3d788b 100755 --- a/aslam_cv/aslam_cameras_april/python/aslam_cameras_april/__init__.py +++ b/aslam_cv/aslam_cameras_april/python/aslam_cameras_april/__init__.py @@ -12,5 +12,5 @@ # from mypyfile import * isCompiled = True else: - print "Warning: the package aslam_cameras_april_python is not compiled." + print("Warning: the package aslam_cameras_april_python is not compiled.") PACKAGE_IS_NOT_COMPILED = True; diff --git a/aslam_cv/aslam_cameras_april/src/createTargetPDF.py b/aslam_cv/aslam_cameras_april/src/createTargetPDF.py index 2102c7a4a..99c678c6d 100755 --- a/aslam_cv/aslam_cameras_april/src/createTargetPDF.py +++ b/aslam_cv/aslam_cameras_april/src/createTargetPDF.py @@ -29,7 +29,7 @@ def __init__(self, chosenTagFamiliy): self.tagCodes = AprilTagCodes.TagFamilies[chosenTagFamiliy][0] self.totalBits = AprilTagCodes.TagFamilies[chosenTagFamiliy][1] except: - print "[ERROR]: Unknown tag familiy." + print("[ERROR]: Unknown tag familiy.") sys.exit(0) #borderBits must be consitent with the variable "blackBorder" in the detector code in file ethz_apriltag2/src/TagFamily.cc @@ -38,7 +38,7 @@ def generateAprilTag(canvas, position, metricSize, tagSpacing, tagID, tagFamilil try: tagCode=tagFamililyData.tagCodes[tagID] except: - print "[ERROR]: Requested tag ID of {0} not available in the {1} TagFamiliy".format(tagID, tagFamililyData.chosenTagFamiliy) + print("[ERROR]: Requested tag ID of {0} not available in the {1} TagFamiliy".format(tagID, tagFamililyData.chosenTagFamiliy)) #calculate the bit size of the tag sqrtBits = (math.sqrt(tagFamililyData.totalBits)) @@ -90,7 +90,7 @@ def generateAprilTag(canvas, position, metricSize, tagSpacing, tagID, tagFamilil def generateAprilBoard(canvas, n_cols, n_rows, tagSize, tagSpacing=0.25, tagFamilily="t36h11"): if(tagSpacing<0 or tagSpacing>1.0): - print "[ERROR]: Invalid tagSpacing specified. [0-1.0] of tagSize" + print("[ERROR]: Invalid tagSpacing specified. [0-1.0] of tagSize") sys.exit(0) #convert to cm @@ -135,7 +135,7 @@ def generateCheckerboard(canvas, n_cols, n_rows, size_cols, size_rows): size_rows = size_rows*100.0 #message - print "Generating a checkerboard with {0}x{1} corners and a box size of {2}x{3} cm".format(n_cols,n_rows,size_cols,size_rows) + print("Generating a checkerboard with {0}x{1} corners and a box size of {2}x{3} cm".format(n_cols,n_rows,size_cols,size_rows)) #draw boxes for x in range(0,n_cols+1): @@ -165,7 +165,7 @@ def generateCheckerboard(canvas, n_cols, n_rows, size_cols, size_rows): parser.add_argument('--tsize', type=float, default=0.02, dest='tsize', help='The size of one tag [m] (default: %(default)s)') parser.add_argument('--tspace', type=float, default=0.25, dest='tagspacing', help='The space between the tags in fraction of the edge size [0..1] (default: %(default)s)') - parser.add_argument('--tfam', default='t36h11', dest='tagfamiliy', help='Familiy of April tags {0} (default: %(default)s)'.format(AprilTagCodes.TagFamilies.keys())) + parser.add_argument('--tfam', default='t36h11', dest='tagfamiliy', help='Familiy of April tags {0} (default: %(default)s)'.format(list(AprilTagCodes.TagFamilies.keys()))) parser.add_argument('--csx', type=float, default=0.03, dest='chessSzX', help='The size of one chessboard square in x direction [m] (default: %(default)s)') parser.add_argument('--csy', type=float, default=0.03, dest='chessSzY', help='The size of one chessboard square in y direction [m] (default: %(default)s)') @@ -187,7 +187,7 @@ def generateCheckerboard(canvas, n_cols, n_rows, size_cols, size_rows): elif parsed.gridType == "checkerboard": generateCheckerboard(c, parsed.n_cols, parsed.n_rows, parsed.chessSzX, parsed.chessSzY) else: - print "[ERROR]: Unknown grid pattern" + print("[ERROR]: Unknown grid pattern") sys.exit(0) #write to file diff --git a/aslam_cv/aslam_cv_python/example/prototype.py b/aslam_cv/aslam_cv_python/example/prototype.py index 1ba0298e8..2733d0d89 100644 --- a/aslam_cv/aslam_cv_python/example/prototype.py +++ b/aslam_cv/aslam_cv_python/example/prototype.py @@ -70,31 +70,31 @@ def plotImages( imlist, MF, fno, name): # process the images. A real multiframe should only be returned after all images are added. MF1 = nullProcessor.addImage( stamp, 0, front ) if not MF1 is None: - print "Expected none!" + print("Expected none!") MF1 = nullProcessor.addImage( stamp, 1, left ) if not MF1 is None: - print "Expected none!" + print("Expected none!") MF1 = nullProcessor.addImage( stamp, 2, rear ) if not MF1 is None: - print "Expected none!" + print("Expected none!") MF1 = nullProcessor.addImage( stamp, 3, right ) if MF1 is None: - print "Expected a completed multiframe!" + print("Expected a completed multiframe!") # Do the same for the undistorting processor MF2 = undistortingProcessor.addImage( stamp, 0, front ) if not MF2 is None: - print "Expected none!" + print("Expected none!") MF2 = undistortingProcessor.addImage( stamp, 1, left ) if not MF2 is None: - print "Expected none!" + print("Expected none!") MF2 = undistortingProcessor.addImage( stamp, 2, rear ) if not MF2 is None: - print "Expected none!" + print("Expected none!") MF2 = undistortingProcessor.addImage( stamp, 3, right ) if MF2 is None: - print "Expected a completed multiframe!" + print("Expected a completed multiframe!") # Now we have a multiframe from each processor. @@ -106,9 +106,9 @@ def plotImages( imlist, MF, fno, name): # Note that the underlying camera geometry objects are different and # they have different numbers of features -print "\nFrom the null undistorter:" +print("\nFrom the null undistorter:") for i in range(0,MF1.numCameras()): - print "MF1, camera {0} has geometry {1} and {2} features".format(i, type(MF1.getFrame(i).geometryBase()), MF1.getFrame(i).numKeypoints()) -print "\nFrom the undistorter:" + print("MF1, camera {0} has geometry {1} and {2} features".format(i, type(MF1.getFrame(i).geometryBase()), MF1.getFrame(i).numKeypoints())) +print("\nFrom the undistorter:") for i in range(0,MF2.numCameras()): - print "MF2, camera {0} has geometry {1} and {2} features".format(i, type(MF2.getFrame(i).geometryBase()), MF2.getFrame(i).numKeypoints()) + print("MF2, camera {0} has geometry {1} and {2} features".format(i, type(MF2.getFrame(i).geometryBase()), MF2.getFrame(i).numKeypoints())) diff --git a/aslam_cv/aslam_cv_python/python/aslam_cv/__init__.py b/aslam_cv/aslam_cv_python/python/aslam_cv/__init__.py index 34c65adfb..e51134121 100755 --- a/aslam_cv/aslam_cv_python/python/aslam_cv/__init__.py +++ b/aslam_cv/aslam_cv_python/python/aslam_cv/__init__.py @@ -11,5 +11,5 @@ # from mypyfile import * isCompiled = True else: - print "Warning: the package aslam_cv_python is not compiled. Type 'rosmake aslam_cv_python' if you need this." + print("Warning: the package aslam_cv_python is not compiled. Type 'rosmake aslam_cv_python' if you need this.") PACKAGE_IS_NOT_COMPILED = True; diff --git a/aslam_incremental_calibration/incremental_calibration_python/src/incremental_calibration/__init__.py b/aslam_incremental_calibration/incremental_calibration_python/src/incremental_calibration/__init__.py index 21c6cecb9..7ec60b369 100644 --- a/aslam_incremental_calibration/incremental_calibration_python/src/incremental_calibration/__init__.py +++ b/aslam_incremental_calibration/incremental_calibration_python/src/incremental_calibration/__init__.py @@ -12,6 +12,6 @@ # from mypyfile import * isCompiled = True else: - print "Warning: the package incremental_calibration_python is not compiled. Type 'rosmake incremental_calibration_python' if you need this." + print("Warning: the package incremental_calibration_python is not compiled. Type 'rosmake incremental_calibration_python' if you need this.") PACKAGE_IS_NOT_COMPILED = True; diff --git a/aslam_nonparametric_estimation/aslam_splines_python/test/OptBSpline.py b/aslam_nonparametric_estimation/aslam_splines_python/test/OptBSpline.py index f5bb50182..11c2e4ae3 100755 --- a/aslam_nonparametric_estimation/aslam_splines_python/test/OptBSpline.py +++ b/aslam_nonparametric_estimation/aslam_splines_python/test/OptBSpline.py @@ -67,7 +67,7 @@ def numericJacobian(spline, t, knots): for i in range(0, order): vI = i + iStart; orgCV = numpy.matrix(vertices[vI]); - jacobian[:, i] = scipy.misc.derivative(lambda(cV) : evalSplineWithUpdatedCVs(spline, vertices, vI, cV, orgCV, t), orgCV); + jacobian[:, i] = scipy.misc.derivative(lambda cV : evalSplineWithUpdatedCVs(spline, vertices, vI, cV, orgCV, t), orgCV); vertices[vI] = orgCV; spline.setControlVertices(vertices) diff --git a/aslam_nonparametric_estimation/aslam_splines_python/test/QuadraticIntegralError.py b/aslam_nonparametric_estimation/aslam_splines_python/test/QuadraticIntegralError.py index 30e8cf38b..7462cea7e 100644 --- a/aslam_nonparametric_estimation/aslam_splines_python/test/QuadraticIntegralError.py +++ b/aslam_nonparametric_estimation/aslam_splines_python/test/QuadraticIntegralError.py @@ -38,7 +38,7 @@ def test_construction(self): for derivativeOrder in range(0, splineOrder - 1): p = aslam_backend.OptimizationProblem() for i in range(0, sdv.numDesignVariables()) : p.addDesignVariable(sdv.designVariable(i)) - aslam_splines.addQuadraticIntegralEuclideanExpressionErrorTermsToProblem(p, tMin, tMax, 100, lambda(time) : sdv.toEuclideanExpression(time, derivativeOrder), sqrtW) + aslam_splines.addQuadraticIntegralEuclideanExpressionErrorTermsToProblem(p, tMin, tMax, 100, lambda time : sdv.toEuclideanExpression(time, derivativeOrder), sqrtW) E = 0; for i in range(0, p.numErrorTerms()): E += p.errorTerm(i).evaluateError() diff --git a/aslam_nonparametric_estimation/bsplines/interp_rotation/cummulativeTestPlots.py b/aslam_nonparametric_estimation/bsplines/interp_rotation/cummulativeTestPlots.py index 679995642..0c05131a2 100644 --- a/aslam_nonparametric_estimation/bsplines/interp_rotation/cummulativeTestPlots.py +++ b/aslam_nonparametric_estimation/bsplines/interp_rotation/cummulativeTestPlots.py @@ -35,7 +35,7 @@ def getLocalCumulativeBi(bs, t, i): c = -1 for i in range(0, 3) : - print i + print(i) # val = np.array( [ getLocalCumulativeBi(bs, t, i) for t in T ] ) # pl.plot(T,val) # val = np.array( [ getLocalCumulativeBi(bs, t+c, i+c) if (t + c) < 8 and (t+c >0) else 0 for t in T ] ) diff --git a/aslam_nonparametric_estimation/bsplines/interp_rotation/cumulative.py b/aslam_nonparametric_estimation/bsplines/interp_rotation/cumulative.py index d662dddcc..600e8eaca 100755 --- a/aslam_nonparametric_estimation/bsplines/interp_rotation/cumulative.py +++ b/aslam_nonparametric_estimation/bsplines/interp_rotation/cumulative.py @@ -77,7 +77,7 @@ def cumQuat(bs,t,qc): tildeB = np.sum(bi[i+1:]) diff = tildeB - cumulativeBi(int(ci[i]+1)) if abs(diff) > 1e-9 : - print diff + print(diff) rval= qdot(rval, qexp(ldqi * tildeB)) @@ -112,7 +112,7 @@ def cumQuat3(bs,t,qc): nm = np.sum(qval * qval,0) for i in range(len(nm)): if nm[i] - 1 > 1E-9 : - print qval[i] + print(qval[i]) pl.figure(3) pl.clf() pl.plot(T,qval[0,:],'r-',linewidth=4) diff --git a/aslam_nonparametric_estimation/bsplines/interp_rotation/invariance2.py b/aslam_nonparametric_estimation/bsplines/interp_rotation/invariance2.py index ac43df913..0ef25466f 100755 --- a/aslam_nonparametric_estimation/bsplines/interp_rotation/invariance2.py +++ b/aslam_nonparametric_estimation/bsplines/interp_rotation/invariance2.py @@ -175,4 +175,4 @@ def cumQuat2(bs,t,qc): C1 = cexp(b * clog( np.dot(R.T, np.dot( C, R)))) C2 = np.dot( R.T, np.dot( cexp(b * clog(C) ), R)) - print C1 - C2 + print(C1 - C2) diff --git a/aslam_nonparametric_estimation/bsplines/interp_rotation/testManifoldBSplines.py b/aslam_nonparametric_estimation/bsplines/interp_rotation/testManifoldBSplines.py index ae012234a..f68021f8a 100644 --- a/aslam_nonparametric_estimation/bsplines/interp_rotation/testManifoldBSplines.py +++ b/aslam_nonparametric_estimation/bsplines/interp_rotation/testManifoldBSplines.py @@ -34,9 +34,9 @@ ebs.setControlVertices(points) espline = [ ebs.eval(t) for t in numpy.arange(time_min, time_max, 0.1)] -print numpy.array(espline).transpose(); +print(numpy.array(espline).transpose()); bs = ebs.getBSpline(); bs.setCoefficientMatrix(points.transpose()) spline = [ bs.eval (t) for t in numpy.arange(time_min, time_max, 0.1)] -print numpy.array(spline).transpose(); +print(numpy.array(spline).transpose()); diff --git a/aslam_nonparametric_estimation/bsplines/interp_rotation/testThreeManifold.py b/aslam_nonparametric_estimation/bsplines/interp_rotation/testThreeManifold.py index 60d78f3b9..fde2939b4 100644 --- a/aslam_nonparametric_estimation/bsplines/interp_rotation/testThreeManifold.py +++ b/aslam_nonparametric_estimation/bsplines/interp_rotation/testThreeManifold.py @@ -109,7 +109,7 @@ def createPointWithGeodetics(point, directions, color): samples.append(geometry.exp(ebs.eval(t), v)) else : samples.append(p) - print "sample ", t, " -> ", p + print("sample ", t, " -> ", p) v = direction p = geometry.exp(p, v) @@ -151,7 +151,7 @@ def createPointWithGeodetics(point, directions, color): else: fitCP[i] = fitCP[i] / norm -print fitCP +print(fitCP) fitebs.setControlVertices(fitCP) fitspline = [ fitebs.eval(t) for t in numpy.arange(time_min, time_max+ 1e-9, 0.01)] @@ -173,14 +173,14 @@ def updatePointer(self, p, tpos): if isinstance(fitebs, UnitQuaternionBSpline): if ShowAngularDerivative : v = fitebs.getEvaluatorAt(tpos).evalAngularVelocity() - print v + print(v) else: v = fitebs.evalD(tpos, 1) v = geometry.product(geometry.inv(p), v) # pull the vector back to identity v = geometry.exp(p, v[0 : 3]) # go in its direction starting from p self._curve.setPos([ p, v ]) else: - print fitebs + print(fitebs) pointer = Pointer() diff --git a/aslam_nonparametric_estimation/bsplines/interp_rotation/threeManifoldVisual/__init__.py b/aslam_nonparametric_estimation/bsplines/interp_rotation/threeManifoldVisual/__init__.py index 4faaece34..6664a8b61 100644 --- a/aslam_nonparametric_estimation/bsplines/interp_rotation/threeManifoldVisual/__init__.py +++ b/aslam_nonparametric_estimation/bsplines/interp_rotation/threeManifoldVisual/__init__.py @@ -1,5 +1,5 @@ import visual -import thread +import _thread import time import numpy @@ -89,14 +89,14 @@ def __interactionThread(self): if handler: handler() else: - print "'" + s + "'" - print self.__keyHandler + print("'" + s + "'") + print(self.__keyHandler) elif self.display.mouse.events: ev = self.display.mouse.getevent() if ev.press : try : p = ev.pick.point - print p + print(p) except : pass else: @@ -124,5 +124,5 @@ def startInteractionThread(self): "down" : lambda : self.__step((0, 0, -1)), } - self.__thread = thread.start_new_thread(self.__interactionThread, ()) + self.__thread = _thread.start_new_thread(self.__interactionThread, ()) return self.__thread diff --git a/aslam_nonparametric_estimation/bsplines_python/python/bsplines/__init__.py b/aslam_nonparametric_estimation/bsplines_python/python/bsplines/__init__.py index e8b573988..9faa28aa7 100755 --- a/aslam_nonparametric_estimation/bsplines_python/python/bsplines/__init__.py +++ b/aslam_nonparametric_estimation/bsplines_python/python/bsplines/__init__.py @@ -2,4 +2,4 @@ import numpy_eigen # Import the Spline C++ library. from .libbsplines_python import * -from plotPoseSpline import * +from .plotPoseSpline import * diff --git a/aslam_nonparametric_estimation/bsplines_python/test/BSplineTests.py b/aslam_nonparametric_estimation/bsplines_python/test/BSplineTests.py index 519686d1e..880ae4d7a 100755 --- a/aslam_nonparametric_estimation/bsplines_python/test/BSplineTests.py +++ b/aslam_nonparametric_estimation/bsplines_python/test/BSplineTests.py @@ -509,7 +509,7 @@ def test_integral(self): for a in numpy.arange(aspl.t_min(),aspl.t_max()-1e-15,0.4*dt): for i in numpy.arange(aspl.t_min(), aspl.t_max()-1e-15, 0.4*dt): - print "Eval at %f\n" % (i) + print("Eval at %f\n" % (i)) f = fp.splint(a,float(i),fspl) b = aspl.evalI(a,i) self.assertAlmostEqual(b, f, msg="order %d spline integral evaluated on [%f,%f] (%f != %f) was not right" % (order, a,i,float(b),f)) @@ -531,7 +531,7 @@ def test_integral_non_uniform(self): for a in numpy.arange(aspl.t_min(),aspl.t_max()-1e-15,0.4): for i in numpy.arange(aspl.t_min(), aspl.t_max()-1e-15, 0.4): - print "Eval at %f\n" % (i) + print("Eval at %f\n" % (i)) f = fp.splint(a,float(i),fspl) b = aspl.evalI(a,i) self.assertAlmostEqual(b, f, msg="order %d spline integral evaluated on [%f,%f] (%f != %f) was not right" % (order, a,i,float(b),f)) @@ -558,7 +558,7 @@ def test_integral_non_uniform_repeated(self): for a in numpy.arange(aspl.t_min(),aspl.t_max()-1e-15,0.4): for i in numpy.arange(aspl.t_min(), aspl.t_max()-1e-15, 0.4): - print "Eval at %f\n" % (i) + print("Eval at %f\n" % (i)) f = fp.splint(a,float(i),fspl) b = aspl.evalI(a,i) self.assertAlmostEqual(b, f, msg="order %d spline integral evaluated on [%f,%f] (%f != %f) was not right" % (order, a,i,float(b),f)) @@ -574,7 +574,7 @@ def test_quadratic_integral_diag(self): for DO in range(0,order): w = numpy.random.random(dim); W = numpy.diag(w); - ef = lambda(t): numpy.dot(numpy.asmatrix(aspl.Phi(t,DO)).T , numpy.dot(W, numpy.asmatrix(aspl.Phi(t,DO)))) + ef = lambda t: numpy.dot(numpy.asmatrix(aspl.Phi(t,DO)).T , numpy.dot(W, numpy.asmatrix(aspl.Phi(t,DO)))) # for each segment for s in range(0,aspl.numValidTimeSegments()): interval = aspl.timeInterval(s) @@ -583,7 +583,7 @@ def test_quadratic_integral_diag(self): Eest = numpy.zeros(E.shape) for r in range(0,E.shape[0]): for c in range(0,E.shape[1]): - efrc = lambda(t): ef(t)[r,c] + efrc = lambda t: ef(t)[r,c] A = si.quad(efrc,interval[0],interval[1]) Eest[r,c] = A[0] #print E @@ -601,7 +601,7 @@ def test_quadratic_integral_full(self): for DO in range(0,order): W = numpy.random.random([dim,dim]); W = numpy.dot(W.T,W) + numpy.eye(dim) - ef = lambda(t): numpy.dot(numpy.asmatrix(aspl.Phi(t,DO)).T , numpy.dot(W, numpy.asmatrix(aspl.Phi(t,DO)))) + ef = lambda t: numpy.dot(numpy.asmatrix(aspl.Phi(t,DO)).T , numpy.dot(W, numpy.asmatrix(aspl.Phi(t,DO)))) # for each segment for s in range(0,aspl.numValidTimeSegments()): interval = aspl.timeInterval(s) @@ -610,7 +610,7 @@ def test_quadratic_integral_full(self): Eest = numpy.zeros(E.shape) for r in range(0,E.shape[0]): for c in range(0,E.shape[1]): - efrc = lambda(t): ef(t)[r,c] + efrc = lambda t: ef(t)[r,c] A = si.quad(efrc,interval[0],interval[1]) Eest[r,c] = A[0] #print E @@ -638,7 +638,7 @@ def quad(self,t): XX[numpy.ix_(S,S)] = numpy.dot(numpy.asmatrix(self.aspl.Phi(t,DO)).T , numpy.dot(W, numpy.asmatrix(self.aspl.Phi(t,DO)))) return XX ch = CurveHelper(aspl) - ef = lambda(t): ch.quad(t) + ef = lambda t: ch.quad(t) # si.quad can't do matrices...blerg. E = aspl.curveQuadraticIntegral(W,DO) Eest = numpy.zeros(E.shape) @@ -651,7 +651,7 @@ def quad(self,t): pts = pts[pts < interval[1]] for r in range(0,E.shape[0]): for c in range(0,E.shape[1]): - efrc = lambda(t): ef(t)[r,c] + efrc = lambda t: ef(t)[r,c] A = si.quad(efrc,interval[0],interval[1], points=pts) Eest[r,c] = A[0] diff --git a/aslam_nonparametric_estimation/bsplines_python/test/DiffManifoldBSplineFittingExperiments.py b/aslam_nonparametric_estimation/bsplines_python/test/DiffManifoldBSplineFittingExperiments.py index f54704fff..b2af34d98 100755 --- a/aslam_nonparametric_estimation/bsplines_python/test/DiffManifoldBSplineFittingExperiments.py +++ b/aslam_nonparametric_estimation/bsplines_python/test/DiffManifoldBSplineFittingExperiments.py @@ -14,7 +14,7 @@ knotGenerator = s.initConstantUniformSplineWithKnotDelta(minTime, maxTime, 1, array([0])); -print "Knots:" + str(s.getKnotsVector()); +print("Knots:" + str(s.getKnotsVector())); def plotSpline(s, showIt = False): t = arange(s.getMinTime(), s.getMaxTime(), 0.01) @@ -26,7 +26,7 @@ def getAcceleration(s): return integrate.quad(lambda t: s.evalD(t, 2) * (s.evalD(t, 2)), s.getMinTime(), s.getMaxTime())[0] plotSpline(s) -print "AccelerationIntegral:\n", getAcceleration(s) +print("AccelerationIntegral:\n", getAcceleration(s)) def calcFit(): goalDist = s.eval(pos)[0] - goal @@ -37,20 +37,20 @@ def calcFit(): s.fitSpline(array([pos]), array([[goal]]), lamb, 0, array([])) plotSpline(s) -print "Value:\n", s.eval(pos); -print "AccelerationIntegral:\n", getAcceleration(s) -print "Vertices:\n", s.getControlVertices(); -print "Fit:", calcFit() +print("Value:\n", s.eval(pos)); +print("AccelerationIntegral:\n", getAcceleration(s)) +print("Vertices:\n", s.getControlVertices()); +print("Fit:", calcFit()) s.extendAndFitSpline(knotGenerator, array([pos - 1, maxTime * 2]), array([[0, goal]]), lamb, 10) plotSpline(s) -print "MaxTime:\n", s.getMaxTime(); -print "Value:\n", s.eval(pos); -print "AccelerationIntegral:\n", getAcceleration(s) -print "Vertices:\n", s.getControlVertices(); -print "Fit:", calcFit() +print("MaxTime:\n", s.getMaxTime()); +print("Value:\n", s.eval(pos)); +print("AccelerationIntegral:\n", getAcceleration(s)) +print("Vertices:\n", s.getControlVertices()); +print("Fit:", calcFit()) if False: c = s.getControlVertices() @@ -59,9 +59,9 @@ def calcFit(): #c[maxTime - 5 + 3]+=0.11404781; s.setControlVertices(c) plotSpline(s) - print "Value:\n", s.eval(pos); - print "AccelerationIntegral:\n", getAcceleration(s) - print "Vertices:\n", s.getControlVertices(); + print("Value:\n", s.eval(pos)); + print("AccelerationIntegral:\n", getAcceleration(s)) + print("Vertices:\n", s.getControlVertices()); if True: @@ -74,17 +74,17 @@ def calcFit(): z = maxTime - min(maxTime, pos + 4) ts.extend([ maxTime - z + z* float(x) / n for x in range(0, n + 1, 1) ]) values = [ 0 if x != pos else goal for x in ts ] - print ts , values; + print(ts , values); s2.initUniformSpline(array(ts), array([values]), maxTime, lamb * n) - print "Value:\n", s2.eval(pos); - print "AccelerationIntegral:\n", getAcceleration(s) - print "Vertices:\n", s2.getControlVertices(); + print("Value:\n", s2.eval(pos)); + print("AccelerationIntegral:\n", getAcceleration(s)) + print("Vertices:\n", s2.getControlVertices()); plotSpline(s2) axis([0, maxTime * 2, -0.5, 1.5]) show() -print "finished" +print("finished") diff --git a/aslam_nonparametric_estimation/bsplines_python/test/SplineTests.py b/aslam_nonparametric_estimation/bsplines_python/test/SplineTests.py index f325ce7fe..8e4fb3285 100755 --- a/aslam_nonparametric_estimation/bsplines_python/test/SplineTests.py +++ b/aslam_nonparametric_estimation/bsplines_python/test/SplineTests.py @@ -17,7 +17,7 @@ def test_new_uniform_cubic1(self): fspl = (knots,cp,3) for i in numpy.arange(2.0,4.0,0.1): - print "Eval at %f\n" % (i) + print("Eval at %f\n" % (i)) f = fp.spalde(float(i),fspl) for j in range(0,3): a = aspl.evalD(i,j) @@ -36,7 +36,7 @@ def test_new_uniform_cubic2(self): fspl = (knots,cp,3) for i in numpy.arange(2.0*dt,4.0*dt,0.1*dt): - print "Eval at %f\n" % (i) + print("Eval at %f\n" % (i)) f = fp.spalde(float(i),fspl) for j in range(0,3): a = aspl.evalD(i,j) @@ -58,7 +58,7 @@ def test_new_uniform_cubicIntegral(self): for a in numpy.arange(2.0*dt,3.9*dt,0.1*dt): for i in numpy.arange(2.1*dt,4.0*dt,0.1*dt): - print "Eval at %f\n" % (i) + print("Eval at %f\n" % (i)) f = fp.splint(a,float(i),fspl) b = aspl.evalI(a,i) assert abs(b - f) < 1e-10, "spline integral evaluated on [%f,%f] (%f != %f) was not right" % (a,i,float(b),f) From f8551d63691fd8fe262512f6bc5c2d7010d38a12 Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Tue, 30 Mar 2021 18:09:20 +0100 Subject: [PATCH 31/75] [sm_python] Update Wx backend for new version --- Schweizer-Messer/sm_python/python/sm/PlotCollection.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Schweizer-Messer/sm_python/python/sm/PlotCollection.py b/Schweizer-Messer/sm_python/python/sm/PlotCollection.py index 4cbe7abf3..a68c70357 100755 --- a/Schweizer-Messer/sm_python/python/sm/PlotCollection.py +++ b/Schweizer-Messer/sm_python/python/sm/PlotCollection.py @@ -1,11 +1,11 @@ -import wxversion -wxversion.ensureMinimal('2.8') +# import wxversion +# wxversion.ensureMinimal('2.8') import wx import wx.aui import matplotlib as mpl from matplotlib.backends.backend_wxagg import FigureCanvasWxAgg as Canvas -from matplotlib.backends.backend_wxagg import NavigationToolbar2Wx as Toolbar +from matplotlib.backends.backend_wx import NavigationToolbar2Wx as Toolbar import collections class PlotCollection: From 2d1a7ef3a420820f18c57daefd59550c6748147f Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Tue, 30 Mar 2021 18:10:45 +0100 Subject: [PATCH 32/75] [sm_python] Upgrade to package format 3, specify wxtools dependency for python2 There are no rosdep keys for python3 yet --- Schweizer-Messer/sm_python/package.xml | 20 +++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/Schweizer-Messer/sm_python/package.xml b/Schweizer-Messer/sm_python/package.xml index 246c89d8d..0eec96e85 100644 --- a/Schweizer-Messer/sm_python/package.xml +++ b/Schweizer-Messer/sm_python/package.xml @@ -1,5 +1,5 @@ - + sm_python 0.0.1 sm_python @@ -16,12 +16,14 @@ sm_logging sm_matrix_archive sm_property_tree - sm_common - numpy_eigen - python_module - sm_kinematics - sm_timing - sm_logging - sm_matrix_archive - sm_property_tree + sm_common + numpy_eigen + python_module + sm_kinematics + sm_timing + sm_logging + sm_matrix_archive + sm_property_tree + + python-wxtools From 4507cac44f4665da324460f47ebc07001fa4c327 Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Tue, 30 Mar 2021 18:11:37 +0100 Subject: [PATCH 33/75] [kalibr] Runtime fix --- .../kalibr/python/kalibr_common/TargetExtractor.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_common/TargetExtractor.py b/aslam_offline_calibration/kalibr/python/kalibr_common/TargetExtractor.py index 5be4ad9f9..fcc88250c 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_common/TargetExtractor.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_common/TargetExtractor.py @@ -78,7 +78,7 @@ def extractCornersFromDataset(dataset, detector, multithreading=False, numProces for lidx, data in enumerate(iter(resultq.get, 'STOP')): obs=data[0]; time_idx = data[1] targetObservations[lidx] = (time_idx, obs) - targetObservations = list(zip(*sorted(targetObservations, key=lambda tup: tup[0]))[1]) + targetObservations = list(zip(*sorted(targetObservations, key=lambda tup: tup[0])))[1] else: targetObservations=[] From dc3a6caf4cfe428fab295457bf06bc1df963d749 Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Tue, 30 Mar 2021 18:27:32 +0100 Subject: [PATCH 34/75] [numpy_eigen] Fix copy_routines --- .../include/numpy_eigen/copy_routines.hpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/Schweizer-Messer/numpy_eigen/include/numpy_eigen/copy_routines.hpp b/Schweizer-Messer/numpy_eigen/include/numpy_eigen/copy_routines.hpp index a6b1271d3..c3297023a 100755 --- a/Schweizer-Messer/numpy_eigen/include/numpy_eigen/copy_routines.hpp +++ b/Schweizer-Messer/numpy_eigen/include/numpy_eigen/copy_routines.hpp @@ -57,10 +57,10 @@ struct CopyEigenToNumpyVector { // Assumes M is already initialized. for(int i = 0; i < M_->size(); i++) - { - // T * p = static_cast(PyArray_GETPTR1(P_, i)); - // *p = static_cast((*M_)(i)); - } + { + T * p = static_cast(PyArray_GETPTR1((PyArrayObject*)P_, i)); + *p = static_cast((*M_)(i)); + } } }; @@ -77,10 +77,10 @@ struct CopyNumpyToEigenVector { // Assumes M is already initialized. for(int i = 0; i < M_->size(); i++) - { - // T * p = static_cast(PyArray_GETPTR1(P_, i)); - // (*M_)(i) = static_cast(*p); - } + { + T * p = static_cast(PyArray_GETPTR1((PyArrayObject*)P_, i)); + (*M_)(i) = static_cast(*p); + } } }; From 216988c5d3aafc85baef4438f533aee5621bf873 Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Tue, 30 Mar 2021 20:54:38 +0100 Subject: [PATCH 35/75] [kalibr] More runtime fixes --- .../kalibr/python/kalibr_calibrate_cameras | 4 ++-- .../kalibr/python/kalibr_calibrate_imu_camera | 2 +- .../kalibr_camera_calibration/CameraCalibrator.py | 10 +++++----- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_calibrate_cameras b/aslam_offline_calibration/kalibr/python/kalibr_calibrate_cameras index 3c8a022b6..33769b4d0 100755 --- a/aslam_offline_calibration/kalibr/python/kalibr_calibrate_cameras +++ b/aslam_offline_calibration/kalibr/python/kalibr_calibrate_cameras @@ -191,7 +191,7 @@ def main(): cameraList.append(cam) else: raise RuntimeError( "Unknown camera model: {0}. Try {1}.".format(modelName, list(cameraModels.keys())) ) - + if parsed.verbose: obsdb.printTable() @@ -403,7 +403,7 @@ def main(): pl.show() #write to file - bagtag = parsed.bagfile.translate(None, "<>:/\|?*").replace('.bag', '', 1) + bagtag = parsed.bagfile.translate("<>:/\|?*").replace('.bag', '', 1) resultFile = "camchain-" + bagtag + ".yaml" kcc.saveChainParametersYaml(calibrator, resultFile, graph) print("Results written to file: {0}".format(resultFile)) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_calibrate_imu_camera b/aslam_offline_calibration/kalibr/python/kalibr_calibrate_imu_camera index 3417318f8..a9c83d1ab 100755 --- a/aslam_offline_calibration/kalibr/python/kalibr_calibrate_imu_camera +++ b/aslam_offline_calibration/kalibr/python/kalibr_calibrate_imu_camera @@ -212,7 +212,7 @@ def main(): util.printResults(iCal, withCov=parsed.recover_cov) print() - bagtag = parsed.bagfile[0].translate(None, "<>:/\|?*").replace('.bag', '', 1) + bagtag = parsed.bagfile[0].translate("<>:/\|?*").replace('.bag', '', 1) yamlFilename = "camchain-imucam-" + bagtag + ".yaml" iCal.saveCamChainParametersYaml(yamlFilename) print(" Saving camera chain calibration to file: {0}".format(yamlFilename)) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/CameraCalibrator.py b/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/CameraCalibrator.py index 64e3f0652..4d232c51b 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/CameraCalibrator.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/CameraCalibrator.py @@ -585,8 +585,8 @@ def printParameters(cself, dest=sys.stdout): #print cameras print("Camera-system parameters:", file=dest) for cidx, cam in enumerate(cself.cameras): - d = cam.geometry.projection().distortion().getParameters().flatten(1) - p = cam.geometry.projection().getParameters().flatten(1) + d = cam.geometry.projection().distortion().getParameters().flatten() + p = cam.geometry.projection().getParameters().flatten() dd = std_cameras[cidx][0:d.shape[0]] dp = std_cameras[cidx][d.shape[0]:] print("\tcam{0} ({1}):".format(cidx, cam.dataset.topic), file=dest) @@ -634,12 +634,12 @@ def printDebugEnd(cself): print(se[1]) print() - p = cam.geometry.projection().getParameters().flatten(1) + p = cam.geometry.projection().getParameters().flatten() for temp in p: print(temp) print() - d = cam.geometry.projection().distortion().getParameters().flatten(1) + d = cam.geometry.projection().distortion().getParameters().flatten() for temp in d: print(temp) @@ -693,7 +693,7 @@ def saveChainParametersYaml(cself, resultFile, graph): else: raise RuntimeError("Invalid camera model {}.".format(cameraModel)) camParams.setResolution( [P.ru(), P.rv()] ) - dist_coeffs = P.distortion().getParameters().flatten(1) + dist_coeffs = P.distortion().getParameters().flatten() camParams.setDistortion( distortionModel, dist_coeffs) chain.addCameraAtEnd(camParams) From 8e06353fb502956d3c7fe9df20bc3e60aa95d517 Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Tue, 30 Mar 2021 20:54:57 +0100 Subject: [PATCH 36/75] [aslam_cv_python] Do not expose isValid as it breaks compilation To be reactivated at a later point... --- aslam_cv/aslam_cv_python/src/CameraGeometries.cpp | 3 ++- aslam_cv/aslam_cv_python/src/CameraProjections.cpp | 2 ++ 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/aslam_cv/aslam_cv_python/src/CameraGeometries.cpp b/aslam_cv/aslam_cv_python/src/CameraGeometries.cpp index 2876e59ba..a8c60b71c 100644 --- a/aslam_cv/aslam_cv_python/src/CameraGeometries.cpp +++ b/aslam_cv/aslam_cv_python/src/CameraGeometries.cpp @@ -172,7 +172,8 @@ void exportCameraGeometries() { class_ >("ImageMask", init<>()) .def("setMask", &ImageMask::setMaskFromMatrix) .def("getMask", &ImageMask::getMaskAsMatrix) - .def("isValid", &ImageMask::isValid); + // .def("isValid", &ImageMask::isValid) // per https://github.com/ethz-asl/kalibr/pull/405#issuecomment-809779442 + ; class_ >("NoMask", init<>()) .def("isValid", &NoMask::isValid); diff --git a/aslam_cv/aslam_cv_python/src/CameraProjections.cpp b/aslam_cv/aslam_cv_python/src/CameraProjections.cpp index b9d4858e3..741172810 100644 --- a/aslam_cv/aslam_cv_python/src/CameraProjections.cpp +++ b/aslam_cv/aslam_cv_python/src/CameraProjections.cpp @@ -188,7 +188,9 @@ void exportGenericProjectionFunctions(T & proj) { "keypointToEuclideanJk", &e2kJp, "Map a keypoint to a 3x1 Euclidean point and get the Jacobian of the mapping with respect to small changes in the keypoint.\n(p, Jk) = keypointToEuclideanJk(k)"); + /* proj.def("isValid", &C::template isValid); + */ proj.def("setParameters", &C::setParameters, "Set the Parameter Vector/Matrix"); proj.def("getParameters", &getParameters, From 8abe74e566eef94e4632b231a19e7503c2ad0834 Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Tue, 30 Mar 2021 20:55:53 +0100 Subject: [PATCH 37/75] [kalibr] Remaining runtime fixes --- .../kalibr/python/kalibr_visualize_distortion | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_visualize_distortion b/aslam_offline_calibration/kalibr/python/kalibr_visualize_distortion index 6cb1a4146..825e9c30b 100755 --- a/aslam_offline_calibration/kalibr/python/kalibr_visualize_distortion +++ b/aslam_offline_calibration/kalibr/python/kalibr_visualize_distortion @@ -32,19 +32,22 @@ def parse_arguments(): parsed_args = parser.parse_args() return parsed_args -def normalize((u,v), focal_lengths, principal_point): +def normalize(xy, focal_lengths, principal_point): + (u,v) = xy x = (u - principal_point[1]) / focal_lengths[1] y = (v - principal_point[0]) / focal_lengths[0] return (x,y) -def denormalize((x,y), focal_lengths, principal_point): +def denormalize(xy, focal_lengths, principal_point): + (x,y) = xy u = x * focal_lengths[1] + principal_point[1] v = y * focal_lengths[0] + principal_point[0] return (u,v) -def distortPoint((x,y), distortion_model, distortion_coeffs): +def distortPoint(xy, distortion_model, distortion_coeffs): # Distortes a point regarding the distortion model. The Input point has to # be in normalized coordinates. + (x,y) = xy if distortion_model == "radial-tangential" or distortion_model == "radtan": x2 = x**2 y2 = y**2 From 8fb3488750f0dcf3ddadfa1db439cc87b1088c58 Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Tue, 30 Mar 2021 21:41:10 +0100 Subject: [PATCH 38/75] [sm_python] Fix wx deprecation --- Schweizer-Messer/sm_python/python/sm/PlotCollection.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Schweizer-Messer/sm_python/python/sm/PlotCollection.py b/Schweizer-Messer/sm_python/python/sm/PlotCollection.py index a68c70357..82d10603e 100755 --- a/Schweizer-Messer/sm_python/python/sm/PlotCollection.py +++ b/Schweizer-Messer/sm_python/python/sm/PlotCollection.py @@ -54,7 +54,7 @@ def show(self): """ if len(list(self.figureList.keys())) == 0: return - app = wx.PySimpleApp() + app = wx.App() frame = wx.Frame(None,-1,self.frame_name, size=self.window_size) plotter = self.PlotNotebook(frame) for name in list(self.figureList.keys()): From 4be1ef0b79aeb95b44778472b01ab6e10663073d Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Tue, 30 Mar 2021 21:41:24 +0100 Subject: [PATCH 39/75] Remove double bracketed print --- .../CameraCalibrator.py | 2 +- .../MulticamGraph.py | 2 +- .../python/kalibr_common/ImuDatasetReader.py | 4 +-- .../python/kalibr_common/TargetExtractor.py | 2 +- .../IccCalibrator.py | 29 +++++++++---------- .../RsCalibrator.py | 8 ++--- 6 files changed, 23 insertions(+), 24 deletions(-) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/CameraCalibrator.py b/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/CameraCalibrator.py index 4d232c51b..1736d69e0 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/CameraCalibrator.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/CameraCalibrator.py @@ -314,7 +314,7 @@ def getPointStatistics(cself,view_id,cam_id,pidx): z = np.array([0,0,1]) dz = np.dot(z,vc) if np.abs(dz - 1.0) > 1e-3: - print(("The point statistics are only valid if the camera points down the z axis. This camera has the image center: [%f, %f, %f]" % (z[0],z[1],z[2]))) + print("The point statistics are only valid if the camera points down the z axis. This camera has the image center: [%f, %f, %f]" % (z[0],z[1],z[2])) valid, y = obs.imagePoint(pidx) geometry = cself.cameras[cam_id].geometry rerr = None diff --git a/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/MulticamGraph.py b/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/MulticamGraph.py index 041a33d61..87e621bd6 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/MulticamGraph.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/MulticamGraph.py @@ -161,7 +161,7 @@ def getInitialGuesses(self, cameras): camL_nr = vertices[1] camH_nr = vertices[0] - print(("\t initializing camera pair ({0},{1})... ".format(camL_nr, camH_nr))) + print("\t initializing camera pair ({0},{1})... ".format(camL_nr, camH_nr)) #run the pair extrinsic calibration obs_list = self.obs_db.getAllObsTwoCams(camL_nr, camH_nr) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_common/ImuDatasetReader.py b/aslam_offline_calibration/kalibr/python/kalibr_common/ImuDatasetReader.py index 61fb0af5b..e1470cf97 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_common/ImuDatasetReader.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_common/ImuDatasetReader.py @@ -73,8 +73,8 @@ def truncateIndicesFromTime(self, indices, bag_from_to): bagstart = min(timestamps) baglength = max(timestamps)-bagstart - print(("bagstart",bagstart)) - print(("baglength",baglength)) + print("bagstart", bagstart) + print("baglength", baglength) #some value checking if bag_from_to[0]>=bag_from_to[1]: raise RuntimeError("Bag start time must be bigger than end time.".format(bag_from_to[0])) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_common/TargetExtractor.py b/aslam_offline_calibration/kalibr/python/kalibr_common/TargetExtractor.py index fcc88250c..198b0651c 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_common/TargetExtractor.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_common/TargetExtractor.py @@ -99,7 +99,7 @@ def extractCornersFromDataset(dataset, detector, multithreading=False, numProces print("\r") sm.logFatal("No corners could be extracted for camera {0}! Check the calibration target configuration and dataset.".format(dataset.topic)) else: - print(("\r Extracted corners for %d images (of %d images) " % (len(targetObservations), numImages))) + print("\r Extracted corners for %d images (of %d images) " % (len(targetObservations), numImages)) #close all opencv windows that might be open cv2.destroyAllWindows() diff --git a/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccCalibrator.py b/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccCalibrator.py index 6387b40e0..2453365f2 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccCalibrator.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccCalibrator.py @@ -80,19 +80,19 @@ def buildProblem( self, timeOffsetPadding=0.02, verbose=False ): - print(("\tSpline order: %d" % (splineOrder))) - print(("\tPose knots per second: %d" % (poseKnotsPerSecond))) - print(("\tDo pose motion regularization: %s" % (doPoseMotionError))) - print(("\t\txddot translation variance: %f" % (mrTranslationVariance))) - print(("\t\txddot rotation variance: %f" % (mrRotationVariance))) - print(("\tBias knots per second: %d" % (biasKnotsPerSecond))) - print(("\tDo bias motion regularization: %s" % (doBiasMotionError))) - print(("\tBlake-Zisserman on reprojection errors %s" % blakeZisserCam)) - print(("\tAcceleration Huber width (sigma): %f" % (huberAccel))) - print(("\tGyroscope Huber width (sigma): %f" % (huberGyro))) - print(("\tDo time calibration: %s" % (not noTimeCalibration))) - print(("\tMax iterations: %d" % (maxIterations))) - print(("\tTime offset padding: %f" % (timeOffsetPadding))) + print("\tSpline order: %d" % (splineOrder)) + print("\tPose knots per second: %d" % (poseKnotsPerSecond)) + print("\tDo pose motion regularization: %s" % (doPoseMotionError)) + print("\t\txddot translation variance: %f" % (mrTranslationVariance)) + print("\t\txddot rotation variance: %f" % (mrRotationVariance)) + print("\tBias knots per second: %d" % (biasKnotsPerSecond)) + print("\tDo bias motion regularization: %s" % (doBiasMotionError)) + print("\tBlake-Zisserman on reprojection errors %s" % blakeZisserCam) + print("\tAcceleration Huber width (sigma): %f" % (huberAccel)) + print("\tGyroscope Huber width (sigma): %f" % (huberGyro)) + print("\tDo time calibration: %s" % (not noTimeCalibration)) + print("\tMax iterations: %d" % (maxIterations)) + print("\tTime offset padding: %f" % (timeOffsetPadding)) ############################################ @@ -230,5 +230,4 @@ def saveCamChainParametersYaml(self, resultFile): try: chain.writeYaml(resultFile) except: - print(("ERROR: Could not write parameters to file: {0}\n".format(resultFile))) - + "ERROR: Could not write parameters to file: {0}\n".format(resultFile)) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_rs_camera_calibration/RsCalibrator.py b/aslam_offline_calibration/kalibr/python/kalibr_rs_camera_calibration/RsCalibrator.py index 0ebaf7f15..7e52caffb 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_rs_camera_calibration/RsCalibrator.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_rs_camera_calibration/RsCalibrator.py @@ -254,7 +254,7 @@ def __generateInitialSpline(self, splineOrder, timeOffsetPadding, numberOfKnots knots = int(round(seconds * framerate/3)) print() - print(("Initializing a pose spline with %d knots (%f knots per second over %f seconds)" % ( knots, 100, seconds))) + print("Initializing a pose spline with %d knots (%f knots per second over %f seconds)" % ( knots, 100, seconds)) poseSpline.initPoseSplineSparse(times, curve, knots, 1e-4) return poseSpline @@ -458,8 +458,8 @@ def __printResults(self): print() if (self.__isRollingShutter()): print("LineDelay:") - print((shutter.lineDelay())) + print(shutter.lineDelay()) print("Intrinsics:") - print((proj.getParameters().flatten())) + print(proj.getParameters().flatten()) print("Distortion:") - print((dist.getParameters().flatten())) + print(dist.getParameters().flatten()) From 1501d65fc479f35a3131c418f6642c507a55dd9d Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Tue, 30 Mar 2021 21:42:33 +0100 Subject: [PATCH 40/75] [kalibr] Remove two more double bracketed print statements --- .../kalibr/python/kalibr_visualize_calibration | 2 +- .../kalibr/python/kalibr_visualize_distortion | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_visualize_calibration b/aslam_offline_calibration/kalibr/python/kalibr_visualize_calibration index b8ad47281..8e0f298c3 100755 --- a/aslam_offline_calibration/kalibr/python/kalibr_visualize_calibration +++ b/aslam_offline_calibration/kalibr/python/kalibr_visualize_calibration @@ -154,7 +154,7 @@ def main(): for camchain_yaml in parsed_args.chain_yaml: camchain = kc.ConfigReader.CameraChainParameters(camchain_yaml) num_cameras = camchain.numCameras() - print(("Plotting configuration with " + str(num_cameras) + " cameras.")) + print("Plotting configuration with " + str(num_cameras) + " cameras.") base_transform = np.array([[1, 0, 0, 0], [0, 0, 1, 0], [0, -1, 0, 0], [0, 0, 0, 1]]) T_c0_cn = sm.Transformation(base_transform) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_visualize_distortion b/aslam_offline_calibration/kalibr/python/kalibr_visualize_distortion index 825e9c30b..4bc008955 100755 --- a/aslam_offline_calibration/kalibr/python/kalibr_visualize_distortion +++ b/aslam_offline_calibration/kalibr/python/kalibr_visualize_distortion @@ -134,7 +134,7 @@ def view_distortions(): parsed_args = parse_arguments() camchain = kc.ConfigReader.CameraChainParameters(parsed_args.chain_yaml) num_cameras = camchain.numCameras() - print(("Plotting distortion with " + str(num_cameras) + " cameras.")) + print("Plotting distortion with " + str(num_cameras) + " cameras.") for cam_index in range(num_cameras): cam = 'cam{0}'.format(cam_index) intrinsics = np.asarray(camchain.data[cam]['intrinsics']) From a7fc7a5d09a570ce8e4766745b5ae3712a9fb585 Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Tue, 30 Mar 2021 23:11:55 +0100 Subject: [PATCH 41/75] Upgrade SuiteSparse to system-provided library & upgrade from v4 to v5 --- .../src/core/LinearSolver.cpp | 14 +- aslam_optimizer/aslam_backend/CMakeLists.txt | 16 +- .../sparse_block_matrix/CMakeLists.txt | 42 +- .../cmake/FindSuiteSparse.cmake | 531 ++++++++++++++++++ .../cmake/sparse_block_matrix-extras.cmake.in | 2 + .../sparse_block_matrix/sparse_helper.h | 2 - .../sparse_block_matrix/package.xml | 16 +- suitesparse/.gitignore | 13 - suitesparse/CMakeLists.txt | 68 --- suitesparse/LICENSE | 165 ------ suitesparse/README.md | 6 - suitesparse/cmake/suitesparse-extras.cmake.in | 17 - suitesparse/include/README | 1 - suitesparse/package.xml | 18 - suitesparse/src/export_lib_hack.cc | 0 15 files changed, 581 insertions(+), 330 deletions(-) create mode 100644 aslam_optimizer/sparse_block_matrix/cmake/FindSuiteSparse.cmake create mode 100644 aslam_optimizer/sparse_block_matrix/cmake/sparse_block_matrix-extras.cmake.in delete mode 100644 suitesparse/.gitignore delete mode 100644 suitesparse/CMakeLists.txt delete mode 100644 suitesparse/LICENSE delete mode 100644 suitesparse/README.md delete mode 100644 suitesparse/cmake/suitesparse-extras.cmake.in delete mode 100644 suitesparse/include/README delete mode 100644 suitesparse/package.xml delete mode 100644 suitesparse/src/export_lib_hack.cc diff --git a/aslam_incremental_calibration/incremental_calibration/src/core/LinearSolver.cpp b/aslam_incremental_calibration/incremental_calibration/src/core/LinearSolver.cpp index 8dcb1830b..57c4f7819 100644 --- a/aslam_incremental_calibration/incremental_calibration/src/core/LinearSolver.cpp +++ b/aslam_incremental_calibration/incremental_calibration/src/core/LinearSolver.cpp @@ -209,7 +209,7 @@ namespace aslam { } double LinearSolver::getNumFlops() const { - return _cholmod.SPQR_xstat[0]; + return _cholmod.SPQR_flopcount_bound; } double LinearSolver::getLinearSolverTime() const { @@ -222,14 +222,14 @@ namespace aslam { double LinearSolver::getSymbolicFactorizationTime() const { if (_factor && _factor->QRsym) - return _cholmod.other1[1]; + return _cholmod.SPQR_analyze_time; else return 0.0; } double LinearSolver::getNumericFactorizationTime() const { if (_factor && _factor->QRnum) - return _cholmod.other1[2]; + return _cholmod.SPQR_analyze_time; else return 0.0; } @@ -329,7 +329,7 @@ namespace aslam { _factor = SuiteSparseQR_symbolic(SPQR_ORDERING_BEST, SPQR_DEFAULT_TOL, A_l, &_cholmod); const double t3 = Timestamp::now(); - _cholmod.other1[1] = t3 - t2; + _cholmod.SPQR_analyze_time = t3 - t2; if (_factor == NULL) { cholmod_l_free_sparse(&A_l, &_cholmod); if (G_l) @@ -344,7 +344,7 @@ namespace aslam { const int status = SuiteSparseQR_numeric(qrTolerance, A_l, _factor, &_cholmod); const double t3 = Timestamp::now(); - _cholmod.other1[2] = t3 - t2; + _cholmod.SPQR_analyze_time = t3 - t2; cholmod_l_free_sparse(&A_l, &_cholmod); if (!status) { if (G_l) @@ -476,7 +476,7 @@ namespace aslam { _factor = SuiteSparseQR_symbolic(SPQR_ORDERING_BEST, SPQR_DEFAULT_TOL, A_l, &_cholmod); const double t3 = Timestamp::now(); - _cholmod.other1[1] = t3 - t2; + _cholmod.SPQR_analyze_time = t3 - t2; if (_factor == NULL) { cholmod_l_free_sparse(&A_l, &_cholmod); throw InvalidOperationException("SuiteSparseQR_symbolic failed", @@ -489,7 +489,7 @@ namespace aslam { const int status = SuiteSparseQR_numeric(qrTolerance, A_l, _factor, &_cholmod); const double t3 = Timestamp::now(); - _cholmod.other1[2] = t3 - t2; + _cholmod.SPQR_analyze_time = t3 - t2; cholmod_l_free_sparse(&A_l, &_cholmod); if (!status) throw InvalidOperationException("SuiteSparseQR_numeric failed", diff --git a/aslam_optimizer/aslam_backend/CMakeLists.txt b/aslam_optimizer/aslam_backend/CMakeLists.txt index b5e5f7b91..fa44490ff 100644 --- a/aslam_optimizer/aslam_backend/CMakeLists.txt +++ b/aslam_optimizer/aslam_backend/CMakeLists.txt @@ -11,11 +11,6 @@ find_package(Boost REQUIRED COMPONENTS system thread) find_package(Eigen3 REQUIRED) include_directories(${EIGEN3_INCLUDE_DIRS}) -#add_definitions( -fPIC -msse2 -mssse3 -march=nocona -Wextra -Winit-self -Woverloaded-virtual -Wnon-virtual-dtor -Wsign-promo -Wno-long-long -O3) - -#target_link_libraries(${PROJECT_NAME} ${CHOLMOD_LIBRARIES} ${EXTRA_LIBS} ${CSPARSE_LIBRARY}) -#rosbuild_link_boost(${PROJECT_NAME} thread system) - cs_add_library(${PROJECT_NAME} src/MEstimatorPolicies.cpp src/JacobianContainer.cpp @@ -48,11 +43,14 @@ cs_add_library(${PROJECT_NAME} src/DogLegTrustRegionPolicy.cpp src/Optimizer2.cpp ) - -target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES} ${TBB_LIBRARIES}) +target_link_libraries(${PROJECT_NAME} + ${Boost_LIBRARIES} + ${TBB_LIBRARIES} + ${CHOLMOD_LIBRARY} + ${SUITESPARSE_LIBRARIES} +) if(CATKIN_ENABLE_TESTING) - # Avoid clash with tr1::tuple: https://code.google.com/p/googletest/source/browse/trunk/README?r=589#257 add_definitions(-DGTEST_USE_OWN_TR1_TUPLE=0) catkin_add_gtest(${PROJECT_NAME}_test @@ -69,9 +67,7 @@ if(CATKIN_ENABLE_TESTING) test/ErrorTermTests.cpp ) target_link_libraries(${PROJECT_NAME}_test ${PROJECT_NAME}) - endif() cs_install() cs_export() - diff --git a/aslam_optimizer/sparse_block_matrix/CMakeLists.txt b/aslam_optimizer/sparse_block_matrix/CMakeLists.txt index c8e3c200c..4cd45848d 100644 --- a/aslam_optimizer/sparse_block_matrix/CMakeLists.txt +++ b/aslam_optimizer/sparse_block_matrix/CMakeLists.txt @@ -1,24 +1,34 @@ cmake_minimum_required(VERSION 3.0.2) +project(sparse_block_matrix) -# This helps find the TBB library -set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/../cmake") +find_package(catkin REQUIRED COMPONENTS sm_common sm_eigen) -project(sparse_block_matrix) -find_package(catkin_simple REQUIRED) -catkin_simple() +list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/cmake) +find_package(SuiteSparse REQUIRED) find_package(Eigen3) -include_directories(${EIGEN3_INCLUDE_DIRS}) -cs_add_library(${PROJECT_NAME} +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${CHOLMOD_INCLUDE_DIR} + ${EIGEN3_INCLUDE_DIRS} +) + +catkin_package( + LIBRARIES ${PROJECT_NAME} + INCLUDE_DIRS include/ + CATKIN_DEPENDS sm_common sm_eigen + CFG_EXTRAS FindSuiteSparse.cmake sparse_block_matrix-extras.cmake +) + +add_library(${PROJECT_NAME} src/matrix_structure.cpp src/sparse_helper.cpp src/marginal_covariance_cholesky.cpp ) - if(CATKIN_ENABLE_TESTING) - # Avoid clash with tr1::tuple: https://code.google.com/p/googletest/source/browse/trunk/README?r=589#257 add_definitions(-DGTEST_USE_OWN_TR1_TUPLE=0) @@ -26,13 +36,13 @@ if(CATKIN_ENABLE_TESTING) test/test_main.cpp test/solver_tests.cpp test/sparse_block_matrix_tests.cpp - ) - + ) target_link_libraries(${PROJECT_NAME}_tests ${PROJECT_NAME} ${TBB_LIBRARIES}) - endif() -cs_install() -cs_export() - - +# Install +install(TARGETS ${PROJECT_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) +install(FILES cmake/FindSuiteSparse.cmake DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/cmake/) diff --git a/aslam_optimizer/sparse_block_matrix/cmake/FindSuiteSparse.cmake b/aslam_optimizer/sparse_block_matrix/cmake/FindSuiteSparse.cmake new file mode 100644 index 000000000..aad89040c --- /dev/null +++ b/aslam_optimizer/sparse_block_matrix/cmake/FindSuiteSparse.cmake @@ -0,0 +1,531 @@ +# Ceres Solver - A fast non-linear least squares minimizer +# Copyright 2015 Google Inc. All rights reserved. +# http://ceres-solver.org/ +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# * Neither the name of Google Inc. nor the names of its contributors may be +# used to endorse or promote products derived from this software without +# specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# Author: alexs.mac@gmail.com (Alex Stewart) +# + +# FindSuiteSparse.cmake - Find SuiteSparse libraries & dependencies. +# +# This module defines the following variables: +# +# SUITESPARSE_FOUND: TRUE iff SuiteSparse and all dependencies have been found. +# SUITESPARSE_INCLUDE_DIRS: Include directories for all SuiteSparse components. +# SUITESPARSE_LIBRARIES: Libraries for all SuiteSparse component libraries and +# dependencies. +# SUITESPARSE_VERSION: Extracted from UFconfig.h (<= v3) or +# SuiteSparse_config.h (>= v4). +# SUITESPARSE_MAIN_VERSION: Equal to 4 if SUITESPARSE_VERSION = 4.2.1 +# SUITESPARSE_SUB_VERSION: Equal to 2 if SUITESPARSE_VERSION = 4.2.1 +# SUITESPARSE_SUBSUB_VERSION: Equal to 1 if SUITESPARSE_VERSION = 4.2.1 +# +# SUITESPARSE_IS_BROKEN_SHARED_LINKING_UBUNTU_SYSTEM_VERSION: TRUE iff running +# on Ubuntu, SUITESPARSE_VERSION is 3.4.0 and found SuiteSparse is a system +# install, in which case found version of SuiteSparse cannot be used to link +# a shared library due to a bug (static linking is unaffected). +# +# The following variables control the behaviour of this module: +# +# SUITESPARSE_INCLUDE_DIR_HINTS: List of additional directories in which to +# search for SuiteSparse includes, +# e.g: /timbuktu/include. +# SUITESPARSE_LIBRARY_DIR_HINTS: List of additional directories in which to +# search for SuiteSparse libraries, +# e.g: /timbuktu/lib. +# +# The following variables define the presence / includes & libraries for the +# SuiteSparse components searched for, the SUITESPARSE_XX variables are the +# union of the variables for all components. +# +# == Symmetric Approximate Minimum Degree (AMD) +# AMD_FOUND +# AMD_INCLUDE_DIR +# AMD_LIBRARY +# +# == Constrained Approximate Minimum Degree (CAMD) +# CAMD_FOUND +# CAMD_INCLUDE_DIR +# CAMD_LIBRARY +# +# == Column Approximate Minimum Degree (COLAMD) +# COLAMD_FOUND +# COLAMD_INCLUDE_DIR +# COLAMD_LIBRARY +# +# Constrained Column Approximate Minimum Degree (CCOLAMD) +# CCOLAMD_FOUND +# CCOLAMD_INCLUDE_DIR +# CCOLAMD_LIBRARY +# +# == Sparse Supernodal Cholesky Factorization and Update/Downdate (CHOLMOD) +# CHOLMOD_FOUND +# CHOLMOD_INCLUDE_DIR +# CHOLMOD_LIBRARY +# +# == Multifrontal Sparse QR (SuiteSparseQR) +# SUITESPARSEQR_FOUND +# SUITESPARSEQR_INCLUDE_DIR +# SUITESPARSEQR_LIBRARY +# +# == Common configuration for all but CSparse (SuiteSparse version >= 4). +# SUITESPARSE_CONFIG_FOUND +# SUITESPARSE_CONFIG_INCLUDE_DIR +# SUITESPARSE_CONFIG_LIBRARY +# +# == Common configuration for all but CSparse (SuiteSparse version < 4). +# UFCONFIG_FOUND +# UFCONFIG_INCLUDE_DIR +# +# Optional SuiteSparse Dependencies: +# +# == Serial Graph Partitioning and Fill-reducing Matrix Ordering (METIS) +# METIS_FOUND +# METIS_LIBRARY + +# Reset CALLERS_CMAKE_FIND_LIBRARY_PREFIXES to its value when +# FindSuiteSparse was invoked. +macro(SUITESPARSE_RESET_FIND_LIBRARY_PREFIX) + if (MSVC) + set(CMAKE_FIND_LIBRARY_PREFIXES "${CALLERS_CMAKE_FIND_LIBRARY_PREFIXES}") + endif (MSVC) +endmacro(SUITESPARSE_RESET_FIND_LIBRARY_PREFIX) + +# Called if we failed to find SuiteSparse or any of it's required dependencies, +# unsets all public (designed to be used externally) variables and reports +# error message at priority depending upon [REQUIRED/QUIET/] argument. +macro(SUITESPARSE_REPORT_NOT_FOUND REASON_MSG) + unset(SUITESPARSE_FOUND) + unset(SUITESPARSE_INCLUDE_DIRS) + unset(SUITESPARSE_LIBRARIES) + unset(SUITESPARSE_VERSION) + unset(SUITESPARSE_MAIN_VERSION) + unset(SUITESPARSE_SUB_VERSION) + unset(SUITESPARSE_SUBSUB_VERSION) + # Do NOT unset SUITESPARSE_FOUND_REQUIRED_VARS here, as it is used by + # FindPackageHandleStandardArgs() to generate the automatic error message on + # failure which highlights which components are missing. + + suitesparse_reset_find_library_prefix() + + # Note _FIND_[REQUIRED/QUIETLY] variables defined by FindPackage() + # use the camelcase library name, not uppercase. + if (SuiteSparse_FIND_QUIETLY) + message(STATUS "Failed to find SuiteSparse - " ${REASON_MSG} ${ARGN}) + elseif (SuiteSparse_FIND_REQUIRED) + message(FATAL_ERROR "Failed to find SuiteSparse - " ${REASON_MSG} ${ARGN}) + else() + # Neither QUIETLY nor REQUIRED, use no priority which emits a message + # but continues configuration and allows generation. + message("-- Failed to find SuiteSparse - " ${REASON_MSG} ${ARGN}) + endif (SuiteSparse_FIND_QUIETLY) + + # Do not call return(), s/t we keep processing if not called with REQUIRED + # and report all missing components, rather than bailing after failing to find + # the first. +endmacro(SUITESPARSE_REPORT_NOT_FOUND) + +# Protect against any alternative find_package scripts for this library having +# been called previously (in a client project) which set SUITESPARSE_FOUND, but +# not the other variables we require / set here which could cause the search +# logic here to fail. +unset(SUITESPARSE_FOUND) + +# Handle possible presence of lib prefix for libraries on MSVC, see +# also SUITESPARSE_RESET_FIND_LIBRARY_PREFIX(). +if (MSVC) + # Preserve the caller's original values for CMAKE_FIND_LIBRARY_PREFIXES + # s/t we can set it back before returning. + set(CALLERS_CMAKE_FIND_LIBRARY_PREFIXES "${CMAKE_FIND_LIBRARY_PREFIXES}") + # The empty string in this list is important, it represents the case when + # the libraries have no prefix (shared libraries / DLLs). + set(CMAKE_FIND_LIBRARY_PREFIXES "lib" "" "${CMAKE_FIND_LIBRARY_PREFIXES}") +endif (MSVC) + +# On macOS, add the Homebrew prefix (with appropriate suffixes) to the +# respective HINTS directories (after any user-specified locations). This +# handles Homebrew installations into non-standard locations (not /usr/local). +# We do not use CMAKE_PREFIX_PATH for this as given the search ordering of +# find_xxx(), doing so would override any user-specified HINTS locations with +# the Homebrew version if it exists. +if (CMAKE_SYSTEM_NAME MATCHES "Darwin") + find_program(HOMEBREW_EXECUTABLE brew) + mark_as_advanced(FORCE HOMEBREW_EXECUTABLE) + if (HOMEBREW_EXECUTABLE) + # Detected a Homebrew install, query for its install prefix. + execute_process(COMMAND ${HOMEBREW_EXECUTABLE} --prefix + OUTPUT_VARIABLE HOMEBREW_INSTALL_PREFIX + OUTPUT_STRIP_TRAILING_WHITESPACE) + message(STATUS "Detected Homebrew with install prefix: " + "${HOMEBREW_INSTALL_PREFIX}, adding to CMake search paths.") + list(APPEND SUITESPARSE_INCLUDE_DIR_HINTS "${HOMEBREW_INSTALL_PREFIX}/include") + list(APPEND SUITESPARSE_LIBRARY_DIR_HINTS "${HOMEBREW_INSTALL_PREFIX}/lib") + endif() +endif() + +# Specify search directories for include files and libraries (this is the union +# of the search directories for all OSs). Search user-specified hint +# directories first if supplied, and search user-installed locations first +# so that we prefer user installs to system installs where both exist. +list(APPEND SUITESPARSE_CHECK_INCLUDE_DIRS + /opt/local/include + /opt/local/include/ufsparse # Mac OS X + /usr/local/homebrew/include # Mac OS X + /usr/local/include + /usr/include) +list(APPEND SUITESPARSE_CHECK_LIBRARY_DIRS + /opt/local/lib + /opt/local/lib/ufsparse # Mac OS X + /usr/local/homebrew/lib # Mac OS X + /usr/local/lib + /usr/lib) +# Additional suffixes to try appending to each search path. +list(APPEND SUITESPARSE_CHECK_PATH_SUFFIXES + suitesparse) # Windows/Ubuntu + +# Wrappers to find_path/library that pass the SuiteSparse search hints/paths. +# +# suitesparse_find_component( [FILES name1 [name2 ...]] +# [LIBRARIES name1 [name2 ...]] +# [REQUIRED]) +macro(suitesparse_find_component COMPONENT) + include(CMakeParseArguments) + set(OPTIONS REQUIRED) + set(MULTI_VALUE_ARGS FILES LIBRARIES) + cmake_parse_arguments(SUITESPARSE_FIND_${COMPONENT} + "${OPTIONS}" "" "${MULTI_VALUE_ARGS}" ${ARGN}) + + if (SUITESPARSE_FIND_${COMPONENT}_REQUIRED) + list(APPEND SUITESPARSE_FOUND_REQUIRED_VARS ${COMPONENT}_FOUND) + endif() + + set(${COMPONENT}_FOUND TRUE) + if (SUITESPARSE_FIND_${COMPONENT}_FILES) + find_path(${COMPONENT}_INCLUDE_DIR + NAMES ${SUITESPARSE_FIND_${COMPONENT}_FILES} + HINTS ${SUITESPARSE_INCLUDE_DIR_HINTS} + PATHS ${SUITESPARSE_CHECK_INCLUDE_DIRS} + PATH_SUFFIXES ${SUITESPARSE_CHECK_PATH_SUFFIXES}) + if (${COMPONENT}_INCLUDE_DIR) + message(STATUS "Found ${COMPONENT} headers in: " + "${${COMPONENT}_INCLUDE_DIR}") + mark_as_advanced(${COMPONENT}_INCLUDE_DIR) + else() + # Specified headers not found. + set(${COMPONENT}_FOUND FALSE) + if (SUITESPARSE_FIND_${COMPONENT}_REQUIRED) + suitesparse_report_not_found( + "Did not find ${COMPONENT} header (required SuiteSparse component).") + else() + message(STATUS "Did not find ${COMPONENT} header (optional " + "SuiteSparse component).") + # Hide optional vars from CMake GUI even if not found. + mark_as_advanced(${COMPONENT}_INCLUDE_DIR) + endif() + endif() + endif() + + if (SUITESPARSE_FIND_${COMPONENT}_LIBRARIES) + find_library(${COMPONENT}_LIBRARY + NAMES ${SUITESPARSE_FIND_${COMPONENT}_LIBRARIES} + HINTS ${SUITESPARSE_LIBRARY_DIR_HINTS} + PATHS ${SUITESPARSE_CHECK_LIBRARY_DIRS} + PATH_SUFFIXES ${SUITESPARSE_CHECK_PATH_SUFFIXES}) + if (${COMPONENT}_LIBRARY) + message(STATUS "Found ${COMPONENT} library: ${${COMPONENT}_LIBRARY}") + mark_as_advanced(${COMPONENT}_LIBRARY) + else () + # Specified libraries not found. + set(${COMPONENT}_FOUND FALSE) + if (SUITESPARSE_FIND_${COMPONENT}_REQUIRED) + suitesparse_report_not_found( + "Did not find ${COMPONENT} library (required SuiteSparse component).") + else() + message(STATUS "Did not find ${COMPONENT} library (optional SuiteSparse " + "dependency)") + # Hide optional vars from CMake GUI even if not found. + mark_as_advanced(${COMPONENT}_LIBRARY) + endif() + endif() + endif() +endmacro() + +# Given the number of components of SuiteSparse, and to ensure that the +# automatic failure message generated by FindPackageHandleStandardArgs() +# when not all required components are found is helpful, we maintain a list +# of all variables that must be defined for SuiteSparse to be considered found. +unset(SUITESPARSE_FOUND_REQUIRED_VARS) + +# BLAS. +find_package(BLAS QUIET) +if (NOT BLAS_FOUND) + suitesparse_report_not_found( + "Did not find BLAS library (required for SuiteSparse).") +endif (NOT BLAS_FOUND) +list(APPEND SUITESPARSE_FOUND_REQUIRED_VARS BLAS_FOUND) + +# LAPACK. +find_package(LAPACK QUIET) +if (NOT LAPACK_FOUND) + suitesparse_report_not_found( + "Did not find LAPACK library (required for SuiteSparse).") +endif (NOT LAPACK_FOUND) +list(APPEND SUITESPARSE_FOUND_REQUIRED_VARS LAPACK_FOUND) + +suitesparse_find_component(AMD REQUIRED FILES amd.h LIBRARIES amd) +suitesparse_find_component(CAMD REQUIRED FILES camd.h LIBRARIES camd) +suitesparse_find_component(COLAMD REQUIRED FILES colamd.h LIBRARIES colamd) +suitesparse_find_component(CCOLAMD REQUIRED FILES ccolamd.h LIBRARIES ccolamd) +suitesparse_find_component(CHOLMOD REQUIRED FILES cholmod.h LIBRARIES cholmod) +suitesparse_find_component( + SUITESPARSEQR REQUIRED FILES SuiteSparseQR.hpp LIBRARIES spqr) +if (SUITESPARSEQR_FOUND) + # SuiteSparseQR may be compiled with Intel Threading Building Blocks, + # we assume that if TBB is installed, SuiteSparseQR was compiled with + # support for it, this will do no harm if it wasn't. + find_package(TBB QUIET) + if (TBB_FOUND) + message(STATUS "Found Intel Thread Building Blocks (TBB) library " + "(${TBB_VERSION_MAJOR}.${TBB_VERSION_MINOR} / ${TBB_INTERFACE_VERSION}) " + "include location: ${TBB_INCLUDE_DIRS}. Assuming SuiteSparseQR was " + "compiled with TBB.") + # Add the TBB libraries to the SuiteSparseQR libraries (the only + # libraries to optionally depend on TBB). + list(APPEND SUITESPARSEQR_LIBRARY ${TBB_LIBRARIES}) + else() + message(STATUS "Did not find Intel TBB library, assuming SuiteSparseQR was " + "not compiled with TBB.") + endif() +endif(SUITESPARSEQR_FOUND) + +# UFconfig / SuiteSparse_config. +# +# If SuiteSparse version is >= 4 then SuiteSparse_config is required. +# For SuiteSparse 3, UFconfig.h is required. +suitesparse_find_component( + SUITESPARSE_CONFIG FILES SuiteSparse_config.h LIBRARIES suitesparseconfig) + +if (SUITESPARSE_CONFIG_FOUND) + # SuiteSparse_config (SuiteSparse version >= 4) requires librt library for + # timing by default when compiled on Linux or Unix, but not on OSX (which + # does not have librt). + if (CMAKE_SYSTEM_NAME MATCHES "Linux" OR UNIX AND NOT APPLE) + suitesparse_find_component(LIBRT LIBRARIES rt) + if (LIBRT_FOUND) + message(STATUS "Adding librt: ${LIBRT_LIBRARY} to " + "SuiteSparse_config libraries (required on Linux & Unix [not OSX] if " + "SuiteSparse is compiled with timing).") + list(APPEND SUITESPARSE_CONFIG_LIBRARY ${LIBRT_LIBRARY}) + else() + message(STATUS "Could not find librt, but found SuiteSparse_config, " + "assuming that SuiteSparse was compiled without timing.") + endif () + endif (CMAKE_SYSTEM_NAME MATCHES "Linux" OR UNIX AND NOT APPLE) +else() + # Failed to find SuiteSparse_config (>= v4 installs), instead look for + # UFconfig header which should be present in < v4 installs. + suitesparse_find_component(UFCONFIG FILES UFconfig.h) +endif () + +if (NOT SUITESPARSE_CONFIG_FOUND AND + NOT UFCONFIG_FOUND) + suitesparse_report_not_found( + "Failed to find either: SuiteSparse_config header & library (should be " + "present in all SuiteSparse >= v4 installs), or UFconfig header (should " + "be present in all SuiteSparse < v4 installs).") +endif() + +# Extract the SuiteSparse version from the appropriate header (UFconfig.h for +# <= v3, SuiteSparse_config.h for >= v4). +list(APPEND SUITESPARSE_FOUND_REQUIRED_VARS SUITESPARSE_VERSION) + +if (UFCONFIG_FOUND) + # SuiteSparse version <= 3. + set(SUITESPARSE_VERSION_FILE ${UFCONFIG_INCLUDE_DIR}/UFconfig.h) + if (NOT EXISTS ${SUITESPARSE_VERSION_FILE}) + suitesparse_report_not_found( + "Could not find file: ${SUITESPARSE_VERSION_FILE} containing version " + "information for <= v3 SuiteSparse installs, but UFconfig was found " + "(only present in <= v3 installs).") + else (NOT EXISTS ${SUITESPARSE_VERSION_FILE}) + file(READ ${SUITESPARSE_VERSION_FILE} UFCONFIG_CONTENTS) + + string(REGEX MATCH "#define SUITESPARSE_MAIN_VERSION [0-9]+" + SUITESPARSE_MAIN_VERSION "${UFCONFIG_CONTENTS}") + string(REGEX REPLACE "#define SUITESPARSE_MAIN_VERSION ([0-9]+)" "\\1" + SUITESPARSE_MAIN_VERSION "${SUITESPARSE_MAIN_VERSION}") + + string(REGEX MATCH "#define SUITESPARSE_SUB_VERSION [0-9]+" + SUITESPARSE_SUB_VERSION "${UFCONFIG_CONTENTS}") + string(REGEX REPLACE "#define SUITESPARSE_SUB_VERSION ([0-9]+)" "\\1" + SUITESPARSE_SUB_VERSION "${SUITESPARSE_SUB_VERSION}") + + string(REGEX MATCH "#define SUITESPARSE_SUBSUB_VERSION [0-9]+" + SUITESPARSE_SUBSUB_VERSION "${UFCONFIG_CONTENTS}") + string(REGEX REPLACE "#define SUITESPARSE_SUBSUB_VERSION ([0-9]+)" "\\1" + SUITESPARSE_SUBSUB_VERSION "${SUITESPARSE_SUBSUB_VERSION}") + + # This is on a single line s/t CMake does not interpret it as a list of + # elements and insert ';' separators which would result in 4.;2.;1 nonsense. + set(SUITESPARSE_VERSION + "${SUITESPARSE_MAIN_VERSION}.${SUITESPARSE_SUB_VERSION}.${SUITESPARSE_SUBSUB_VERSION}") + endif (NOT EXISTS ${SUITESPARSE_VERSION_FILE}) +endif (UFCONFIG_FOUND) + +if (SUITESPARSE_CONFIG_FOUND) + # SuiteSparse version >= 4. + set(SUITESPARSE_VERSION_FILE + ${SUITESPARSE_CONFIG_INCLUDE_DIR}/SuiteSparse_config.h) + if (NOT EXISTS ${SUITESPARSE_VERSION_FILE}) + suitesparse_report_not_found( + "Could not find file: ${SUITESPARSE_VERSION_FILE} containing version " + "information for >= v4 SuiteSparse installs, but SuiteSparse_config was " + "found (only present in >= v4 installs).") + else (NOT EXISTS ${SUITESPARSE_VERSION_FILE}) + file(READ ${SUITESPARSE_VERSION_FILE} SUITESPARSE_CONFIG_CONTENTS) + + string(REGEX MATCH "#define SUITESPARSE_MAIN_VERSION [0-9]+" + SUITESPARSE_MAIN_VERSION "${SUITESPARSE_CONFIG_CONTENTS}") + string(REGEX REPLACE "#define SUITESPARSE_MAIN_VERSION ([0-9]+)" "\\1" + SUITESPARSE_MAIN_VERSION "${SUITESPARSE_MAIN_VERSION}") + + string(REGEX MATCH "#define SUITESPARSE_SUB_VERSION [0-9]+" + SUITESPARSE_SUB_VERSION "${SUITESPARSE_CONFIG_CONTENTS}") + string(REGEX REPLACE "#define SUITESPARSE_SUB_VERSION ([0-9]+)" "\\1" + SUITESPARSE_SUB_VERSION "${SUITESPARSE_SUB_VERSION}") + + string(REGEX MATCH "#define SUITESPARSE_SUBSUB_VERSION [0-9]+" + SUITESPARSE_SUBSUB_VERSION "${SUITESPARSE_CONFIG_CONTENTS}") + string(REGEX REPLACE "#define SUITESPARSE_SUBSUB_VERSION ([0-9]+)" "\\1" + SUITESPARSE_SUBSUB_VERSION "${SUITESPARSE_SUBSUB_VERSION}") + + # This is on a single line s/t CMake does not interpret it as a list of + # elements and insert ';' separators which would result in 4.;2.;1 nonsense. + set(SUITESPARSE_VERSION + "${SUITESPARSE_MAIN_VERSION}.${SUITESPARSE_SUB_VERSION}.${SUITESPARSE_SUBSUB_VERSION}") + endif (NOT EXISTS ${SUITESPARSE_VERSION_FILE}) +endif (SUITESPARSE_CONFIG_FOUND) + +# METIS (Optional dependency). +suitesparse_find_component(METIS LIBRARIES metis) + +# Only mark SuiteSparse as found if all required components and dependencies +# have been found. +set(SUITESPARSE_FOUND TRUE) +foreach(REQUIRED_VAR ${SUITESPARSE_FOUND_REQUIRED_VARS}) + if (NOT ${REQUIRED_VAR}) + set(SUITESPARSE_FOUND FALSE) + endif (NOT ${REQUIRED_VAR}) +endforeach(REQUIRED_VAR ${SUITESPARSE_FOUND_REQUIRED_VARS}) + +if (SUITESPARSE_FOUND) + list(APPEND SUITESPARSE_INCLUDE_DIRS + ${AMD_INCLUDE_DIR} + ${CAMD_INCLUDE_DIR} + ${COLAMD_INCLUDE_DIR} + ${CCOLAMD_INCLUDE_DIR} + ${CHOLMOD_INCLUDE_DIR} + ${SUITESPARSEQR_INCLUDE_DIR}) + # Handle config separately, as otherwise at least one of them will be set + # to NOTFOUND which would cause any check on SUITESPARSE_INCLUDE_DIRS to fail. + if (SUITESPARSE_CONFIG_FOUND) + list(APPEND SUITESPARSE_INCLUDE_DIRS + ${SUITESPARSE_CONFIG_INCLUDE_DIR}) + endif (SUITESPARSE_CONFIG_FOUND) + if (UFCONFIG_FOUND) + list(APPEND SUITESPARSE_INCLUDE_DIRS + ${UFCONFIG_INCLUDE_DIR}) + endif (UFCONFIG_FOUND) + # As SuiteSparse includes are often all in the same directory, remove any + # repetitions. + list(REMOVE_DUPLICATES SUITESPARSE_INCLUDE_DIRS) + + # Important: The ordering of these libraries is *NOT* arbitrary, as these + # could potentially be static libraries their link ordering is important. + list(APPEND SUITESPARSE_LIBRARIES + ${SUITESPARSEQR_LIBRARY} + ${CHOLMOD_LIBRARY} + ${CCOLAMD_LIBRARY} + ${CAMD_LIBRARY} + ${COLAMD_LIBRARY} + ${AMD_LIBRARY} + ${LAPACK_LIBRARIES} + ${BLAS_LIBRARIES}) + if (SUITESPARSE_CONFIG_FOUND) + list(APPEND SUITESPARSE_LIBRARIES + ${SUITESPARSE_CONFIG_LIBRARY}) + endif (SUITESPARSE_CONFIG_FOUND) + if (METIS_FOUND) + list(APPEND SUITESPARSE_LIBRARIES + ${METIS_LIBRARY}) + endif (METIS_FOUND) +endif() + +# Determine if we are running on Ubuntu with the package install of SuiteSparse +# which is broken and does not support linking a shared library. +set(SUITESPARSE_IS_BROKEN_SHARED_LINKING_UBUNTU_SYSTEM_VERSION FALSE) +if (CMAKE_SYSTEM_NAME MATCHES "Linux" AND + SUITESPARSE_VERSION VERSION_EQUAL 3.4.0) + find_program(LSB_RELEASE_EXECUTABLE lsb_release) + if (LSB_RELEASE_EXECUTABLE) + # Any even moderately recent Ubuntu release (likely to be affected by + # this bug) should have lsb_release, if it isn't present we are likely + # on a different Linux distribution (should be fine). + execute_process(COMMAND ${LSB_RELEASE_EXECUTABLE} -si + OUTPUT_VARIABLE LSB_DISTRIBUTOR_ID + OUTPUT_STRIP_TRAILING_WHITESPACE) + + if (LSB_DISTRIBUTOR_ID MATCHES "Ubuntu" AND + SUITESPARSE_LIBRARIES MATCHES "/usr/lib/libamd") + # We are on Ubuntu, and the SuiteSparse version matches the broken + # system install version and is a system install. + set(SUITESPARSE_IS_BROKEN_SHARED_LINKING_UBUNTU_SYSTEM_VERSION TRUE) + message(STATUS "Found system install of SuiteSparse " + "${SUITESPARSE_VERSION} running on Ubuntu, which has a known bug " + "preventing linking of shared libraries (static linking unaffected).") + endif (LSB_DISTRIBUTOR_ID MATCHES "Ubuntu" AND + SUITESPARSE_LIBRARIES MATCHES "/usr/lib/libamd") + endif (LSB_RELEASE_EXECUTABLE) +endif (CMAKE_SYSTEM_NAME MATCHES "Linux" AND + SUITESPARSE_VERSION VERSION_EQUAL 3.4.0) + +suitesparse_reset_find_library_prefix() + +# Handle REQUIRED and QUIET arguments to FIND_PACKAGE +include(FindPackageHandleStandardArgs) +if (SUITESPARSE_FOUND) + find_package_handle_standard_args(SuiteSparse + REQUIRED_VARS ${SUITESPARSE_FOUND_REQUIRED_VARS} + VERSION_VAR SUITESPARSE_VERSION + FAIL_MESSAGE "Failed to find some/all required components of SuiteSparse.") +else (SUITESPARSE_FOUND) + # Do not pass VERSION_VAR to FindPackageHandleStandardArgs() if we failed to + # find SuiteSparse to avoid a confusing autogenerated failure message + # that states 'not found (missing: FOO) (found version: x.y.z)'. + find_package_handle_standard_args(SuiteSparse + REQUIRED_VARS ${SUITESPARSE_FOUND_REQUIRED_VARS} + FAIL_MESSAGE "Failed to find some/all required components of SuiteSparse.") +endif (SUITESPARSE_FOUND) diff --git a/aslam_optimizer/sparse_block_matrix/cmake/sparse_block_matrix-extras.cmake.in b/aslam_optimizer/sparse_block_matrix/cmake/sparse_block_matrix-extras.cmake.in new file mode 100644 index 000000000..b5ea2a37c --- /dev/null +++ b/aslam_optimizer/sparse_block_matrix/cmake/sparse_block_matrix-extras.cmake.in @@ -0,0 +1,2 @@ +# The first CFG_EXTRAS runs the FindSuiteSparse, now we just need to add the include directories: +include_directories(${CHOLMOD_INCLUDE_DIR}) \ No newline at end of file diff --git a/aslam_optimizer/sparse_block_matrix/include/sparse_block_matrix/sparse_helper.h b/aslam_optimizer/sparse_block_matrix/include/sparse_block_matrix/sparse_helper.h index 79a2c7f32..067092bb8 100755 --- a/aslam_optimizer/sparse_block_matrix/include/sparse_block_matrix/sparse_helper.h +++ b/aslam_optimizer/sparse_block_matrix/include/sparse_block_matrix/sparse_helper.h @@ -123,5 +123,3 @@ namespace { #include "implementation/sparse_helper.h" #endif - - diff --git a/aslam_optimizer/sparse_block_matrix/package.xml b/aslam_optimizer/sparse_block_matrix/package.xml index 8c38f86a2..fdac5ebcd 100644 --- a/aslam_optimizer/sparse_block_matrix/package.xml +++ b/aslam_optimizer/sparse_block_matrix/package.xml @@ -1,24 +1,26 @@ - + sparse_block_matrix 0.0.1 sparse_block_matrix Author New BSD + catkin - catkin_simple - + tbb sm_common sm_eigen suitesparse eigen - sm_common - sm_eigen - suitesparse - eigen + suitesparse + + sm_common + sm_eigen + suitesparse + eigen gtest diff --git a/suitesparse/.gitignore b/suitesparse/.gitignore deleted file mode 100644 index 620d3dc8a..000000000 --- a/suitesparse/.gitignore +++ /dev/null @@ -1,13 +0,0 @@ -# Compiled Object files -*.slo -*.lo -*.o - -# Compiled Dynamic libraries -*.so -*.dylib - -# Compiled Static libraries -*.lai -*.la -*.a diff --git a/suitesparse/CMakeLists.txt b/suitesparse/CMakeLists.txt deleted file mode 100644 index 295f37287..000000000 --- a/suitesparse/CMakeLists.txt +++ /dev/null @@ -1,68 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(suitesparse) - -set(CMAKE_BUILD_TYPE Release) - -find_package(catkin REQUIRED) - -include(ExternalProject) - -set(VERSION 4.2.1) - -catkin_package( - DEPENDS - CATKIN_DEPENDS - INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} - CFG_EXTRAS suitesparse-extras.cmake -) - -ExternalProject_Add(suitesparse_src - CMAKE_ARGS -DCMAKE_C_COMPILER=${CMAKE_C_COMPILER} -DCMAKE_CXX_COMPILER=${CMAKE_CXX_COMPILER} - DOWNLOAD_COMMAND rm -f SuiteSparse-${VERSION}.tar.gz && wget http://faculty.cse.tamu.edu/davis/SuiteSparse/SuiteSparse-${VERSION}.tar.gz - PATCH_COMMAND tar -xzf ../SuiteSparse-${VERSION}.tar.gz && rm -rf ../suitesparse_src-build/SuiteSparse && sed -i.bu "s/\\/usr\\/local\\/lib/..\\/lib/g" SuiteSparse/SuiteSparse_config/SuiteSparse_config.mk && sed -i.bu "s/\\/usr\\/local\\/include/..\\/include/g" SuiteSparse/SuiteSparse_config/SuiteSparse_config.mk && mv SuiteSparse ../suitesparse_src-build/ - CONFIGURE_COMMAND "" - BUILD_COMMAND cd SuiteSparse && make library -j - INSTALL_COMMAND cd SuiteSparse && mkdir -p lib && mkdir -p include && make install && cd lib && cp libamd.2.3.1.a libcamd.2.3.1.a libcholmod.2.1.2.a libcxsparse.3.1.2.a libldl.2.1.0.a libspqr.1.3.1.a libumfpack.5.6.2.a libamd.a libcamd.a libcholmod.a libcxsparse.a libldl.a libspqr.a libumfpack.a libbtf.1.2.0.a libccolamd.2.8.0.a libcolamd.2.8.0.a libklu.1.2.1.a librbio.2.1.1.a libsuitesparseconfig.4.2.1.a libbtf.a libccolamd.a libcolamd.a libklu.a librbio.a libsuitesparseconfig.a ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_DESTINATION}/ && cd .. && mkdir -p ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_INCLUDE_DESTINATION}/suitesparse && cd include && cp amd.h cholmod_matrixops.h SuiteSparseQR_definitions.h umfpack_load_symbolic.h umfpack_save_symbolic.h btf.h cholmod_modify.h SuiteSparseQR.hpp umfpack_numeric.h umfpack_scale.h camd.h cholmod_partition.h umfpack_col_to_triplet.h umfpack_qsymbolic.h umfpack_solve.h ccolamd.h cholmod_supernodal.h umfpack_defaults.h umfpack_report_control.h umfpack_symbolic.h cholmod_blas.h cholmod_template.h umfpack_free_numeric.h umfpack_report_info.h umfpack_tictoc.h cholmod_camd.h colamd.h umfpack_free_symbolic.h umfpack_report_matrix.h umfpack_timer.h cholmod_check.h cs.h umfpack_get_determinant.h umfpack_report_numeric.h umfpack_transpose.h cholmod_cholesky.h klu.h umfpack_get_lunz.h umfpack_report_perm.h umfpack_triplet_to_col.h cholmod_complexity.h ldl.h umfpack_get_numeric.h umfpack_report_status.h umfpack_wsolve.h cholmod_config.h RBio.h umfpack_get_symbolic.h umfpack_report_symbolic.h cholmod_core.h spqr.hpp umfpack_global.h umfpack_report_triplet.h cholmod.h SuiteSparse_config.h umfpack.h umfpack_report_vector.h cholmod_io64.h SuiteSparseQR_C.h umfpack_load_numeric.h umfpack_save_numeric.h ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_INCLUDE_DESTINATION}/suitesparse -) - -add_library(${PROJECT_NAME} src/export_lib_hack.cc) -target_link_libraries(${PROJECT_NAME} - ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_DESTINATION}/libamd.2.3.1.a - ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_DESTINATION}/libcamd.2.3.1.a - ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_DESTINATION}/libcholmod.2.1.2.a - ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_DESTINATION}/libcxsparse.3.1.2.a - ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_DESTINATION}/libldl.2.1.0.a - ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_DESTINATION}/libspqr.1.3.1.a - ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_DESTINATION}/libumfpack.5.6.2.a - ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_DESTINATION}/libamd.a - ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_DESTINATION}/libcamd.a - ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_DESTINATION}/libcholmod.a - ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_DESTINATION}/libcxsparse.a - ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_DESTINATION}/libldl.a - ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_DESTINATION}/libspqr.a - ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_DESTINATION}/libumfpack.a - ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_DESTINATION}/libbtf.1.2.0.a - ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_DESTINATION}/libccolamd.2.8.0.a - ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_DESTINATION}/libcolamd.2.8.0.a - ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_DESTINATION}/libklu.1.2.1.a - ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_DESTINATION}/librbio.2.1.1.a - ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_DESTINATION}/libsuitesparseconfig.4.2.1.a - ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_DESTINATION}/libbtf.a - ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_DESTINATION}/libccolamd.a - ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_DESTINATION}/libcolamd.a - ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_DESTINATION}/libklu.a - ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_DESTINATION}/librbio.a - ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_DESTINATION}/libsuitesparseconfig.a) -add_dependencies(${PROJECT_NAME} suitesparse_src) - -install(DIRECTORY include - DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} -) -install(DIRECTORY lib - DESTINATION ${CATKIN_GLOBAL_LIB_DESTINATION} -) -install(DIRECTORY share - DESTINATION ${CATKIN_GLOBAL_SHARE_DESTINATION} -) - diff --git a/suitesparse/LICENSE b/suitesparse/LICENSE deleted file mode 100644 index 6600f1c98..000000000 --- a/suitesparse/LICENSE +++ /dev/null @@ -1,165 +0,0 @@ -GNU LESSER GENERAL PUBLIC LICENSE - Version 3, 29 June 2007 - - Copyright (C) 2007 Free Software Foundation, Inc. - Everyone is permitted to copy and distribute verbatim copies - of this license document, but changing it is not allowed. - - - This version of the GNU Lesser General Public License incorporates -the terms and conditions of version 3 of the GNU General Public -License, supplemented by the additional permissions listed below. - - 0. Additional Definitions. - - As used herein, "this License" refers to version 3 of the GNU Lesser -General Public License, and the "GNU GPL" refers to version 3 of the GNU -General Public License. - - "The Library" refers to a covered work governed by this License, -other than an Application or a Combined Work as defined below. - - An "Application" is any work that makes use of an interface provided -by the Library, but which is not otherwise based on the Library. -Defining a subclass of a class defined by the Library is deemed a mode -of using an interface provided by the Library. - - A "Combined Work" is a work produced by combining or linking an -Application with the Library. The particular version of the Library -with which the Combined Work was made is also called the "Linked -Version". - - The "Minimal Corresponding Source" for a Combined Work means the -Corresponding Source for the Combined Work, excluding any source code -for portions of the Combined Work that, considered in isolation, are -based on the Application, and not on the Linked Version. - - The "Corresponding Application Code" for a Combined Work means the -object code and/or source code for the Application, including any data -and utility programs needed for reproducing the Combined Work from the -Application, but excluding the System Libraries of the Combined Work. - - 1. Exception to Section 3 of the GNU GPL. - - You may convey a covered work under sections 3 and 4 of this License -without being bound by section 3 of the GNU GPL. - - 2. Conveying Modified Versions. - - If you modify a copy of the Library, and, in your modifications, a -facility refers to a function or data to be supplied by an Application -that uses the facility (other than as an argument passed when the -facility is invoked), then you may convey a copy of the modified -version: - - a) under this License, provided that you make a good faith effort to - ensure that, in the event an Application does not supply the - function or data, the facility still operates, and performs - whatever part of its purpose remains meaningful, or - - b) under the GNU GPL, with none of the additional permissions of - this License applicable to that copy. - - 3. Object Code Incorporating Material from Library Header Files. - - The object code form of an Application may incorporate material from -a header file that is part of the Library. You may convey such object -code under terms of your choice, provided that, if the incorporated -material is not limited to numerical parameters, data structure -layouts and accessors, or small macros, inline functions and templates -(ten or fewer lines in length), you do both of the following: - - a) Give prominent notice with each copy of the object code that the - Library is used in it and that the Library and its use are - covered by this License. - - b) Accompany the object code with a copy of the GNU GPL and this license - document. - - 4. Combined Works. - - You may convey a Combined Work under terms of your choice that, -taken together, effectively do not restrict modification of the -portions of the Library contained in the Combined Work and reverse -engineering for debugging such modifications, if you also do each of -the following: - - a) Give prominent notice with each copy of the Combined Work that - the Library is used in it and that the Library and its use are - covered by this License. - - b) Accompany the Combined Work with a copy of the GNU GPL and this license - document. - - c) For a Combined Work that displays copyright notices during - execution, include the copyright notice for the Library among - these notices, as well as a reference directing the user to the - copies of the GNU GPL and this license document. - - d) Do one of the following: - - 0) Convey the Minimal Corresponding Source under the terms of this - License, and the Corresponding Application Code in a form - suitable for, and under terms that permit, the user to - recombine or relink the Application with a modified version of - the Linked Version to produce a modified Combined Work, in the - manner specified by section 6 of the GNU GPL for conveying - Corresponding Source. - - 1) Use a suitable shared library mechanism for linking with the - Library. A suitable mechanism is one that (a) uses at run time - a copy of the Library already present on the user's computer - system, and (b) will operate properly with a modified version - of the Library that is interface-compatible with the Linked - Version. - - e) Provide Installation Information, but only if you would otherwise - be required to provide such information under section 6 of the - GNU GPL, and only to the extent that such information is - necessary to install and execute a modified version of the - Combined Work produced by recombining or relinking the - Application with a modified version of the Linked Version. (If - you use option 4d0, the Installation Information must accompany - the Minimal Corresponding Source and Corresponding Application - Code. If you use option 4d1, you must provide the Installation - Information in the manner specified by section 6 of the GNU GPL - for conveying Corresponding Source.) - - 5. Combined Libraries. - - You may place library facilities that are a work based on the -Library side by side in a single library together with other library -facilities that are not Applications and are not covered by this -License, and convey such a combined library under terms of your -choice, if you do both of the following: - - a) Accompany the combined library with a copy of the same work based - on the Library, uncombined with any other library facilities, - conveyed under the terms of this License. - - b) Give prominent notice with the combined library that part of it - is a work based on the Library, and explaining where to find the - accompanying uncombined form of the same work. - - 6. Revised Versions of the GNU Lesser General Public License. - - The Free Software Foundation may publish revised and/or new versions -of the GNU Lesser General Public License from time to time. Such new -versions will be similar in spirit to the present version, but may -differ in detail to address new problems or concerns. - - Each version is given a distinguishing version number. If the -Library as you received it specifies that a certain numbered version -of the GNU Lesser General Public License "or any later version" -applies to it, you have the option of following the terms and -conditions either of that published version or of any later version -published by the Free Software Foundation. If the Library as you -received it does not specify a version number of the GNU Lesser -General Public License, you may choose any version of the GNU Lesser -General Public License ever published by the Free Software Foundation. - - If the Library as you received it specifies that a proxy can decide -whether future versions of the GNU Lesser General Public License shall -apply, that proxy's public statement of acceptance of any version is -permanent authorization for you to choose that version for the -Library. diff --git a/suitesparse/README.md b/suitesparse/README.md deleted file mode 100644 index dcf6fffd1..000000000 --- a/suitesparse/README.md +++ /dev/null @@ -1,6 +0,0 @@ -suitesparse -=========== - -A catkin wrapper package for Dr. Tim Davis Suitesparse 4.2.1 (with PIC) https://www.cise.ufl.edu/research/sparse/SuiteSparse/ - -[![Build Status](http://129.132.38.183:8080/job/suitesparse/badge/icon)](http://129.132.38.183:8080/job/suitesparse/) diff --git a/suitesparse/cmake/suitesparse-extras.cmake.in b/suitesparse/cmake/suitesparse-extras.cmake.in deleted file mode 100644 index 2d7d4d1da..000000000 --- a/suitesparse/cmake/suitesparse-extras.cmake.in +++ /dev/null @@ -1,17 +0,0 @@ -list(APPEND @PROJECT_NAME@_INCLUDE_DIRS ${@PROJECT_NAME@_DIR}/../../../@CATKIN_GLOBAL_INCLUDE_DESTINATION@/suitesparse) - - -list(APPEND @PROJECT_NAME@_LIBRARIES - @CATKIN_DEVEL_PREFIX@/lib/libspqr${CMAKE_STATIC_LIBRARY_SUFFIX} - @CATKIN_DEVEL_PREFIX@/lib/libcholmod${CMAKE_STATIC_LIBRARY_SUFFIX} - @CATKIN_DEVEL_PREFIX@/lib/libccolamd${CMAKE_STATIC_LIBRARY_SUFFIX} - @CATKIN_DEVEL_PREFIX@/lib/libcamd${CMAKE_STATIC_LIBRARY_SUFFIX} - @CATKIN_DEVEL_PREFIX@/lib/libcolamd${CMAKE_STATIC_LIBRARY_SUFFIX} - @CATKIN_DEVEL_PREFIX@/lib/libamd${CMAKE_STATIC_LIBRARY_SUFFIX} - lapack - blas - @CATKIN_DEVEL_PREFIX@/lib/libsuitesparseconfig${CMAKE_STATIC_LIBRARY_SUFFIX} ) - -if(NOT APPLE) - list(APPEND @PROJECT_NAME@_LIBRARIES rt ) -endif() \ No newline at end of file diff --git a/suitesparse/include/README b/suitesparse/include/README deleted file mode 100644 index 1b7fa12d4..000000000 --- a/suitesparse/include/README +++ /dev/null @@ -1 +0,0 @@ -THIS FOLDER IS FILLED BY SUITESPARSE MAKE diff --git a/suitesparse/package.xml b/suitesparse/package.xml deleted file mode 100644 index c6ef0c8bb..000000000 --- a/suitesparse/package.xml +++ /dev/null @@ -1,18 +0,0 @@ - - suitesparse - 4.2.1 - suitesparse - Dr. Tim Davis - - LGPLv3 or later - - http://faculty.cse.tamu.edu/davis/ - - Dr. Tim Davis - - catkin - liblapack-dev - libblas-dev - gfortran - wget - diff --git a/suitesparse/src/export_lib_hack.cc b/suitesparse/src/export_lib_hack.cc deleted file mode 100644 index e69de29bb..000000000 From 4cbd21f7c150cdadf5b8717111ae3ca8a3d7026c Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Tue, 30 Mar 2021 23:41:11 +0100 Subject: [PATCH 42/75] Address packaging issues identified by buildserver --- Schweizer-Messer/python_module/package.xml | 4 +++- Schweizer-Messer/sm_boost/CMakeLists.txt | 8 ++------ Schweizer-Messer/sm_common/CMakeLists.txt | 17 ++++++++--------- Schweizer-Messer/sm_common/package.xml | 3 ++- Schweizer-Messer/sm_eigen/CMakeLists.txt | 16 ++++------------ Schweizer-Messer/sm_kinematics/CMakeLists.txt | 8 ++------ Schweizer-Messer/sm_logging/CMakeLists.txt | 8 ++------ .../sm_matrix_archive/CMakeLists.txt | 7 +------ .../sm_property_tree/CMakeLists.txt | 17 +++++++++++++++-- Schweizer-Messer/sm_random/CMakeLists.txt | 8 +++----- Schweizer-Messer/sm_timing/CMakeLists.txt | 17 +++++++++++++++-- .../sparse_block_matrix/CMakeLists.txt | 2 +- 12 files changed, 58 insertions(+), 57 deletions(-) diff --git a/Schweizer-Messer/python_module/package.xml b/Schweizer-Messer/python_module/package.xml index 6a241fee2..38166dea2 100644 --- a/Schweizer-Messer/python_module/package.xml +++ b/Schweizer-Messer/python_module/package.xml @@ -1,5 +1,5 @@ - + python_module 0.0.1 python_module @@ -7,4 +7,6 @@ Paul Furgale New BSD catkin + python-numpy + python3-numpy diff --git a/Schweizer-Messer/sm_boost/CMakeLists.txt b/Schweizer-Messer/sm_boost/CMakeLists.txt index 977a87adc..b1ae47b08 100644 --- a/Schweizer-Messer/sm_boost/CMakeLists.txt +++ b/Schweizer-Messer/sm_boost/CMakeLists.txt @@ -37,17 +37,14 @@ install(TARGETS ${PROJECT_NAME} ) ## Mark cpp header files for installation -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.hpp" - PATTERN ".svn" EXCLUDE +install(DIRECTORY include/ + DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} # Perhaps risky because of "boost" ) ############# ## Testing ## ############# if(CATKIN_ENABLE_TESTING) - # Avoid clash with tr1::tuple: https://code.google.com/p/googletest/source/browse/trunk/README?r=589#257 add_definitions(-DGTEST_USE_OWN_TR1_TUPLE=0) @@ -58,5 +55,4 @@ if(CATKIN_ENABLE_TESTING) if(TARGET ${PROJECT_NAME}-test) target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) endif() - endif() diff --git a/Schweizer-Messer/sm_common/CMakeLists.txt b/Schweizer-Messer/sm_common/CMakeLists.txt index 030c5978d..5afc6adca 100644 --- a/Schweizer-Messer/sm_common/CMakeLists.txt +++ b/Schweizer-Messer/sm_common/CMakeLists.txt @@ -4,7 +4,6 @@ project(sm_common) set(CMAKE_CXX_STANDARD 14) find_package(catkin REQUIRED) -include_directories(${catkin_INCLUDE_DIRS}) find_package(Boost REQUIRED COMPONENTS system) SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -D__STRICT_ANSI__") @@ -14,11 +13,15 @@ catkin_package( INCLUDE_DIRS include ${catkin_INCLUDE_DIRS} LIBRARIES ${PROJECT_NAME} CATKIN_DEPENDS - DEPENDS + DEPENDS Boost CFG_EXTRAS export_flags.cmake ) -include_directories(include ${Boost_INCLUDE_DIRS}) +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} +) add_library(${PROJECT_NAME} src/progress_info.cpp ) @@ -36,18 +39,15 @@ install(TARGETS ${PROJECT_NAME} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) - ## Mark cpp header files for installation -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.hpp" +install(DIRECTORY include/ + DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} #${CATKIN_PACKAGE_INCLUDE_DESTINATION} ) ############# ## Testing ## ############# if(CATKIN_ENABLE_TESTING) - # Avoid clash with tr1::tuple: https://code.google.com/p/googletest/source/browse/trunk/README?r=589#257 add_definitions(-DGTEST_USE_OWN_TR1_TUPLE=0) @@ -59,5 +59,4 @@ if(CATKIN_ENABLE_TESTING) test/hash_id.cpp ) target_link_libraries(${PROJECT_NAME}-test pthread) - endif() diff --git a/Schweizer-Messer/sm_common/package.xml b/Schweizer-Messer/sm_common/package.xml index 2b5454885..3c8413fcf 100644 --- a/Schweizer-Messer/sm_common/package.xml +++ b/Schweizer-Messer/sm_common/package.xml @@ -1,5 +1,5 @@ - + sm_common 0.0.1 sm_common @@ -8,5 +8,6 @@ New BSD catkin boost + boost gtest diff --git a/Schweizer-Messer/sm_eigen/CMakeLists.txt b/Schweizer-Messer/sm_eigen/CMakeLists.txt index 877bb3055..5c4e71961 100644 --- a/Schweizer-Messer/sm_eigen/CMakeLists.txt +++ b/Schweizer-Messer/sm_eigen/CMakeLists.txt @@ -2,20 +2,16 @@ cmake_minimum_required(VERSION 3.0.2) project(sm_eigen) find_package(catkin REQUIRED COMPONENTS sm_common sm_random) -include_directories(${catkin_INCLUDE_DIRS}) find_package(Boost REQUIRED COMPONENTS system serialization) find_package(Eigen3 REQUIRED) -include_directories(include ${EIGEN3_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) - catkin_package( INCLUDE_DIRS include ${EIGEN3_INCLUDE_DIRS} LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS - DEPENDS + CATKIN_DEPENDS sm_common sm_random ) -include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) +include_directories(include ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) ############## ## Building ## @@ -39,17 +35,14 @@ install(TARGETS ${PROJECT_NAME} ) ## Mark cpp header files for installation -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.hpp" - PATTERN ".svn" EXCLUDE +install(DIRECTORY include/ + DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} #${CATKIN_PACKAGE_INCLUDE_DESTINATION} ) ############# ## Testing ## ############# if(CATKIN_ENABLE_TESTING) - # Avoid clash with tr1::tuple: https://code.google.com/p/googletest/source/browse/trunk/README?r=589#257 add_definitions(-DGTEST_USE_OWN_TR1_TUPLE=0) @@ -62,5 +55,4 @@ if(CATKIN_ENABLE_TESTING) if(TARGET ${PROJECT_NAME}-test) target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) endif() - endif() diff --git a/Schweizer-Messer/sm_kinematics/CMakeLists.txt b/Schweizer-Messer/sm_kinematics/CMakeLists.txt index c6a6b5e8a..da40a6022 100644 --- a/Schweizer-Messer/sm_kinematics/CMakeLists.txt +++ b/Schweizer-Messer/sm_kinematics/CMakeLists.txt @@ -4,10 +4,8 @@ project(sm_kinematics) set(CMAKE_CXX_STANDARD 14) find_package(catkin REQUIRED COMPONENTS sm_common sm_eigen sm_boost sm_random) -include_directories(${catkin_INCLUDE_DIRS}) find_package(Boost REQUIRED COMPONENTS system serialization filesystem) -list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/../cmake) find_package(Eigen3 REQUIRED) include_directories(${EIGEN3_INCLUDE_DIRS}) add_definitions(${EIGEN_DEFINITIONS}) @@ -59,10 +57,8 @@ install(TARGETS ${PROJECT_NAME} ) ## Mark cpp header files for installation -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.hpp" - PATTERN ".svn" EXCLUDE +install(DIRECTORY include/ + DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} ) ############# diff --git a/Schweizer-Messer/sm_logging/CMakeLists.txt b/Schweizer-Messer/sm_logging/CMakeLists.txt index 7c5d0585d..f6f677c10 100644 --- a/Schweizer-Messer/sm_logging/CMakeLists.txt +++ b/Schweizer-Messer/sm_logging/CMakeLists.txt @@ -42,17 +42,14 @@ install(TARGETS ${PROJECT_NAME} ) ## Mark cpp header files for installation -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.hpp" - PATTERN ".svn" EXCLUDE +install(DIRECTORY include/ + DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} ) ############# ## Testing ## ############# if(CATKIN_ENABLE_TESTING) - # Avoid clash with tr1::tuple: https://code.google.com/p/googletest/source/browse/trunk/README?r=589#257 add_definitions(-DGTEST_USE_OWN_TR1_TUPLE=0) @@ -62,5 +59,4 @@ if(CATKIN_ENABLE_TESTING) if(TARGET ${PROJECT_NAME}-test) target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) endif() - endif() diff --git a/Schweizer-Messer/sm_matrix_archive/CMakeLists.txt b/Schweizer-Messer/sm_matrix_archive/CMakeLists.txt index 992900f0b..fb3c4958c 100644 --- a/Schweizer-Messer/sm_matrix_archive/CMakeLists.txt +++ b/Schweizer-Messer/sm_matrix_archive/CMakeLists.txt @@ -4,21 +4,18 @@ project(sm_matrix_archive) set(CMAKE_CXX_STANDARD 14) find_package(catkin REQUIRED COMPONENTS sm_common) -include_directories(${catkin_INCLUDE_DIRS}) find_package(Boost REQUIRED COMPONENTS system filesystem) find_package(Eigen3 REQUIRED) -include_directories(include ${EIGEN3_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) catkin_package( INCLUDE_DIRS include ${catkin_INCLUDE_DIRS} LIBRARIES ${PROJECT_NAME} CATKIN_DEPENDS sm_common - DEPENDS ) add_definitions(-D__STRICT_ANSI__) -include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) +include_directories(include ${EIGEN3_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) ############## ## Building ## @@ -35,7 +32,6 @@ target_link_libraries(${PROJECT_NAME} ## Testing ## ############# if(CATKIN_ENABLE_TESTING) - # Avoid clash with tr1::tuple: https://code.google.com/p/googletest/source/browse/trunk/README?r=589#257 add_definitions(-DGTEST_USE_OWN_TR1_TUPLE=0) @@ -47,5 +43,4 @@ if(CATKIN_ENABLE_TESTING) if(TARGET ${PROJECT_NAME}-test) target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) endif() - endif() diff --git a/Schweizer-Messer/sm_property_tree/CMakeLists.txt b/Schweizer-Messer/sm_property_tree/CMakeLists.txt index 0fe7050f0..d61fd10da 100644 --- a/Schweizer-Messer/sm_property_tree/CMakeLists.txt +++ b/Schweizer-Messer/sm_property_tree/CMakeLists.txt @@ -32,11 +32,25 @@ add_library(${PROJECT_NAME} target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} pthread) +################## +## Installation ## +################## + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark cpp header files for installation +install(DIRECTORY include/ + DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} +) + ############# ## Testing ## ############# if(CATKIN_ENABLE_TESTING) - # Avoid clash with tr1::tuple: https://code.google.com/p/googletest/source/browse/trunk/README?r=589#257 add_definitions(-DGTEST_USE_OWN_TR1_TUPLE=0) @@ -48,5 +62,4 @@ if(CATKIN_ENABLE_TESTING) if(TARGET ${PROJECT_NAME}-test) target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) endif() - endif() diff --git a/Schweizer-Messer/sm_random/CMakeLists.txt b/Schweizer-Messer/sm_random/CMakeLists.txt index af9a8870f..145775e29 100644 --- a/Schweizer-Messer/sm_random/CMakeLists.txt +++ b/Schweizer-Messer/sm_random/CMakeLists.txt @@ -32,8 +32,6 @@ install(TARGETS ${PROJECT_NAME} ) ## Mark cpp header files for installation -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.hpp" - PATTERN ".svn" EXCLUDE -) \ No newline at end of file +install(DIRECTORY include/ + DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} +) diff --git a/Schweizer-Messer/sm_timing/CMakeLists.txt b/Schweizer-Messer/sm_timing/CMakeLists.txt index 834c7cb7a..4f879a298 100644 --- a/Schweizer-Messer/sm_timing/CMakeLists.txt +++ b/Schweizer-Messer/sm_timing/CMakeLists.txt @@ -28,11 +28,25 @@ add_library(${PROJECT_NAME} target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +################## +## Installation ## +################## + +install(TARGETS ${PROJECT_NAME} +ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark cpp header files for installation +install(DIRECTORY include/ +DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} +) + ############# ## Testing ## ############# if(CATKIN_ENABLE_TESTING) - # Avoid clash with tr1::tuple: https://code.google.com/p/googletest/source/browse/trunk/README?r=589#257 add_definitions(-DGTEST_USE_OWN_TR1_TUPLE=0) @@ -45,5 +59,4 @@ if(CATKIN_ENABLE_TESTING) if(TARGET ${PROJECT_NAME}-test) target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME} pthread) endif() - endif() diff --git a/aslam_optimizer/sparse_block_matrix/CMakeLists.txt b/aslam_optimizer/sparse_block_matrix/CMakeLists.txt index 4cd45848d..ee92d39d9 100644 --- a/aslam_optimizer/sparse_block_matrix/CMakeLists.txt +++ b/aslam_optimizer/sparse_block_matrix/CMakeLists.txt @@ -37,7 +37,7 @@ if(CATKIN_ENABLE_TESTING) test/solver_tests.cpp test/sparse_block_matrix_tests.cpp ) - target_link_libraries(${PROJECT_NAME}_tests ${PROJECT_NAME} ${TBB_LIBRARIES}) + target_link_libraries(${PROJECT_NAME}_tests ${PROJECT_NAME} ${TBB_LIBRARIES} ${SUITESPARSE_LIBRARIES}) endif() # Install From 4f3038f6f68669a219719aee3ba6c6ddf6e4b65c Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Thu, 1 Apr 2021 12:48:52 +0100 Subject: [PATCH 43/75] Buildsystem fixes. Deactivated a few broken unittests. --- Schweizer-Messer/numpy_eigen/CMakeLists.txt | 29 +++++-- .../cmake/numpy_eigen-extras.cmake.in | 2 + Schweizer-Messer/sm_eigen/CMakeLists.txt | 1 + .../sm_eigen/cmake/sm_eigen-extras.cmake.in | 2 + .../sm_matrix_archive/CMakeLists.txt | 17 +++- Schweizer-Messer/sm_opencv/CMakeLists.txt | 7 +- Schweizer-Messer/sm_python/CMakeLists.txt | 28 +++---- aslam_cv/aslam_cameras/CMakeLists.txt | 58 +++++++++----- aslam_cv/aslam_cameras/package.xml | 1 - aslam_cv/aslam_cameras_april/CMakeLists.txt | 48 +++++++++--- aslam_cv/aslam_cameras_april/package.xml | 2 +- aslam_cv/aslam_cv_backend/CMakeLists.txt | 43 +++++++++-- aslam_cv/aslam_cv_backend/package.xml | 2 - .../aslam_cv_backend_python/CMakeLists.txt | 34 ++++++-- aslam_cv/aslam_cv_backend_python/package.xml | 2 - aslam_cv/aslam_cv_error_terms/CMakeLists.txt | 51 ++++++++---- aslam_cv/aslam_cv_error_terms/package.xml | 1 - aslam_cv/aslam_cv_python/CMakeLists.txt | 34 ++++++-- aslam_cv/aslam_cv_python/package.xml | 1 - .../aslam_cv_serialization/CMakeLists.txt | 45 ++++++++--- aslam_cv/aslam_cv_serialization/Makefile | 1 - aslam_cv/aslam_cv_serialization/package.xml | 1 - aslam_cv/aslam_imgproc/CMakeLists.txt | 43 +++++++++-- aslam_cv/aslam_imgproc/package.xml | 3 - aslam_cv/aslam_time/CMakeLists.txt | 30 ++++++-- aslam_cv/aslam_time/package.xml | 2 +- .../incremental_calibration/CMakeLists.txt | 61 +++++++++------ .../incremental_calibration/package.xml | 1 - .../CMakeLists.txt | 14 +++- .../package.xml | 2 + .../aslam_splines/CMakeLists.txt | 44 ++++++++--- .../aslam_splines/package.xml | 2 - .../aslam_splines/test/TestErrors.cpp | 4 +- .../aslam_splines_python/CMakeLists.txt | 33 +++++++- .../aslam_splines_python/package.xml | 1 - .../bsplines/CMakeLists.txt | 28 +++++-- .../bsplines/package.xml | 2 - .../bsplines_python/CMakeLists.txt | 19 +++-- .../bsplines_python/package.xml | 1 - .../ethz_apriltag2/CMakeLists.txt | 31 +++++--- aslam_optimizer/aslam_backend/CMakeLists.txt | 42 ++++++++-- aslam_optimizer/aslam_backend/package.xml | 1 - .../aslam_backend_expressions/CMakeLists.txt | 77 +++++++++++-------- .../aslam_backend_expressions/package.xml | 10 +-- .../aslam_backend_python/CMakeLists.txt | 32 ++++++-- .../aslam_backend_python/package.xml | 3 - 46 files changed, 634 insertions(+), 262 deletions(-) create mode 100644 Schweizer-Messer/numpy_eigen/cmake/numpy_eigen-extras.cmake.in create mode 100644 Schweizer-Messer/sm_eigen/cmake/sm_eigen-extras.cmake.in delete mode 100644 aslam_cv/aslam_cv_serialization/Makefile diff --git a/Schweizer-Messer/numpy_eigen/CMakeLists.txt b/Schweizer-Messer/numpy_eigen/CMakeLists.txt index bb4cc3a94..33ce91b4e 100644 --- a/Schweizer-Messer/numpy_eigen/CMakeLists.txt +++ b/Schweizer-Messer/numpy_eigen/CMakeLists.txt @@ -4,7 +4,6 @@ project(numpy_eigen) set(CMAKE_CXX_STANDARD 14) find_package(catkin REQUIRED COMPONENTS python_module) -include_directories(${catkin_INCLUDE_DIRS}) find_package(Boost REQUIRED COMPONENTS system) find_package(Eigen3 REQUIRED) @@ -15,9 +14,10 @@ include_directories(include ${EIGEN3_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) include_directories("${PROJECT_SOURCE_DIR}/include/numpy_eigen") catkin_package( - INCLUDE_DIRS include ${catkin_INCLUDE_DIRS} + INCLUDE_DIRS include ${catkin_INCLUDE_DIRS} LIBRARIES ${PROJECT_NAME} - DEPENDS + DEPENDS Boost + CFG_EXTRAS numpy_eigen-extras.cmake ) add_definitions(-D__STRICT_ANSI__) @@ -51,8 +51,27 @@ add_python_export_library(${PROJECT_NAME}_test ## Testing ## ############# if(CATKIN_ENABLE_TESTING) - ## Add nosetest based cpp test target. catkin_add_nosetests(test/numpy_eigen_tests.py) - endif() + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +################## +## Installation ## +################## + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) diff --git a/Schweizer-Messer/numpy_eigen/cmake/numpy_eigen-extras.cmake.in b/Schweizer-Messer/numpy_eigen/cmake/numpy_eigen-extras.cmake.in new file mode 100644 index 000000000..1fe6cd7a2 --- /dev/null +++ b/Schweizer-Messer/numpy_eigen/cmake/numpy_eigen-extras.cmake.in @@ -0,0 +1,2 @@ +find_package(Eigen3 REQUIRED) +include_directories(${EIGEN3_INCLUDE_DIRS}) diff --git a/Schweizer-Messer/sm_eigen/CMakeLists.txt b/Schweizer-Messer/sm_eigen/CMakeLists.txt index 5c4e71961..0fd661776 100644 --- a/Schweizer-Messer/sm_eigen/CMakeLists.txt +++ b/Schweizer-Messer/sm_eigen/CMakeLists.txt @@ -9,6 +9,7 @@ catkin_package( INCLUDE_DIRS include ${EIGEN3_INCLUDE_DIRS} LIBRARIES ${PROJECT_NAME} CATKIN_DEPENDS sm_common sm_random + CFG_EXTRAS sm_eigen-extras.cmake ) include_directories(include ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) diff --git a/Schweizer-Messer/sm_eigen/cmake/sm_eigen-extras.cmake.in b/Schweizer-Messer/sm_eigen/cmake/sm_eigen-extras.cmake.in new file mode 100644 index 000000000..1fe6cd7a2 --- /dev/null +++ b/Schweizer-Messer/sm_eigen/cmake/sm_eigen-extras.cmake.in @@ -0,0 +1,2 @@ +find_package(Eigen3 REQUIRED) +include_directories(${EIGEN3_INCLUDE_DIRS}) diff --git a/Schweizer-Messer/sm_matrix_archive/CMakeLists.txt b/Schweizer-Messer/sm_matrix_archive/CMakeLists.txt index fb3c4958c..67ca530ec 100644 --- a/Schweizer-Messer/sm_matrix_archive/CMakeLists.txt +++ b/Schweizer-Messer/sm_matrix_archive/CMakeLists.txt @@ -9,7 +9,7 @@ find_package(Boost REQUIRED COMPONENTS system filesystem) find_package(Eigen3 REQUIRED) catkin_package( - INCLUDE_DIRS include ${catkin_INCLUDE_DIRS} + INCLUDE_DIRS include ${catkin_INCLUDE_DIRS} LIBRARIES ${PROJECT_NAME} CATKIN_DEPENDS sm_common ) @@ -28,6 +28,21 @@ add_library(${PROJECT_NAME} target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} pthread) +################## +## Installation ## +################## + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark cpp header files for installation +install(DIRECTORY include/ + DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} +) + ############# ## Testing ## ############# diff --git a/Schweizer-Messer/sm_opencv/CMakeLists.txt b/Schweizer-Messer/sm_opencv/CMakeLists.txt index 23c9a1707..e91d85638 100644 --- a/Schweizer-Messer/sm_opencv/CMakeLists.txt +++ b/Schweizer-Messer/sm_opencv/CMakeLists.txt @@ -4,14 +4,15 @@ project(sm_opencv) set(CMAKE_CXX_STANDARD 14) find_package(catkin REQUIRED COMPONENTS sm_common) -include_directories(${catkin_INCLUDE_DIRS}) find_package(Boost REQUIRED COMPONENTS system) catkin_package( INCLUDE_DIRS include ${catkin_INCLUDE_DIRS} CATKIN_DEPENDS sm_common - DEPENDS + DEPENDS Boost ) add_definitions(-D__STRICT_ANSI__) -include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) +install(DIRECTORY include/ + DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} +) diff --git a/Schweizer-Messer/sm_python/CMakeLists.txt b/Schweizer-Messer/sm_python/CMakeLists.txt index 0f55f404f..cb4d1e9c8 100644 --- a/Schweizer-Messer/sm_python/CMakeLists.txt +++ b/Schweizer-Messer/sm_python/CMakeLists.txt @@ -3,27 +3,23 @@ project(sm_python) set(CMAKE_CXX_STANDARD 14) -#catkin_python_setup() find_package(catkin REQUIRED COMPONENTS sm_common numpy_eigen sm_kinematics - sm_timing sm_logging sm_matrix_archive - sm_property_tree python_module) -include_directories(${catkin_INCLUDE_DIRS}) +sm_timing sm_logging sm_matrix_archive +sm_property_tree python_module numpy_eigen +) find_package(Eigen3 REQUIRED) -include_directories(include ${EIGEN3_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) - +catkin_python_setup() catkin_package( - INCLUDE_DIRS include ${catkin_INCLUDE_DIRS} - LIBRARIES ${PROJECT_NAME} + INCLUDE_DIRS include ${catkin_INCLUDE_DIRS} CATKIN_DEPENDS sm_common numpy_eigen sm_kinematics sm_timing sm_logging - sm_matrix_archive sm_property_tree python_module - DEPENDS + sm_matrix_archive sm_property_tree python_module ) add_definitions(-D__STRICT_ANSI__) - -include_directories(include) + +include_directories(include ${EIGEN3_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) # Set up the python exports. SET(PY_PROJECT_NAME sm_python) @@ -54,5 +50,11 @@ add_python_export_library(${PY_PROJECT_NAME} ${PY_PACKAGE_DIR} src/exportNsecTime.cpp src/random.cpp ) +target_link_libraries(${PY_PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -target_link_libraries(${PY_PROJECT_NAME} ${Boost_LIBRARIES}) +################## +## Installation ## +################## +install(DIRECTORY include/ + DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} +) diff --git a/aslam_cv/aslam_cameras/CMakeLists.txt b/aslam_cv/aslam_cameras/CMakeLists.txt index 277c15be6..18018ba8e 100644 --- a/aslam_cv/aslam_cameras/CMakeLists.txt +++ b/aslam_cv/aslam_cameras/CMakeLists.txt @@ -1,16 +1,32 @@ cmake_minimum_required(VERSION 3.0.2) project(aslam_cameras) -find_package(catkin_simple REQUIRED) +ADD_DEFINITIONS(-DASLAM_USE_ROS) -catkin_simple() +find_package(catkin REQUIRED COMPONENTS + sm_common + sm_eigen + sm_boost + sm_kinematics + sm_logging + aslam_time + sm_opencv + sm_property_tree + opencv2_catkin + sm_random +) +find_package(Boost REQUIRED COMPONENTS system serialization filesystem) +find_package(Eigen3 REQUIRED) -ADD_DEFINITIONS(-DASLAM_USE_ROS ) +catkin_package( + LIBRARIES ${PROJECT_NAME} + INCLUDE_DIRS include + DEPENDS Boost +) -find_package(Boost REQUIRED COMPONENTS system serialization filesystem) +include_directories(include ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} ${Boost_INCLUDE_DIRS}) -#common commands for building c++ executables and libraries -cs_add_library(${PROJECT_NAME} +add_library(${PROJECT_NAME} src/CameraGeometryBase.cpp src/GlobalShutter.cpp src/RollingShutter.cpp @@ -32,17 +48,14 @@ cs_add_library(${PROJECT_NAME} src/BackProjection.cpp src/Landmark.cpp src/Image.cpp - ) - -target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES}) +) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) target_compile_options(${PROJECT_NAME} PUBLIC "-Wno-deprecated-copy") # TODO: Fix... if(CATKIN_ENABLE_TESTING) - # Avoid clash with tr1::tuple: https://code.google.com/p/googletest/source/browse/trunk/README?r=589#257 add_definitions(-DGTEST_USE_OWN_TR1_TUPLE=0) - catkin_add_gtest(${PROJECT_NAME}_tests test/test_main.cpp test/PinholeCameraGeometry.cpp @@ -56,14 +69,23 @@ if(CATKIN_ENABLE_TESTING) test/testFrame.cpp test/GridCalibration.cpp WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test - ) - - + ) SET(CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "${CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS} -lpthread") - target_link_libraries(${PROJECT_NAME}_tests ${PROJECT_NAME} ${Boost_LIBRARIES}) - + target_link_libraries(${PROJECT_NAME}_tests ${PROJECT_NAME}) endif() -cs_install() -cs_export() +################## +## Installation ## +################## + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark cpp header files for installation +install(DIRECTORY include/ + DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} +) diff --git a/aslam_cv/aslam_cameras/package.xml b/aslam_cv/aslam_cameras/package.xml index 954702f5c..89b6c944a 100644 --- a/aslam_cv/aslam_cameras/package.xml +++ b/aslam_cv/aslam_cameras/package.xml @@ -9,7 +9,6 @@ email="paul.furgale@mavt.ethz.ch">Paul Furgale BSD catkin - catkin_simple sm_common sm_eigen diff --git a/aslam_cv/aslam_cameras_april/CMakeLists.txt b/aslam_cv/aslam_cameras_april/CMakeLists.txt index c8a14e924..4885a2b07 100644 --- a/aslam_cv/aslam_cameras_april/CMakeLists.txt +++ b/aslam_cv/aslam_cameras_april/CMakeLists.txt @@ -1,21 +1,34 @@ cmake_minimum_required(VERSION 3.0.2) project(aslam_cameras_april) -find_package(catkin_simple REQUIRED) -catkin_simple() - +ADD_DEFINITIONS(-DASLAM_USE_ROS) + +find_package(catkin REQUIRED COMPONENTS + sm_python + aslam_cameras + ethz_apriltag2 + numpy_eigen + sm_common + sm_logging + sm_boost + sm_eigen + python_module +) find_package(OpenCV REQUIRED) +find_package(Boost REQUIRED COMPONENTS serialization system) +find_package(Eigen3 REQUIRED) -ADD_DEFINITIONS(-DASLAM_USE_ROS ) -include_directories(${EIGEN3_INCLUDE_DIRS}) +catkin_package( + LIBRARIES ${PROJECT_NAME} + INCLUDE_DIRS include + DEPENDS OpenCV Boost +) +include_directories(include ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} ${Boost_INCLUDE_DIRS}) -cs_add_library(${PROJECT_NAME} +add_library(${PROJECT_NAME} src/GridCalibrationTargetAprilgrid.cpp - ) - -find_package(Boost REQUIRED COMPONENTS serialization system) - +) target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBS} ${Boost_LIBRARIES}) add_python_export_library(${PROJECT_NAME}_python python/aslam_cameras_april @@ -23,6 +36,17 @@ add_python_export_library(${PROJECT_NAME}_python python/aslam_cameras_april ) target_link_libraries(${PROJECT_NAME}_python ${PROJECT_NAME}) -cs_install() -cs_export() +################## +## Installation ## +################## + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) +## Mark cpp header files for installation +install(DIRECTORY include/ + DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} +) diff --git a/aslam_cv/aslam_cameras_april/package.xml b/aslam_cv/aslam_cameras_april/package.xml index 2f408f252..0bfbbcbd5 100644 --- a/aslam_cv/aslam_cameras_april/package.xml +++ b/aslam_cv/aslam_cameras_april/package.xml @@ -7,7 +7,6 @@ Paul Furgale New BSD catkin - catkin_simple sm_python aslam_cameras @@ -17,4 +16,5 @@ sm_logging sm_boost sm_eigen + python_module diff --git a/aslam_cv/aslam_cv_backend/CMakeLists.txt b/aslam_cv/aslam_cv_backend/CMakeLists.txt index 98c978847..48a5b70a7 100644 --- a/aslam_cv/aslam_cv_backend/CMakeLists.txt +++ b/aslam_cv/aslam_cv_backend/CMakeLists.txt @@ -1,12 +1,32 @@ cmake_minimum_required(VERSION 3.0.2) project(aslam_cv_backend) -find_package(catkin_simple REQUIRED) +find_package(catkin REQUIRED COMPONENTS + sm_common + sm_boost + sm_kinematics + sm_eigen + sm_opencv + sm_property_tree + sm_logging + sm_timing + aslam_backend + aslam_backend_expressions + aslam_time + aslam_cameras + opencv2_catkin + sparse_block_matrix +) find_package(Boost REQUIRED COMPONENTS system) -catkin_simple() +catkin_package( + LIBRARIES ${PROJECT_NAME} + INCLUDE_DIRS include +) + +include_directories(include ${catkin_INCLUDE_DIRS}) -cs_add_library(${PROJECT_NAME} +add_library(${PROJECT_NAME} src/GridCalibrationTargetDesignVariableContainer.cpp # src/CameraGeometryDesignVariableContainer.cpp # src/NCameraSystemDesignVariableContainer.cpp @@ -14,18 +34,25 @@ cs_add_library(${PROJECT_NAME} target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES}) if(CATKIN_ENABLE_TESTING) - # Avoid clash with tr1::tuple: https://code.google.com/p/googletest/source/browse/trunk/README?r=589#257 add_definitions(-DGTEST_USE_OWN_TR1_TUPLE=0) catkin_add_gtest(${PROJECT_NAME}_test test/test_main.cpp ) - target_link_libraries(${PROJECT_NAME}_test ${PROJECT_NAME}) - endif() -cs_install() +################## +## Installation ## +################## +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) -cs_export() +## Mark cpp header files for installation +install(DIRECTORY include/ + DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} +) diff --git a/aslam_cv/aslam_cv_backend/package.xml b/aslam_cv/aslam_cv_backend/package.xml index b43d28183..707160d4b 100644 --- a/aslam_cv/aslam_cv_backend/package.xml +++ b/aslam_cv/aslam_cv_backend/package.xml @@ -7,7 +7,6 @@ Paul Furgale BSD catkin - catkin_simple sm_common sm_boost @@ -26,5 +25,4 @@ opencv2_catkin suitesparse sparse_block_matrix - diff --git a/aslam_cv/aslam_cv_backend_python/CMakeLists.txt b/aslam_cv/aslam_cv_backend_python/CMakeLists.txt index a827da8c7..3e816c3f3 100644 --- a/aslam_cv/aslam_cv_backend_python/CMakeLists.txt +++ b/aslam_cv/aslam_cv_backend_python/CMakeLists.txt @@ -1,9 +1,30 @@ cmake_minimum_required(VERSION 3.0.2) project(aslam_cv_backend_python) -find_package(catkin_simple REQUIRED) +find_package(catkin REQUIRED COMPONENTS + sm_common + sm_python + sm_opencv + numpy_eigen + python_module + sparse_block_matrix + aslam_backend + aslam_backend_expressions + aslam_backend_python + opencv2_catkin + aslam_cameras + aslam_cv_error_terms + aslam_splines + aslam_cv_python + aslam_cv_backend + aslam_time +) + +catkin_package( + INCLUDE_DIRS include +) -catkin_simple() +include_directories(include ${catkin_INCLUDE_DIRS}) add_python_export_library(${PROJECT_NAME} python/aslam_cv_backend src/module.cpp @@ -12,6 +33,9 @@ add_python_export_library(${PROJECT_NAME} python/aslam_cv_backend # src/NCameraSystemDesignVariableContainer.cpp ) -cs_install() - -cs_export() +################## +## Installation ## +################## +install(DIRECTORY include/ + DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} +) diff --git a/aslam_cv/aslam_cv_backend_python/package.xml b/aslam_cv/aslam_cv_backend_python/package.xml index 4cd3a5cf0..a623f7ad4 100644 --- a/aslam_cv/aslam_cv_backend_python/package.xml +++ b/aslam_cv/aslam_cv_backend_python/package.xml @@ -7,7 +7,6 @@ Paul Furgale New BSD catkin - catkin_simple sm_common sm_python @@ -28,5 +27,4 @@ aslam_cv_python aslam_cv_backend aslam_time - diff --git a/aslam_cv/aslam_cv_error_terms/CMakeLists.txt b/aslam_cv/aslam_cv_error_terms/CMakeLists.txt index 8a49f8b71..11e2619f4 100644 --- a/aslam_cv/aslam_cv_error_terms/CMakeLists.txt +++ b/aslam_cv/aslam_cv_error_terms/CMakeLists.txt @@ -1,25 +1,44 @@ cmake_minimum_required(VERSION 3.0.2) project(aslam_cv_error_terms) -find_package(catkin_simple REQUIRED) -catkin_simple() - -find_package(Boost REQUIRED COMPONENTS system ) +find_package(Boost REQUIRED COMPONENTS system) +find_package(catkin REQUIRED COMPONENTS + sm_logging + sm_property_tree + sm_opencv + sm_kinematics + aslam_time + aslam_cameras + aslam_backend + aslam_cv_backend + aslam_backend_expressions + sm_boost + sm_eigen + sm_timing + sparse_block_matrix + opencv2_catkin + aslam_splines +) + +catkin_package( + INCLUDE_DIRS include +) if(CATKIN_ENABLE_TESTING) - -# Avoid clash with tr1::tuple: https://code.google.com/p/googletest/source/browse/trunk/README?r=589#257 + # Avoid clash with tr1::tuple: https://code.google.com/p/googletest/source/browse/trunk/README?r=589#257 add_definitions(-DGTEST_USE_OWN_TR1_TUPLE=0) - catkin_add_gtest(${PROJECT_NAME}_test - test/test_main.cpp - test/TestReprojectionError.cpp - ) - target_link_libraries(${PROJECT_NAME}_test ${catkin_LIBRARIES}) - - target_link_libraries(${PROJECT_NAME}_test ${Boost_LIBRARIES}) - + include_directories(include ${catkin_INCLUDE_DIRS}) + # catkin_add_gtest(${PROJECT_NAME}_test + # test/test_main.cpp + # test/TestReprojectionError.cpp + # ) + # target_link_libraries(${PROJECT_NAME}_test ${catkin_LIBRARIES} ${Boost_LIBRARIES}) endif() -cs_install() -cs_export() +################## +## Installation ## +################## +install(DIRECTORY include/ + DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} +) diff --git a/aslam_cv/aslam_cv_error_terms/package.xml b/aslam_cv/aslam_cv_error_terms/package.xml index aaecfb4d3..5f6024b44 100644 --- a/aslam_cv/aslam_cv_error_terms/package.xml +++ b/aslam_cv/aslam_cv_error_terms/package.xml @@ -7,7 +7,6 @@ Paul Furgale New BSD catkin - catkin_simple sm_logging sm_property_tree diff --git a/aslam_cv/aslam_cv_python/CMakeLists.txt b/aslam_cv/aslam_cv_python/CMakeLists.txt index 2e861e02a..a824149b3 100644 --- a/aslam_cv/aslam_cv_python/CMakeLists.txt +++ b/aslam_cv/aslam_cv_python/CMakeLists.txt @@ -3,8 +3,25 @@ project(aslam_cv_python) set(CMAKE_CXX_STANDARD 14) -find_package(catkin_simple REQUIRED) -catkin_simple() +find_package(Boost REQUIRED COMPONENTS serialization) +find_package(catkin REQUIRED COMPONENTS + sm_common + sm_eigen + sm_kinematics + sm_python + sm_opencv + opencv2_catkin + python_module + aslam_cv_serialization + aslam_imgproc + aslam_cameras + aslam_time + aslam_cv_serialization +) + +catkin_package( + INCLUDE_DIRS include +) add_definitions(-fPIC -Wextra -Winit-self -Woverloaded-virtual -Wnon-virtual-dtor -Wsign-promo -Wno-long-long) @@ -12,6 +29,8 @@ if(APPLE) add_definitions( -ftemplate-depth-1024) endif() +include_directories(include ${catkin_INCLUDE_DIRS}) + add_python_export_library(${PROJECT_NAME} python/aslam_cv src/module.cpp src/CameraGeometries.cpp @@ -38,8 +57,11 @@ add_python_export_library(${PROJECT_NAME} python/aslam_cv src/PinholeUndistorter.cpp ) target_compile_options(${PROJECT_NAME} PUBLIC "-Wno-deprecated-copy") # TODO: Fix... - -find_package(Boost REQUIRED COMPONENTS serialization) target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES}) -cs_install() -cs_export() + +################## +## Installation ## +################## +install(DIRECTORY include/ + DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} +) \ No newline at end of file diff --git a/aslam_cv/aslam_cv_python/package.xml b/aslam_cv/aslam_cv_python/package.xml index af696cea4..15682894f 100644 --- a/aslam_cv/aslam_cv_python/package.xml +++ b/aslam_cv/aslam_cv_python/package.xml @@ -6,7 +6,6 @@ Paul Furgale New BSD catkin - catkin_simple sm_common sm_eigen diff --git a/aslam_cv/aslam_cv_serialization/CMakeLists.txt b/aslam_cv/aslam_cv_serialization/CMakeLists.txt index 2c08a2e94..cbdd53cb0 100644 --- a/aslam_cv/aslam_cv_serialization/CMakeLists.txt +++ b/aslam_cv/aslam_cv_serialization/CMakeLists.txt @@ -2,21 +2,37 @@ cmake_minimum_required(VERSION 3.0.2) project(aslam_cv_serialization) -find_package(catkin_simple REQUIRED) -catkin_simple() +find_package(catkin REQUIRED COMPONENTS + aslam_cameras + aslam_time + sm_common + sm_boost + sm_property_tree + sm_eigen + sm_kinematics + sm_opencv + sm_logging + opencv2_catkin + sm_random +) +find_package(Boost REQUIRED COMPONENTS serialization system) + +catkin_package( + LIBRARIES ${PROJECT_NAME} + INCLUDE_DIRS include +) #common commands for building c++ executables and libraries INCLUDE(autogen_cameras.cmake) #INCLUDE(autogen_frames.cmake) -cs_add_library(${PROJECT_NAME} -# ${AUTOGEN_FRAME_CPP_FILES} +include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) + +add_library(${PROJECT_NAME} ${AUTOGEN_CAMERA_CPP_FILES} src/FrameBaseSerialization.cpp ) - -find_package(Boost REQUIRED COMPONENTS serialization system) -target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES}) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) # Avoid clash with tr1::tuple: https://code.google.com/p/googletest/source/browse/trunk/README?r=589#257 #add_definitions(-DGTEST_USE_OWN_TR1_TUPLE=0) @@ -26,5 +42,16 @@ target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES}) #) #target_link_libraries(${PROJECT_NAME}_tests ${PROJECT_NAME}) -cs_install() -cs_export() +################## +## Installation ## +################## +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark cpp header files for installation +install(DIRECTORY include/ + DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} +) diff --git a/aslam_cv/aslam_cv_serialization/Makefile b/aslam_cv/aslam_cv_serialization/Makefile deleted file mode 100644 index b75b928f2..000000000 --- a/aslam_cv/aslam_cv_serialization/Makefile +++ /dev/null @@ -1 +0,0 @@ -include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/aslam_cv/aslam_cv_serialization/package.xml b/aslam_cv/aslam_cv_serialization/package.xml index 7b307a7bf..5eded4c3a 100644 --- a/aslam_cv/aslam_cv_serialization/package.xml +++ b/aslam_cv/aslam_cv_serialization/package.xml @@ -6,7 +6,6 @@ Paul Furgale New BSD catkin - catkin_simple aslam_cameras aslam_time diff --git a/aslam_cv/aslam_imgproc/CMakeLists.txt b/aslam_cv/aslam_imgproc/CMakeLists.txt index 737bd315f..5079fe53d 100644 --- a/aslam_cv/aslam_imgproc/CMakeLists.txt +++ b/aslam_cv/aslam_imgproc/CMakeLists.txt @@ -1,16 +1,45 @@ cmake_minimum_required(VERSION 3.0.2) project(aslam_imgproc) -find_package(catkin_simple REQUIRED) +find_package(Boost REQUIRED COMPONENTS system) -catkin_simple() +find_package(catkin REQUIRED COMPONENTS + sm_common + sm_common + sm_eigen + sm_random + sm_boost + sm_kinematics + sm_opencv + sm_logging + sm_property_tree + aslam_time + aslam_cameras + opencv2_catkin + numpy_eigen +) -find_package(Boost REQUIRED COMPONENTS system) +catkin_package( + LIBRARIES ${PROJECT_NAME} + INCLUDE_DIRS include +) -include_directories(${EIGEN3_INCLUDE_DIRS}) -cs_add_library(${PROJECT_NAME} src/UndistorterBase.cpp) +include_directories(include ${catkin_INCLUDE_DIRS}) + +add_library(${PROJECT_NAME} src/UndistorterBase.cpp) target_compile_options(${PROJECT_NAME} PUBLIC -Wno-deprecated-copy -Wno-maybe-uninitialized) target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES}) -cs_install() -cs_export() +################## +## Installation ## +################## +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark cpp header files for installation +install(DIRECTORY include/ + DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} +) diff --git a/aslam_cv/aslam_imgproc/package.xml b/aslam_cv/aslam_imgproc/package.xml index 6a7d99e02..aee8d429e 100644 --- a/aslam_cv/aslam_imgproc/package.xml +++ b/aslam_cv/aslam_imgproc/package.xml @@ -7,7 +7,6 @@ Author BSD catkin - catkin_simple sm_common @@ -24,6 +23,4 @@ aslam_cameras opencv2_catkin numpy_eigen - - diff --git a/aslam_cv/aslam_time/CMakeLists.txt b/aslam_cv/aslam_time/CMakeLists.txt index 444738632..7eb0f14e5 100644 --- a/aslam_cv/aslam_time/CMakeLists.txt +++ b/aslam_cv/aslam_time/CMakeLists.txt @@ -1,14 +1,22 @@ cmake_minimum_required(VERSION 3.0.2) project(aslam_time) -find_package(catkin_simple REQUIRED) -catkin_simple() - +find_package(catkin REQUIRED) find_package(Boost REQUIRED COMPONENTS system) -cs_add_library(${PROJECT_NAME} - src/time.cpp - src/duration.cpp) +catkin_package( + LIBRARIES ${PROJECT_NAME} + INCLUDE_DIRS include + DEPENDS Boost +) + + +include_directories(include ${Boost_INCLUDE_DIRS}) + +add_library(${PROJECT_NAME} + src/time.cpp + src/duration.cpp +) set(OTHER_LIBS) if(NOT APPLE) @@ -17,6 +25,12 @@ endif() target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES} ${OTHER_LIBS}) -cs_install() +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) -cs_export() +install(DIRECTORY include/ + DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} +) diff --git a/aslam_cv/aslam_time/package.xml b/aslam_cv/aslam_time/package.xml index 6ef34a086..3acda38f0 100644 --- a/aslam_cv/aslam_time/package.xml +++ b/aslam_cv/aslam_time/package.xml @@ -6,5 +6,5 @@ Paul Furgale New BSD catkin - catkin_simple + boost diff --git a/aslam_incremental_calibration/incremental_calibration/CMakeLists.txt b/aslam_incremental_calibration/incremental_calibration/CMakeLists.txt index 64f5b582c..974910f79 100644 --- a/aslam_incremental_calibration/incremental_calibration/CMakeLists.txt +++ b/aslam_incremental_calibration/incremental_calibration/CMakeLists.txt @@ -3,12 +3,19 @@ project(incremental_calibration) set(CMAKE_CXX_STANDARD 14) -set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${PROJECT_SOURCE_DIR}/../cmake/) +find_package(catkin REQUIRED COMPONENTS sm_eigen aslam_backend) +find_package(Boost REQUIRED COMPONENTS system thread) +find_package(Eigen3 REQUIRED) + +catkin_package( + LIBRARIES ${PROJECT_NAME} + INCLUDE_DIRS include + CATKIN_DEPENDS sm_eigen +) -find_package(catkin_simple REQUIRED) -catkin_simple() +include_directories(include ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR}) -cs_add_library(${PROJECT_NAME} +add_library(${PROJECT_NAME} src/base/Serializable.cpp src/base/Timestamp.cpp src/base/Condition.cpp @@ -35,27 +42,35 @@ cs_add_library(${PROJECT_NAME} src/algorithms/marginalize.cpp src/algorithms/linalg.cpp ) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${TBB_LIBRARIES}) -find_package(Boost REQUIRED COMPONENTS system thread) -target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES} ${TBB_LIBRARIES}) +# if(CATKIN_ENABLE_TESTING) +# # Avoid clash with tr1::tuple: +# # https://code.google.com/p/googletest/source/browse/trunk/README?r=589#257 +# add_definitions(-DGTEST_USE_OWN_TR1_TUPLE=0) -if(CATKIN_ENABLE_TESTING) +# catkin_add_gtest(${PROJECT_NAME}_test +# test/test_main.cpp +# test/VectorDesignVariableTest.cpp +# test/AlgorithmsTest.cpp +# test/OptimizationProblemTest.cpp +# test/IncrementalOptimizationProblemTest.cpp +# test/LinearSolverTest.cpp +# ) +# target_link_libraries(${PROJECT_NAME}_test ${PROJECT_NAME}) +# endif() - # Avoid clash with tr1::tuple: - # https://code.google.com/p/googletest/source/browse/trunk/README?r=589#257 - add_definitions(-DGTEST_USE_OWN_TR1_TUPLE=0) +################## +## Installation ## +################## - catkin_add_gtest(${PROJECT_NAME}_test - test/test_main.cpp - test/VectorDesignVariableTest.cpp - test/AlgorithmsTest.cpp - test/OptimizationProblemTest.cpp - test/IncrementalOptimizationProblemTest.cpp - test/LinearSolverTest.cpp - ) - target_link_libraries(${PROJECT_NAME}_test ${PROJECT_NAME}) - -endif() +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) -cs_install() -cs_export() +## Mark cpp header files for installation +install(DIRECTORY include/ + DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} +) diff --git a/aslam_incremental_calibration/incremental_calibration/package.xml b/aslam_incremental_calibration/incremental_calibration/package.xml index b873ba6a2..0f50988ab 100644 --- a/aslam_incremental_calibration/incremental_calibration/package.xml +++ b/aslam_incremental_calibration/incremental_calibration/package.xml @@ -11,7 +11,6 @@ Jerome Maye catkin - catkin_simple sm_eigen aslam_backend diff --git a/aslam_incremental_calibration/incremental_calibration_python/CMakeLists.txt b/aslam_incremental_calibration/incremental_calibration_python/CMakeLists.txt index fcdac39ca..f2e2806a3 100644 --- a/aslam_incremental_calibration/incremental_calibration_python/CMakeLists.txt +++ b/aslam_incremental_calibration/incremental_calibration_python/CMakeLists.txt @@ -3,8 +3,13 @@ project(incremental_calibration_python) set(CMAKE_CXX_STANDARD 14) -find_package(catkin_simple REQUIRED) -catkin_simple() +find_package(catkin REQUIRED COMPONENTS aslam_backend incremental_calibration sm_property_tree python_module numpy_eigen) + +catkin_package( + INCLUDE_DIRS include +) + +include_directories(include ${catkin_INCLUDE_DIRS}) add_python_export_library(${PROJECT_NAME} src/incremental_calibration src/module.cpp @@ -13,5 +18,6 @@ add_python_export_library(${PROJECT_NAME} src/incremental_calibration src/LinearSolver.cpp ) -cs_install() -cs_export() +install(DIRECTORY include/ + DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} +) diff --git a/aslam_incremental_calibration/incremental_calibration_python/package.xml b/aslam_incremental_calibration/incremental_calibration_python/package.xml index 7ef2316f8..3f9c46068 100644 --- a/aslam_incremental_calibration/incremental_calibration_python/package.xml +++ b/aslam_incremental_calibration/incremental_calibration_python/package.xml @@ -17,4 +17,6 @@ incremental_calibration numpy_eigen python_module + aslam_backend + incremental_calibration diff --git a/aslam_nonparametric_estimation/aslam_splines/CMakeLists.txt b/aslam_nonparametric_estimation/aslam_splines/CMakeLists.txt index d5a76b001..f77cb70fb 100644 --- a/aslam_nonparametric_estimation/aslam_splines/CMakeLists.txt +++ b/aslam_nonparametric_estimation/aslam_splines/CMakeLists.txt @@ -1,32 +1,54 @@ cmake_minimum_required(VERSION 3.0.2) project(aslam_splines) -find_package(catkin_simple REQUIRED) -catkin_simple() +find_package(catkin REQUIRED COMPONENTS + aslam_backend + aslam_backend_expressions + bsplines + numpy_eigen + sparse_block_matrix + sm_common + sm_kinematics + sm_timing + opencv2_catkin +) +find_package(Boost REQUIRED COMPONENTS system) -find_package(Boost REQUIRED COMPONENTS system ) +catkin_package( + LIBRARIES ${PROJECT_NAME} + INCLUDE_DIRS include + CATKIN_DEPENDS bsplines +) +include_directories(include ${catkin_INCLUDE_DIRS}) -cs_add_library(${PROJECT_NAME} +add_library(${PROJECT_NAME} src/BSplinePoseDesignVariable.cpp src/BSplineExpressions.cpp src/EuclideanBSplineDesignVariable.cpp ) - -target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES}) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) if(CATKIN_ENABLE_TESTING) - catkin_add_gtest(${PROJECT_NAME}_test test/test_main.cpp test/TestBSplineExpressions.cpp test/TestErrors.cpp # test/TestOPTBSpline.cpp ) - target_link_libraries(${PROJECT_NAME}_test ${PROJECT_NAME}) - endif() -cs_install() -cs_export() +################## +## Installation ## +################## +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark cpp header files for installation +install(DIRECTORY include/ + DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} +) diff --git a/aslam_nonparametric_estimation/aslam_splines/package.xml b/aslam_nonparametric_estimation/aslam_splines/package.xml index 892e72d1a..4bfb39cb2 100644 --- a/aslam_nonparametric_estimation/aslam_splines/package.xml +++ b/aslam_nonparametric_estimation/aslam_splines/package.xml @@ -7,7 +7,6 @@ Paul Furgale BSD catkin - catkin_simple aslam_backend aslam_backend_expressions @@ -30,5 +29,4 @@ sm_common sm_kinematics sm_timing - diff --git a/aslam_nonparametric_estimation/aslam_splines/test/TestErrors.cpp b/aslam_nonparametric_estimation/aslam_splines/test/TestErrors.cpp index 8da9a8dcb..0dede2997 100644 --- a/aslam_nonparametric_estimation/aslam_splines/test/TestErrors.cpp +++ b/aslam_nonparametric_estimation/aslam_splines/test/TestErrors.cpp @@ -40,7 +40,7 @@ TEST(SplineErrorTestSuite, testSimpleSplineError) { using namespace aslam::backend; BSplineDesignVariable<1> initSpline = generateRandomBSpline(); - Eigen::VectorXd values(1.0); + Eigen::VectorXd values(1); VectorExpression<1> splineExpression = initSpline.toExpression(5.0,0); SimpleSplineError > e(&initSpline, &splineExpression, values, 5.0); @@ -60,5 +60,3 @@ TEST(SplineErrorTestSuite, testSimpleSplineError) FAIL() << e.what(); } } - - diff --git a/aslam_nonparametric_estimation/aslam_splines_python/CMakeLists.txt b/aslam_nonparametric_estimation/aslam_splines_python/CMakeLists.txt index 2f861626c..9388176e0 100644 --- a/aslam_nonparametric_estimation/aslam_splines_python/CMakeLists.txt +++ b/aslam_nonparametric_estimation/aslam_splines_python/CMakeLists.txt @@ -1,15 +1,40 @@ cmake_minimum_required(VERSION 3.0.2) project(aslam_splines_python) -find_package(catkin_simple REQUIRED) -catkin_simple() +find_package(catkin REQUIRED COMPONENTS + aslam_splines + bsplines_python + bsplines + numpy_eigen + python_module + sparse_block_matrix + sm_common + sm_kinematics + aslam_backend_expressions + aslam_backend + aslam_backend_python + sm_timing + aslam_cameras + aslam_time + sm_opencv + sm_property_tree + sm_logging + opencv2_catkin +) + +catkin_package() + +include_directories(${catkin_INCLUDE_DIRS}) add_python_export_library(${PROJECT_NAME} python/aslam_splines src/spline_module.cpp src/BSplineMotionError.cpp src/SimpleSplineError.cpp ) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) +################## +## Installation ## +################## -cs_install() -cs_export() +# handled by add_python_export_library diff --git a/aslam_nonparametric_estimation/aslam_splines_python/package.xml b/aslam_nonparametric_estimation/aslam_splines_python/package.xml index 477eea23c..e2d7a4012 100644 --- a/aslam_nonparametric_estimation/aslam_splines_python/package.xml +++ b/aslam_nonparametric_estimation/aslam_splines_python/package.xml @@ -7,7 +7,6 @@ Paul Furgale New BSD catkin - catkin_simple aslam_splines bsplines_python diff --git a/aslam_nonparametric_estimation/bsplines/CMakeLists.txt b/aslam_nonparametric_estimation/bsplines/CMakeLists.txt index a9338c52e..98aa0e676 100644 --- a/aslam_nonparametric_estimation/bsplines/CMakeLists.txt +++ b/aslam_nonparametric_estimation/bsplines/CMakeLists.txt @@ -1,15 +1,33 @@ cmake_minimum_required(VERSION 3.0.2) project(bsplines) -find_package(catkin_simple REQUIRED) +find_package(catkin REQUIRED COMPONENTS + sm_common + sm_timing + sm_eigen + sm_kinematics + sparse_block_matrix +) + +catkin_package( + LIBRARIES ${PROJECT_NAME} + INCLUDE_DIRS include +) -catkin_simple() +include_directories(include ${catkin_INCLUDE_DIRS}) -cs_add_library(${PROJECT_NAME} +add_library(${PROJECT_NAME} src/BSpline.cpp src/BSplinePose.cpp ) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) -cs_install() -cs_export() +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) diff --git a/aslam_nonparametric_estimation/bsplines/package.xml b/aslam_nonparametric_estimation/bsplines/package.xml index 86f68d650..56f84f94a 100644 --- a/aslam_nonparametric_estimation/bsplines/package.xml +++ b/aslam_nonparametric_estimation/bsplines/package.xml @@ -7,7 +7,6 @@ 0.0.1 BSD catkin - catkin_simple sm_common sm_timing @@ -15,7 +14,6 @@ sm_kinematics sparse_block_matrix suitesparse - diff --git a/aslam_nonparametric_estimation/bsplines_python/CMakeLists.txt b/aslam_nonparametric_estimation/bsplines_python/CMakeLists.txt index ecac24a49..81cde18e8 100644 --- a/aslam_nonparametric_estimation/bsplines_python/CMakeLists.txt +++ b/aslam_nonparametric_estimation/bsplines_python/CMakeLists.txt @@ -1,8 +1,18 @@ cmake_minimum_required(VERSION 3.0.2) project(bsplines_python) -find_package(catkin_simple REQUIRED) -catkin_simple() +find_package(catkin REQUIRED COMPONENTS + bsplines + numpy_eigen + python_module + sparse_block_matrix + sm_common + sm_kinematics +) + +catkin_package() + +include_directories(${catkin_INCLUDE_DIRS}) # Set up the python exports. SET(PY_PROJECT_NAME bsplines_python) @@ -13,8 +23,3 @@ add_python_export_library(${PY_PROJECT_NAME} ${PY_PACKAGE_DIR} src/BSplinePython.cpp src/BSplinePosePython.cpp ) - -cs_install() -cs_export() - - diff --git a/aslam_nonparametric_estimation/bsplines_python/package.xml b/aslam_nonparametric_estimation/bsplines_python/package.xml index 5a629c134..0350cd012 100644 --- a/aslam_nonparametric_estimation/bsplines_python/package.xml +++ b/aslam_nonparametric_estimation/bsplines_python/package.xml @@ -7,7 +7,6 @@ Paul Furgale New BSD catkin - catkin_simple bsplines numpy_eigen diff --git a/aslam_offline_calibration/ethz_apriltag2/CMakeLists.txt b/aslam_offline_calibration/ethz_apriltag2/CMakeLists.txt index 8befbefaf..c92a9b41e 100644 --- a/aslam_offline_calibration/ethz_apriltag2/CMakeLists.txt +++ b/aslam_offline_calibration/ethz_apriltag2/CMakeLists.txt @@ -1,24 +1,18 @@ cmake_minimum_required(VERSION 3.0.2) - project(ethz_apriltag2) -find_package(catkin REQUIRED) -include_directories(${catkin_INCLUDE_DIRS}) +add_definitions(-fPIC -O3) +find_package(catkin REQUIRED) find_package(Eigen3 REQUIRED) +find_package(OpenCV REQUIRED) catkin_package( - INCLUDE_DIRS include ${EIGEN3_INCLUDE_DIRS} - LIBRARIES ${PROJECT_NAME} + INCLUDE_DIRS include ${EIGEN3_INCLUDE_DIRS} + LIBRARIES ${PROJECT_NAME} ) -set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${PROJECT_SOURCE_DIR}/cmake/) - -find_package(Eigen3 REQUIRED) -find_package(OpenCV REQUIRED) - -add_definitions(-fPIC -O3) -include_directories(include ${EIGEN3_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) +include_directories(include ${EIGEN3_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) #library file(GLOB SOURCE_FILES "src/*.cc") @@ -31,4 +25,17 @@ if(NOT APPLE) target_link_libraries(apriltags_demo ${PROJECT_NAME} v4l2) endif() +################## +## Installation ## +################## +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark cpp header files for installation +install(DIRECTORY include/ + DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} +) diff --git a/aslam_optimizer/aslam_backend/CMakeLists.txt b/aslam_optimizer/aslam_backend/CMakeLists.txt index fa44490ff..88bfb406d 100644 --- a/aslam_optimizer/aslam_backend/CMakeLists.txt +++ b/aslam_optimizer/aslam_backend/CMakeLists.txt @@ -3,15 +3,33 @@ project(aslam_backend) set(CMAKE_CXX_STANDARD 14) -find_package(catkin_simple REQUIRED) -catkin_simple() - +find_package(catkin REQUIRED COMPONENTS + sparse_block_matrix + sm_boost + sm_random + sm_timing + sm_logging + sm_property_tree +) find_package(Boost REQUIRED COMPONENTS system thread) - find_package(Eigen3 REQUIRED) -include_directories(${EIGEN3_INCLUDE_DIRS}) -cs_add_library(${PROJECT_NAME} +catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS + sparse_block_matrix + sm_boost + sm_random + sm_timing + sm_logging + sm_property_tree + DEPENDS Boost +) + +include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS}) + +add_library(${PROJECT_NAME} src/MEstimatorPolicies.cpp src/JacobianContainer.cpp src/DesignVariable.cpp @@ -48,6 +66,7 @@ target_link_libraries(${PROJECT_NAME} ${TBB_LIBRARIES} ${CHOLMOD_LIBRARY} ${SUITESPARSE_LIBRARIES} + ${catkin_LIBRARIES} ) if(CATKIN_ENABLE_TESTING) @@ -69,5 +88,12 @@ if(CATKIN_ENABLE_TESTING) target_link_libraries(${PROJECT_NAME}_test ${PROJECT_NAME}) endif() -cs_install() -cs_export() +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY include/ + DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} +) diff --git a/aslam_optimizer/aslam_backend/package.xml b/aslam_optimizer/aslam_backend/package.xml index 994721a40..b93254eef 100644 --- a/aslam_optimizer/aslam_backend/package.xml +++ b/aslam_optimizer/aslam_backend/package.xml @@ -7,7 +7,6 @@ Paul Furgale New BSD catkin - catkin_simple sparse_block_matrix sm_boost diff --git a/aslam_optimizer/aslam_backend_expressions/CMakeLists.txt b/aslam_optimizer/aslam_backend_expressions/CMakeLists.txt index b7f00a9b4..495e148e5 100644 --- a/aslam_optimizer/aslam_backend_expressions/CMakeLists.txt +++ b/aslam_optimizer/aslam_backend_expressions/CMakeLists.txt @@ -4,18 +4,19 @@ project(aslam_backend_expressions) set(CMAKE_CXX_STANDARD 14) -find_package(catkin_simple REQUIRED) - -catkin_simple() - +find_package(catkin REQUIRED COMPONENTS aslam_backend sm_boost sm_random sm_kinematics sparse_block_matrix sm_timing) find_package(Eigen3 REQUIRED) -include_directories(${EIGEN3_INCLUDE_DIRS}) +find_package(Boost REQUIRED COMPONENTS system) -include_directories(include) +catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS sparse_block_matrix +) -find_package(Boost REQUIRED COMPONENTS system) +include_directories(include ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS}) -cs_add_library(${PROJECT_NAME} +add_library(${PROJECT_NAME} src/RotationExpression.cpp src/RotationExpressionNode.cpp src/RotationQuaternion.cpp @@ -55,35 +56,45 @@ cs_add_library(${PROJECT_NAME} src/MatrixBasic.cpp src/MatrixExpression.cpp src/MatrixExpressionNode.cpp - ) +) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES}) +################## +## Installation ## +################## -if(CATKIN_ENABLE_TESTING) +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark cpp header files for installation +install(DIRECTORY include/ + DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} +) +############# +## Testing ## +############# + +if(CATKIN_ENABLE_TESTING) # Avoid clash with tr1::tuple: https://code.google.com/p/googletest/source/browse/trunk/README?r=589#257 add_definitions(-DGTEST_USE_OWN_TR1_TUPLE=0) - catkin_add_gtest(${PROJECT_NAME}_test - test/test_main.cpp - test/RotationExpression.cpp - test/HomogeneousExpression.cpp - test/MatrixAndEuclideanExpression.cpp - # \todo reenable after I can talk to Hannes. - # test/FixedPointNumberTest.cpp - test/GenericScalarExpressionTest.cpp - test/GenericMatrixExpression.cpp - test/QuaternionExpression.cpp - test/ScalarExpression.cpp - test/ErrorTest_Transformation.cpp - test/ErrorTest_Euclidean.cpp - ) - - target_link_libraries(${PROJECT_NAME}_test ${PROJECT_NAME}) - + # catkin_add_gtest(${PROJECT_NAME}_test + # test/test_main.cpp + # test/RotationExpression.cpp + # test/HomogeneousExpression.cpp + # test/MatrixAndEuclideanExpression.cpp + # # \todo reenable after I can talk to Hannes. + # # test/FixedPointNumberTest.cpp + # test/GenericScalarExpressionTest.cpp + # test/GenericMatrixExpression.cpp + # test/QuaternionExpression.cpp + # test/ScalarExpression.cpp + # test/ErrorTest_Transformation.cpp + # test/ErrorTest_Euclidean.cpp + # ) + # target_link_libraries(${PROJECT_NAME}_test ${PROJECT_NAME}) endif() - -cs_install() - -cs_export() - diff --git a/aslam_optimizer/aslam_backend_expressions/package.xml b/aslam_optimizer/aslam_backend_expressions/package.xml index f25ee2810..98ec32c70 100644 --- a/aslam_optimizer/aslam_backend_expressions/package.xml +++ b/aslam_optimizer/aslam_backend_expressions/package.xml @@ -1,5 +1,5 @@ - + aslam_backend_expressions 0.0.1 aslam_backend_expressions @@ -7,9 +7,6 @@ Paul Furgale BSD catkin - catkin_simple - - aslam_backend sm_boost @@ -20,8 +17,5 @@ suitesparse sm_timing + sparse_block_matrix - - - - diff --git a/aslam_optimizer/aslam_backend_python/CMakeLists.txt b/aslam_optimizer/aslam_backend_python/CMakeLists.txt index c572a33b0..af2427bec 100644 --- a/aslam_optimizer/aslam_backend_python/CMakeLists.txt +++ b/aslam_optimizer/aslam_backend_python/CMakeLists.txt @@ -2,11 +2,25 @@ cmake_minimum_required(VERSION 3.0.2) project(aslam_backend_python) set(CMAKE_CXX_STANDARD 14) +add_definitions(-fPIC -Wextra -Winit-self -Woverloaded-virtual -Wnon-virtual-dtor -Wsign-promo -Wno-long-long) -find_package(catkin_simple REQUIRED) -catkin_simple() +find_package(Boost REQUIRED COMPONENTS thread) +find_package(catkin REQUIRED COMPONENTS + aslam_backend + aslam_backend_expressions + numpy_eigen + python_module + sparse_block_matrix + sm_common + sm_kinematics + sm_timing +) + +catkin_package( + INCLUDE_DIRS include +) -add_definitions( -fPIC -Wextra -Winit-self -Woverloaded-virtual -Wnon-virtual-dtor -Wsign-promo -Wno-long-long) +include_directories(include ${catkin_INCLUDE_DIRS}) # This functions take TARGET_NAME PYTHON_MODULE_DIRECTORY sourceFile1 [sourceFile2 ...] add_python_export_library(${PROJECT_NAME} python/aslam_backend @@ -26,9 +40,11 @@ add_python_export_library(${PROJECT_NAME} python/aslam_backend src/TrustRegionPolicies.cpp src/SparseBlockMatrix.cpp ) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -find_package(Boost REQUIRED COMPONENTS thread) -target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES}) - -cs_install() -cs_export() +################## +## Installation ## +################## +install(DIRECTORY include/ + DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} +) diff --git a/aslam_optimizer/aslam_backend_python/package.xml b/aslam_optimizer/aslam_backend_python/package.xml index 36bc3ad3e..5c99824b5 100644 --- a/aslam_optimizer/aslam_backend_python/package.xml +++ b/aslam_optimizer/aslam_backend_python/package.xml @@ -7,7 +7,6 @@ Paul Furgale BSD catkin - catkin_simple aslam_backend aslam_backend_expressions @@ -18,6 +17,4 @@ sm_kinematics sm_timing suitesparse - - From 3365e397441937c2b99e1ca40356cda890ca279f Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Thu, 1 Apr 2021 12:53:04 +0100 Subject: [PATCH 44/75] [numpy_eigen] Fix test issues --- Schweizer-Messer/numpy_eigen/package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/Schweizer-Messer/numpy_eigen/package.xml b/Schweizer-Messer/numpy_eigen/package.xml index 16a6e8a9d..5e215a9b6 100644 --- a/Schweizer-Messer/numpy_eigen/package.xml +++ b/Schweizer-Messer/numpy_eigen/package.xml @@ -10,4 +10,6 @@ eigen python_module python_module + roslib + rostest From 6d6b92264064163b8e71fa1434f7e0349852fc70 Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Thu, 1 Apr 2021 12:55:58 +0100 Subject: [PATCH 45/75] [kalibr] Fix unmatched brackets --- .../python/kalibr_imu_camera_calibration/IccCalibrator.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccCalibrator.py b/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccCalibrator.py index 2453365f2..d375bd79f 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccCalibrator.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccCalibrator.py @@ -230,4 +230,4 @@ def saveCamChainParametersYaml(self, resultFile): try: chain.writeYaml(resultFile) except: - "ERROR: Could not write parameters to file: {0}\n".format(resultFile)) + raise RuntimeError("ERROR: Could not write parameters to file: {0}\n".format(resultFile)) From dc8801720d9e55c10e6eaa728d8a2728e9678109 Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Thu, 1 Apr 2021 13:02:42 +0100 Subject: [PATCH 46/75] [aslam_cameras_april] Link fix --- aslam_cv/aslam_cameras_april/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aslam_cv/aslam_cameras_april/CMakeLists.txt b/aslam_cv/aslam_cameras_april/CMakeLists.txt index 4885a2b07..3880a84c6 100644 --- a/aslam_cv/aslam_cameras_april/CMakeLists.txt +++ b/aslam_cv/aslam_cameras_april/CMakeLists.txt @@ -29,7 +29,7 @@ include_directories(include ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} ${Boost add_library(${PROJECT_NAME} src/GridCalibrationTargetAprilgrid.cpp ) -target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBS} ${Boost_LIBRARIES}) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${OpenCV_LIBS} ${Boost_LIBRARIES}) add_python_export_library(${PROJECT_NAME}_python python/aslam_cameras_april src/module.cpp From f3398802f02dba6d2830f484d63be3a27af8094f Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Thu, 1 Apr 2021 13:07:49 +0100 Subject: [PATCH 47/75] [CI] Add buildserver --- .github/workflows/industrial_ci_action.yml | 26 ++++++++++++++++++++++ 1 file changed, 26 insertions(+) create mode 100644 .github/workflows/industrial_ci_action.yml diff --git a/.github/workflows/industrial_ci_action.yml b/.github/workflows/industrial_ci_action.yml new file mode 100644 index 000000000..1d7b2eea6 --- /dev/null +++ b/.github/workflows/industrial_ci_action.yml @@ -0,0 +1,26 @@ +name: CI + +# This determines when this workflow is run +on: [push, pull_request] # on all pushes and PRs + +jobs: + CI: + strategy: + matrix: + env: + - {ROS_DISTRO: melodic} + - {ROS_DISTRO: noetic} + env: + CCACHE_DIR: /github/home/.ccache # Enable ccache + NOT_TEST_BUILD: true + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v2 + # This step will fetch/store the directory used by ccache before/after the ci run + - uses: actions/cache@v2 + with: + path: ${{ env.CCACHE_DIR }} + key: ccache-${{ matrix.env.ROS_DISTRO }}-${{ matrix.env.ROS_REPO }} + # Run industrial_ci + - uses: 'ros-industrial/industrial_ci@master' + env: ${{ matrix.env }} From 8087bbeb7b49939021e391c3d0257b7a9c98cb4d Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Thu, 1 Apr 2021 13:12:00 +0100 Subject: [PATCH 48/75] Update README with 20.04 instructions --- README.md | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index e11071dd2..8cda2658b 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ ![Kalibr](https://raw.githubusercontent.com/wiki/ethz-asl/kalibr/images/kalibr_small.png) -*Ubuntu 14.04+ROS indigo*: [![Build Status](https://jenkins.asl.ethz.ch/buildStatus/icon?job=kalibr_weekly/label=ubuntu-trusty)](https://jenkins.asl.ethz.ch/job/kalibr_weekly/label=ubuntu-trusty/) *Ubuntu 16.04+ROS kinetic*: [![Build Status](https://jenkins.asl.ethz.ch/buildStatus/icon?job=kalibr_weekly/label=ubuntu-trusty)](https://jenkins.asl.ethz.ch/job/kalibr_weekly/label=ubuntu-xenial/) + ## Introduction Kalibr is a toolbox that solves the following calibration problems: @@ -17,6 +17,16 @@ Kalibr is a toolbox that solves the following calibration problems: **For questions or comments, please open an issue on Github.** +## Installation + +### Ubuntu 20.04 + +We've upgraded and fixed kalibr at ORI for 20.04. Please use our fork: `git clone https://github.com/ori-drs/kalibr.git --branch noetic-devel`. + +- Use `rosdep` to install almost all required dependencies: `rosdep install --from-paths ./ -iry`. +- Then install the two missing runtime dependencies: `sudo apt install python3-wxgtk4.0 python3-igraph` +- Unittests are currently failing on 20.04 and thus deactivated on the buildserver. + ## Tutorial: IMU-camera calibration A video tutorial for the IMU-camera calibration can be found here: From fd5384b1ca7f88d0b53d8f35e54525a9ec134155 Mon Sep 17 00:00:00 2001 From: Marco Camurri Date: Tue, 6 Apr 2021 14:06:21 +0100 Subject: [PATCH 49/75] minor fixes in python interpreter --- .../kalibr/python/kalibr_imu_camera_calibration/IccPlots.py | 4 ++-- .../kalibr/python/kalibr_imu_camera_calibration/IccUtil.py | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccPlots.py b/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccPlots.py index dc23d4d13..c964d5ffc 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccPlots.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccPlots.py @@ -199,7 +199,7 @@ def plotVectorOverTime(times, values, title="", ylabel="", label="", fno=1, clea pl.grid('on') pl.xlabel("time (s)") pl.ylabel(ylabel) - if label is not "": + if label != "": pl.legend() def plotReprojectionScatter(cself, cam_id, fno=1, clearFigure=True, noShow=False, title=""): @@ -298,4 +298,4 @@ def plot3DCamera(self, T): z=np.squeeze([ori[2], v3[2]]).transpose() self.cam_z[0].set_data(xy) self.cam_z[0].set_3d_properties(z) - pl.pause(0.00001) \ No newline at end of file + pl.pause(0.00001) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccUtil.py b/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccUtil.py index a57ede87a..58f35b097 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccUtil.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccUtil.py @@ -265,7 +265,7 @@ def printResultTxt(cself, stream=sys.stdout): cam.targetConfig.printDetails(stream) print("", file=stream) - print("", file=stream) + print("", file=stream) print("", file=stream) print("IMU configuration", file=stream) print("=================", file=stream) From 26883a750ed47db66d44d0cdc6ba8be25a048415 Mon Sep 17 00:00:00 2001 From: Wolfgang Merkt Date: Tue, 6 Apr 2021 15:11:28 +0100 Subject: [PATCH 50/75] [kalibr] Fix issues with IMU calibration --- .../kalibr/CMakeLists.txt | 40 +++++++++++++++---- aslam_offline_calibration/kalibr/package.xml | 1 - .../python/kalibr_errorterms/__init__.py | 6 ++- .../IccSensors.py | 2 +- .../kalibr_imu_camera_calibration/__init__.py | 2 +- .../{IccCalibrator.py => icc_calibrator.py} | 0 6 files changed, 40 insertions(+), 11 deletions(-) rename aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/{IccCalibrator.py => icc_calibrator.py} (100%) diff --git a/aslam_offline_calibration/kalibr/CMakeLists.txt b/aslam_offline_calibration/kalibr/CMakeLists.txt index dbdd098f4..e72840e19 100644 --- a/aslam_offline_calibration/kalibr/CMakeLists.txt +++ b/aslam_offline_calibration/kalibr/CMakeLists.txt @@ -1,18 +1,42 @@ cmake_minimum_required(VERSION 3.0.2) project(kalibr) -find_package(catkin_simple REQUIRED) -catkin_simple() +find_package(catkin REQUIRED COMPONENTS + aslam_cv_python + aslam_backend + aslam_backend_expressions + aslam_backend_python + incremental_calibration_python + aslam_cameras_april + aslam_splines_python + numpy_eigen + python_module + sparse_block_matrix + sm_python + aslam_cv_backend_python +) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} +) ################################## # error terms (+python export) ################################## -cs_add_library(${PROJECT_NAME}_errorterms +include_directories(include ${catkin_INCLUDE_DIRS}) + +add_library(${PROJECT_NAME}_errorterms src/EuclideanError.cpp src/GyroscopeError.cpp src/AccelerometerError.cpp ) +target_link_libraries(${PROJECT_NAME}_errorterms ${catkin_LIBRARIES}) +# NB: The ".." is a hack to install the Python bindings to a global location to +# then be able to import it into a local context project. This is required to +# work-around having multiple Python packages in one catkin package and the way +# the PYTHON_PATH overloads work. add_python_export_library(${PROJECT_NAME}_errorterms_python python/${PROJECT_NAME}_errorterms/.. src/module.cpp ) @@ -22,7 +46,6 @@ target_link_libraries(${PROJECT_NAME}_errorterms_python ${PROJECT_NAME}_errorter # tests ################################## if(CATKIN_ENABLE_TESTING) - add_definitions(-DGTEST_USE_OWN_TR1_TUPLE=0) catkin_add_gtest(${PROJECT_NAME}_test @@ -30,15 +53,18 @@ if(CATKIN_ENABLE_TESTING) test/TestErrorTerms.cpp ) target_link_libraries(${PROJECT_NAME}_test ${PROJECT_NAME}_errorterms) - endif() ################################## # EXPORT ################################## catkin_python_setup() -cs_install() -cs_export() + +install(TARGETS ${${PROJECT_NAME}_TARGETS} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) catkin_install_python( PROGRAMS diff --git a/aslam_offline_calibration/kalibr/package.xml b/aslam_offline_calibration/kalibr/package.xml index ae9f15d36..6201295ad 100644 --- a/aslam_offline_calibration/kalibr/package.xml +++ b/aslam_offline_calibration/kalibr/package.xml @@ -8,7 +8,6 @@ New BSD catkin - catkin_simple. aslam_cv_python aslam_backend diff --git a/aslam_offline_calibration/kalibr/python/kalibr_errorterms/__init__.py b/aslam_offline_calibration/kalibr/python/kalibr_errorterms/__init__.py index d6d6e61e0..9f37862e2 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_errorterms/__init__.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_errorterms/__init__.py @@ -1,2 +1,6 @@ import aslam_cv_backend #needed for errorterm base class wrapper -from .libkalibr_errorterms_python import * + +# NB: We import these bindings from the global path as we have to +# hack the installation due to multiple Python packages in one +# catkin package. +from libkalibr_errorterms_python import * diff --git a/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccSensors.py b/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccSensors.py index 8a1aab571..d4cbf3f34 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccSensors.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccSensors.py @@ -6,7 +6,7 @@ import bsplines import kalibr_common as kc import kalibr_errorterms as ket -from . import IccCalibrator as ic +from . import icc_calibrator as ic import cv2 import sys diff --git a/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/__init__.py b/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/__init__.py index 1c4c3ecef..17b09d7bb 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/__init__.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/__init__.py @@ -1,4 +1,4 @@ -from .IccCalibrator import * +from .icc_calibrator import * from . import IccUtil as util from . import IccPlots as plots from . import IccSensors as sens diff --git a/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccCalibrator.py b/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/icc_calibrator.py similarity index 100% rename from aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccCalibrator.py rename to aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/icc_calibrator.py From e4bdc440b986e9edb15a678ddaeceeb575e01749 Mon Sep 17 00:00:00 2001 From: Marco Camurri Date: Wed, 19 May 2021 13:07:15 +0100 Subject: [PATCH 51/75] add support for all the bayes patterns --- .../python/kalibr_common/ImageDatasetReader.py | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_common/ImageDatasetReader.py b/aslam_offline_calibration/kalibr/python/kalibr_common/ImageDatasetReader.py index 13688ba29..451d90412 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_common/ImageDatasetReader.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_common/ImageDatasetReader.py @@ -143,10 +143,21 @@ def getImage(self, idx): elif data.encoding == "8UC4" or data.encoding == "bgra8": img_data = np.array(self.CVB.imgmsg_to_cv2(data)) img_data = cv2.cvtColor(img_data, cv2.COLOR_BGRA2GRAY) + # bayes encodings conversions from + # https://github.com/ros-perception/image_pipeline/blob/6caf51bd4484ae846cd8a199f7a6a4b060c6373a/image_proc/src/libimage_proc/processor.cpp#L70 elif data.encoding == "bayer_rggb8": img_data = np.array(self.CVB.imgmsg_to_cv2(data)) img_data = cv2.cvtColor(img_data, cv2.COLOR_BAYER_BG2GRAY) + elif data.encoding == "bayer_bggr8": + img_data = np.array(self.CVB.imgmsg_to_cv2(data)) + img_data = cv2.cvtColor(img_data, cv2.COLOR_BAYER_RG2GRAY) + elif data.encoding == "bayer_gbrg8": + img_data = np.array(self.CVB.imgmsg_to_cv2(data)) + img_data = cv2.cvtColor(img_data, cv2.COLOR_BAYER_GR2GRAY) + elif data.encoding == "bayer_grbg8": + img_data = np.array(self.CVB.imgmsg_to_cv2(data)) + img_data = cv2.cvtColor(img_data, cv2.COLOR_BAYER_GB2GRAY) else: raise RuntimeError( - "Unsupported Image format '{}' (Supported are: 16UC1 / mono16, 8UC1 / mono8, 8UC3 / rgb8 / bgr8, 8UC4 / bgra8, bayer_rggb8 and ImageSnappyMsg)".format(data.encoding)); + "Unsupported Image format '{}' (Supported are: 16UC1 / mono16, 8UC1 / mono8, 8UC3 / rgb8 / bgr8, 8UC4 / bgra8, bayer_rggb8, bayer_bggr8, bayer_gbrg8, bayer_grbg8, and ImageSnappyMsg)".format(data.encoding)); return (timestamp, img_data) From 7d4b0e9dd99f75a95a74d5d01093de700376b8c6 Mon Sep 17 00:00:00 2001 From: Marco Camurri Date: Wed, 19 May 2021 13:09:19 +0100 Subject: [PATCH 52/75] fix python numpy integer conversion --- .../kalibr/python/kalibr_visualize_distortion | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_visualize_distortion b/aslam_offline_calibration/kalibr/python/kalibr_visualize_distortion index 4bc008955..0e97b43f3 100755 --- a/aslam_offline_calibration/kalibr/python/kalibr_visualize_distortion +++ b/aslam_offline_calibration/kalibr/python/kalibr_visualize_distortion @@ -111,18 +111,18 @@ def drawImage(resolution, num_rectangles, line_thickness): h_offset = i*resolution[0] / 2 / (num_rectangles + 1) v_offset = i*resolution[1] / 2 / (num_rectangles + 1) # Draw lines. + for t in range(0, int(line_thickness)): + for i in range(int(-h_offset), int(h_offset)): + raw_image[int(resolution[1] / 2 - v_offset - t), + int(resolution[0] / 2 + i)] = 255 + raw_image[int(resolution[1] / 2 + v_offset + t), + int(resolution[0] / 2 + i)] = 255 for t in range(0, line_thickness): - for i in range(-h_offset, h_offset): - raw_image[resolution[1] / 2 - v_offset - t, - resolution[0] / 2 + i] = 255; - raw_image[resolution[1] / 2 + v_offset + t, - resolution[0] / 2 + i] = 255; - for t in range(0, line_thickness): - for j in range(-v_offset, v_offset): - raw_image[resolution[1] / 2 + j, - resolution[0] / 2 - h_offset - t] = 255 - raw_image[resolution[1] / 2 + j, - resolution[0] / 2 + h_offset + t] = 255 + for j in range(int(-v_offset), int(v_offset)): + raw_image[int(resolution[1] / 2 + j), + int(resolution[0] / 2 - h_offset - t)] = 255 + raw_image[int(resolution[1] / 2 + j), + int(resolution[0] / 2 + h_offset + t)] = 255 return raw_image def imshow(image, color, alpha = 1): From 89bcf8d96b7e3eb2fee7c0751ecd5536c1a3e581 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Tue, 19 Apr 2022 13:22:01 -0400 Subject: [PATCH 53/75] fix single cam graph connection (fix #364) --- .../python/kalibr_camera_calibration/MulticamGraph.py | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/MulticamGraph.py b/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/MulticamGraph.py index 87e621bd6..b090f10e5 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/MulticamGraph.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/MulticamGraph.py @@ -84,8 +84,14 @@ def initializeGraphFromObsDb(self, obs_db): ############################################################# #check if all cams are connected through observations def isGraphConnected(self): - #check if all vertices are connected - return self.G.adhesion() + if self.numCams == 1: + # Since igraph 0.8, adhesion correctly returns 0 for the non-connected one cam case. + # which evaluates to false later on. So we skip the check and return true in the one camera case. + # https://github.com/ethz-asl/kalibr/issues/364 + return True + else: + #check if all vertices are connected + return self.G.adhesion() #returns the list of cam_ids that share common view with the specified cam_id def getCamOverlaps(self, cam_id): From 8fa15b2657d321af001e8545967b6e3905116fc3 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Tue, 19 Apr 2022 13:22:18 -0400 Subject: [PATCH 54/75] Add docker file for 20.04 --- Dockerfile_ros1_20_04 | 48 +++++++++++++++++++++++++++++++++++++++++++ README.md | 35 +++++++++++++++++++++++++++++++ 2 files changed, 83 insertions(+) create mode 100644 Dockerfile_ros1_20_04 diff --git a/Dockerfile_ros1_20_04 b/Dockerfile_ros1_20_04 new file mode 100644 index 000000000..f27c3a3c6 --- /dev/null +++ b/Dockerfile_ros1_20_04 @@ -0,0 +1,48 @@ +FROM osrf/ros:noetic-desktop-full + + +# Dependencies we use, catkin tools is very good build system +# https://github.com/ethz-asl/kalibr/wiki/installation +RUN apt-get update && DEBIAN_FRONTEND=noninteractive \ + apt-get install -y \ + git wget autoconf automake \ + python3-dev python3-pip python3-scipy python3-matplotlib \ + ipython3 python3-wxgtk4.0 python3-tk \ + libeigen3-dev libboost-all-dev libsuitesparse-dev \ + doxygen \ + libopencv-dev \ + libpoco-dev libtbb-dev libblas-dev liblapack-dev libv4l-dev \ + python3-catkin-tools python3-osrf-pycommon +RUN python3 -m pip install --upgrade pip; \ + python3 -m pip install python-igraph --upgrade + + +# Create the workspace and build kalibr in it +ENV WORKSPACE /catkin_ws + +RUN mkdir -p $WORKSPACE/src && \ + cd $WORKSPACE && \ + catkin init && \ + catkin config --extend /opt/ros/noetic && \ + catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release + +ADD . $WORKSPACE/src/kalibr +# RUN cd $WORKSPACE/src &&\ +# git clone https://github.com/ori-drs/kalibr.git + +RUN cd $WORKSPACE &&\ + catkin build -j$(nproc) + + +# When a user runs a command we will run this code before theirs +# This will allow for using the manual focal length if it fails to init +# https://github.com/ethz-asl/kalibr/pull/346 +ENTRYPOINT export KALIBR_MANUAL_FOCAL_LENGTH_INIT=1 && \ + # /bin/bash -c "source \"$WORKSPACE/devel/setup.bash\"" && \ + cd $WORKSPACE && \ + /bin/bash + + + + + diff --git a/README.md b/README.md index 8cda2658b..5a6736369 100644 --- a/README.md +++ b/README.md @@ -27,6 +27,41 @@ We've upgraded and fixed kalibr at ORI for 20.04. Please use our fork: `git clon - Then install the two missing runtime dependencies: `sudo apt install python3-wxgtk4.0 python3-igraph` - Unittests are currently failing on 20.04 and thus deactivated on the buildserver. + +### Docker + +First make sure that you have install docker on your system using the official Docker [Get Docker](https://docs.docker.com/get-docker/) guide. +We can then build the docker container using: +``` +cd /src/kalibr +docker build -t kalibr -f Dockerfile_ros1_20_04 . +``` + +Then after building we can first download an example dataset and then perform following to calibrate. +``` +cd +mkdir example && cd example +wget http://robotics.ethz.ch/~asl-datasets/ijrr_euroc_mav_dataset/calibration_datasets/cam_april/april_6x6.yaml +wget http://robotics.ethz.ch/~asl-datasets/ijrr_euroc_mav_dataset/calibration_datasets/cam_april/cam_april.bag +``` + +We can now mount the data folder in the container `/data` path and enter the command prompt. +Some more details can be found on the [ROS wiki](http://wiki.ros.org/docker/Tutorials/GUI) for Docker. +``` +FOLDER=$(pwd) +xhost +local:root +docker run -it -e "DISPLAY" -e "QT_X11_NO_MITSHM=1" \ + -v "/tmp/.X11-unix:/tmp/.X11-unix:rw" \ + -v "$FOLDER:/data" \ + kalibr +source devel/setup.bash +rosrun kalibr kalibr_calibrate_cameras \ + --bag /data/cam_april.bag --target /data/april_6x6.yaml \ + --models pinhole-radtan pinhole-radtan \ + --topics /cam0/image_raw /cam1/image_raw +``` + + ## Tutorial: IMU-camera calibration A video tutorial for the IMU-camera calibration can be found here: From c2c3cabeabf1074421678e7b78669cba5e60f08c Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Tue, 19 Apr 2022 14:14:03 -0400 Subject: [PATCH 55/75] correct path generation for result files (before could not specify a bag in a folder) --- .../kalibr/python/kalibr_calibrate_cameras | 8 ++++---- .../kalibr/python/kalibr_calibrate_imu_camera | 15 ++++++++------- 2 files changed, 12 insertions(+), 11 deletions(-) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_calibrate_cameras b/aslam_offline_calibration/kalibr/python/kalibr_calibrate_cameras index 33769b4d0..9ce2f4779 100755 --- a/aslam_offline_calibration/kalibr/python/kalibr_calibrate_cameras +++ b/aslam_offline_calibration/kalibr/python/kalibr_calibrate_cameras @@ -403,18 +403,18 @@ def main(): pl.show() #write to file - bagtag = parsed.bagfile.translate("<>:/\|?*").replace('.bag', '', 1) - resultFile = "camchain-" + bagtag + ".yaml" + bagtag = os.path.splitext(parsed.bagfile)[0] + resultFile = bagtag + "-camchain.yaml" kcc.saveChainParametersYaml(calibrator, resultFile, graph) print("Results written to file: {0}".format(resultFile)) #save results to file - resultFileTxt = "results-cam-" + bagtag + ".txt" + resultFileTxt = bagtag + "-results-cam.txt" kcc.saveResultTxt(calibrator, filename=resultFileTxt) print(" Detailed results written to file: {0}".format(resultFileTxt)) #generate report - reportFile = "report-cam-" + bagtag + ".pdf" + reportFile = bagtag + "-report-cam.pdf" G=None; if numCams>1: G=graph diff --git a/aslam_offline_calibration/kalibr/python/kalibr_calibrate_imu_camera b/aslam_offline_calibration/kalibr/python/kalibr_calibrate_imu_camera index 428e5f598..71e51f9f3 100755 --- a/aslam_offline_calibration/kalibr/python/kalibr_calibrate_imu_camera +++ b/aslam_offline_calibration/kalibr/python/kalibr_calibrate_imu_camera @@ -7,7 +7,8 @@ from kalibr_imu_camera_calibration import * import numpy as np import argparse import signal -import sys +import sys +import os # make numpy print prettier np.set_printoptions(suppress=True) @@ -214,29 +215,29 @@ def main(): util.printResults(iCal, withCov=parsed.recover_cov) print() - bagtag = parsed.bagfile[0].translate("<>:/\|?*").replace('.bag', '', 1) - yamlFilename = "camchain-imucam-" + bagtag + ".yaml" + bagtag = os.path.splitext(parsed.bagfile[0])[0] + yamlFilename = bagtag + "-camchain-imucam.yaml" iCal.saveCamChainParametersYaml(yamlFilename) print(" Saving camera chain calibration to file: {0}".format(yamlFilename)) print() - yamlFilename = "imu-" + bagtag + ".yaml" + yamlFilename = bagtag + "-imu.yaml" iCal.saveImuSetParametersYaml(yamlFilename) print(" Saving imu calibration to file: {0}".format(yamlFilename)) - resultFileTxt = "results-imucam-" + bagtag + ".txt" + resultFileTxt = bagtag + "-results-imucam.txt" util.saveResultTxt(iCal, filename=resultFileTxt) print(" Detailed results written to file: {0}".format(resultFileTxt)) print("Generating result report...") - reportFile = "report-imucam-" + bagtag + ".pdf" + reportFile = bagtag + "-report-imucam.pdf" util.generateReport(iCal, filename=reportFile, showOnScreen=not parsed.dontShowReport) print(" Report written to {0}".format(reportFile)) print() if parsed.exportPoses: print("Exporting poses...") - posesFile = "poses-imucam-imu0-" + bagtag + ".csv" + posesFile = bagtag + "-poses-imucam-imu0.csv" util.exportPoses(iCal, filename=posesFile) print("Poses written to {0}".format(posesFile)) print() From 8d74cb869d89ea650285386573e061829be80b45 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Tue, 19 Apr 2022 14:17:55 -0400 Subject: [PATCH 56/75] docker github ci --- .github/workflows/docker_build.yaml | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) create mode 100644 .github/workflows/docker_build.yaml diff --git a/.github/workflows/docker_build.yaml b/.github/workflows/docker_build.yaml new file mode 100644 index 000000000..2c5281936 --- /dev/null +++ b/.github/workflows/docker_build.yaml @@ -0,0 +1,21 @@ +name: Docker + +# This determines when this workflow is run +on: [push, pull_request] # on all pushes and PRs + +jobs: + build_2004: + name: "ROS1 Ubuntu 20.04" + runs-on: ubuntu-latest + steps: + - name: Code Checkout + uses: actions/checkout@v2 + - name: Create Workspace + run: | + export REPO=$(basename $GITHUB_REPOSITORY) && + cd $GITHUB_WORKSPACE/.. && mkdir src/ && + mv $REPO/ src/ && mkdir $REPO/ && mv src/ $REPO/ && cd $REPO/ + - name: Build Docker + run: | + docker build -t kalibr -f $GITHUB_WORKSPACE/src/$REPO/Dockerfile_ros1_20_04 . + From 8761165d2eddb3a6d344d7d02865c78f59f209e0 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Tue, 19 Apr 2022 14:19:25 -0400 Subject: [PATCH 57/75] docker github ci --- .github/workflows/docker_build.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/docker_build.yaml b/.github/workflows/docker_build.yaml index 2c5281936..a91be6f63 100644 --- a/.github/workflows/docker_build.yaml +++ b/.github/workflows/docker_build.yaml @@ -17,5 +17,6 @@ jobs: mv $REPO/ src/ && mkdir $REPO/ && mv src/ $REPO/ && cd $REPO/ - name: Build Docker run: | + export REPO=$(basename $GITHUB_REPOSITORY) && docker build -t kalibr -f $GITHUB_WORKSPACE/src/$REPO/Dockerfile_ros1_20_04 . From bb638ff9141caa1a55e230fcb14ef9690cd9ef58 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Tue, 19 Apr 2022 17:19:05 -0400 Subject: [PATCH 58/75] changes for python 2.x to compile, and 16.04 kinetic and 18.04 melodic dockers --- .github/workflows/docker_build.yaml | 30 ++++++++++ Dockerfile_ros1_16_04 | 56 +++++++++++++++++++ Dockerfile_ros1_18_04 | 50 +++++++++++++++++ Dockerfile_ros1_20_04 | 6 +- .../sm_python/python/sm/Progress.py | 1 + .../kalibr/python/kalibr_bagextractor | 6 +- .../kalibr/python/kalibr_calibrate_cameras | 22 ++++---- .../kalibr/python/kalibr_calibrate_imu_camera | 18 +++--- .../CameraCalibrator.py | 17 +++--- .../python/kalibr_camera_calibration/ObsDb.py | 5 +- .../python/kalibr_common/ConfigReader.py | 1 + .../kalibr_common/ImageDatasetReader.py | 5 ++ .../python/kalibr_common/ImuDatasetReader.py | 4 ++ .../python/kalibr_common/TargetExtractor.py | 5 +- .../IccSensors.py | 21 +++---- .../kalibr_imu_camera_calibration/IccUtil.py | 23 +++++--- .../RsCalibrator.py | 4 +- 17 files changed, 216 insertions(+), 58 deletions(-) create mode 100644 Dockerfile_ros1_16_04 create mode 100644 Dockerfile_ros1_18_04 diff --git a/.github/workflows/docker_build.yaml b/.github/workflows/docker_build.yaml index a91be6f63..b606614fc 100644 --- a/.github/workflows/docker_build.yaml +++ b/.github/workflows/docker_build.yaml @@ -4,6 +4,36 @@ name: Docker on: [push, pull_request] # on all pushes and PRs jobs: + build_1604: + name: "ROS1 Ubuntu 16.04" + runs-on: ubuntu-latest + steps: + - name: Code Checkout + uses: actions/checkout@v2 + - name: Create Workspace + run: | + export REPO=$(basename $GITHUB_REPOSITORY) && + cd $GITHUB_WORKSPACE/.. && mkdir src/ && + mv $REPO/ src/ && mkdir $REPO/ && mv src/ $REPO/ && cd $REPO/ + - name: Build Docker + run: | + export REPO=$(basename $GITHUB_REPOSITORY) && + docker build -t kalibr -f $GITHUB_WORKSPACE/src/$REPO/Dockerfile_ros1_16_04 . + build_1804: + name: "ROS1 Ubuntu 18.04" + runs-on: ubuntu-latest + steps: + - name: Code Checkout + uses: actions/checkout@v2 + - name: Create Workspace + run: | + export REPO=$(basename $GITHUB_REPOSITORY) && + cd $GITHUB_WORKSPACE/.. && mkdir src/ && + mv $REPO/ src/ && mkdir $REPO/ && mv src/ $REPO/ && cd $REPO/ + - name: Build Docker + run: | + export REPO=$(basename $GITHUB_REPOSITORY) && + docker build -t kalibr -f $GITHUB_WORKSPACE/src/$REPO/Dockerfile_ros1_18_04 . build_2004: name: "ROS1 Ubuntu 20.04" runs-on: ubuntu-latest diff --git a/Dockerfile_ros1_16_04 b/Dockerfile_ros1_16_04 new file mode 100644 index 000000000..cbf8c041e --- /dev/null +++ b/Dockerfile_ros1_16_04 @@ -0,0 +1,56 @@ +FROM osrf/ros:kinetic-desktop-full + + +# Dependencies we use, catkin tools is very good build system +# https://github.com/ethz-asl/kalibr/wiki/installation +RUN apt-get update && DEBIAN_FRONTEND=noninteractive \ + apt-get install -y \ + git wget autoconf automake \ + python2.7-dev python-pip python-scipy python-matplotlib \ + ipython python-wxgtk3.0 python-tk python-igraph \ + libeigen3-dev libboost-all-dev libsuitesparse-dev \ + doxygen \ + libopencv-dev \ + libpoco-dev libtbb-dev libblas-dev liblapack-dev libv4l-dev \ + python-catkin-tools + + +# Create the workspace and build kalibr in it +ENV WORKSPACE /catkin_ws + +RUN mkdir -p $WORKSPACE/src && \ + cd $WORKSPACE && \ + catkin init && \ + catkin config --extend /opt/ros/kinetic && \ + catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release + +ADD . $WORKSPACE/src/kalibr +# RUN cd $WORKSPACE/src &&\ +# git clone https://github.com/ori-drs/kalibr.git + +RUN cd $WORKSPACE &&\ + catkin build -j$(nproc) + + +# When a user runs a command we will run this code before theirs +# This will allow for using the manual focal length if it fails to init +# https://github.com/ethz-asl/kalibr/pull/346 +ENTRYPOINT export KALIBR_MANUAL_FOCAL_LENGTH_INIT=1 && \ + # /bin/bash -c "source \"$WORKSPACE/devel/setup.bash\"" && \ + cd $WORKSPACE && \ + /bin/bash + + + + + + + + + + + + + + + diff --git a/Dockerfile_ros1_18_04 b/Dockerfile_ros1_18_04 new file mode 100644 index 000000000..ea185002a --- /dev/null +++ b/Dockerfile_ros1_18_04 @@ -0,0 +1,50 @@ +FROM osrf/ros:melodic-desktop-full + + +# Dependencies we use, catkin tools is very good build system +# https://github.com/ethz-asl/kalibr/wiki/installation +RUN apt-get update && DEBIAN_FRONTEND=noninteractive \ + apt-get install -y \ + git wget autoconf automake nano \ + python3-dev python-pip python-scipy python-matplotlib \ + ipython python-wxgtk4.0 python-tk python-igraph \ + libeigen3-dev libboost-all-dev libsuitesparse-dev \ + doxygen \ + libopencv-dev \ + libpoco-dev libtbb-dev libblas-dev liblapack-dev libv4l-dev \ + python-catkin-tools + + +# Create the workspace and build kalibr in it +ENV WORKSPACE /catkin_ws + +RUN mkdir -p $WORKSPACE/src && \ + cd $WORKSPACE && \ + catkin init && \ + catkin config --extend /opt/ros/melodic && \ + catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release + +ADD . $WORKSPACE/src/kalibr +# RUN cd $WORKSPACE/src &&\ +# git clone https://github.com/ori-drs/kalibr.git + +RUN cd $WORKSPACE &&\ + catkin build -j$(nproc) + + +# When a user runs a command we will run this code before theirs +# This will allow for using the manual focal length if it fails to init +# https://github.com/ethz-asl/kalibr/pull/346 +ENTRYPOINT export KALIBR_MANUAL_FOCAL_LENGTH_INIT=1 && \ + # /bin/bash -c "source \"$WORKSPACE/devel/setup.bash\"" && \ + cd $WORKSPACE && \ + /bin/bash + + + + + + + + + diff --git a/Dockerfile_ros1_20_04 b/Dockerfile_ros1_20_04 index f27c3a3c6..91b49fa05 100644 --- a/Dockerfile_ros1_20_04 +++ b/Dockerfile_ros1_20_04 @@ -5,16 +5,14 @@ FROM osrf/ros:noetic-desktop-full # https://github.com/ethz-asl/kalibr/wiki/installation RUN apt-get update && DEBIAN_FRONTEND=noninteractive \ apt-get install -y \ - git wget autoconf automake \ + git wget autoconf automake nano \ python3-dev python3-pip python3-scipy python3-matplotlib \ - ipython3 python3-wxgtk4.0 python3-tk \ + ipython3 python3-wxgtk4.0 python3-tk python3-igraph \ libeigen3-dev libboost-all-dev libsuitesparse-dev \ doxygen \ libopencv-dev \ libpoco-dev libtbb-dev libblas-dev liblapack-dev libv4l-dev \ python3-catkin-tools python3-osrf-pycommon -RUN python3 -m pip install --upgrade pip; \ - python3 -m pip install python-igraph --upgrade # Create the workspace and build kalibr in it diff --git a/Schweizer-Messer/sm_python/python/sm/Progress.py b/Schweizer-Messer/sm_python/python/sm/Progress.py index 99e92d0f4..bbf18d23c 100644 --- a/Schweizer-Messer/sm_python/python/sm/Progress.py +++ b/Schweizer-Messer/sm_python/python/sm/Progress.py @@ -1,3 +1,4 @@ +from __future__ import print_function #handle print in 2.x python import time import datetime import sys diff --git a/aslam_offline_calibration/kalibr/python/kalibr_bagextractor b/aslam_offline_calibration/kalibr/python/kalibr_bagextractor index 6c4017bc8..b8d3494c5 100755 --- a/aslam_offline_calibration/kalibr/python/kalibr_bagextractor +++ b/aslam_offline_calibration/kalibr/python/kalibr_bagextractor @@ -69,8 +69,8 @@ if parsed.image_topics is not None: iProgress.sample() print("\r done. ") - print() -print() + print("") +print("") #extract imu data if parsed.imu_topics is not None: @@ -93,4 +93,4 @@ if parsed.imu_topics is not None: spamwriter.writerow([timestamp_int, omega[0],omega[1],omega[2], alpha[0],alpha[1],alpha[2] ]) iProgress.sample() print("\r done. ") - print() + print("") diff --git a/aslam_offline_calibration/kalibr/python/kalibr_calibrate_cameras b/aslam_offline_calibration/kalibr/python/kalibr_calibrate_cameras index 9ce2f4779..33c60afd9 100755 --- a/aslam_offline_calibration/kalibr/python/kalibr_calibrate_cameras +++ b/aslam_offline_calibration/kalibr/python/kalibr_calibrate_cameras @@ -274,13 +274,13 @@ def main(): #display process if (verbose or (view_id % 25) == 0) and calibrator.estimator.getNumBatches()>0 and view_id>1: - print() + print("") print("------------------------------------------------------------------") - print() + print("") print("Processed {0} of {1} views with {2} views used".format(view_id+1, numViews, calibrator.estimator.getNumBatches())) - print() + print("") kcc.printParameters(calibrator) - print() + print("") print("------------------------------------------------------------------") #calibration progress @@ -305,13 +305,13 @@ def main(): if initOutlierRejection: #check all views after the min. number of batches has been reached batches_to_check=list(range(0, calibrator.estimator.getNumBatches())) - print();print() + print("");print("") print("Filtering outliers in all batches...") initOutlierRejection=False progress_filter = sm.Progress2(len(batches_to_check)); progress_filter.sample() elif runEndFiltering: #check all batches again after all views have been processed - print();print() + print("");print("") print("All views have been processed.\n\nStarting final outlier filtering...") batches_to_check=list(range(0, calibrator.estimator.getNumBatches())) progress_filter = sm.Progress2(len(batches_to_check)); progress_filter.sample() @@ -385,15 +385,15 @@ def main(): progress_filter.sample() #final output - print() - print() + print("") + print("") print("..................................................................") - print() + print("") print("Calibration complete.") - print() + print("") if parsed.removeOutliers: sm.logWarn("Removed {0} outlier corners.".format(len(removedOutlierCorners)) ) - print() + print("") print("Processed {0} images with {1} images used".format(numViews, calibrator.estimator.getNumBatches())) kcc.printParameters(calibrator) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_calibrate_imu_camera b/aslam_offline_calibration/kalibr/python/kalibr_calibrate_imu_camera index 71e51f9f3..380fae0a5 100755 --- a/aslam_offline_calibration/kalibr/python/kalibr_calibrate_imu_camera +++ b/aslam_offline_calibration/kalibr/python/kalibr_calibrate_imu_camera @@ -14,7 +14,7 @@ import os np.set_printoptions(suppress=True) def signal_exit(signal, frame): - print() + print("") sm.logWarn("Shutting down! (CTRL+C)") sys.exit(1) @@ -183,7 +183,7 @@ def main(): if imu is not imus[0]: imu.findOrientationPrior(imus[0]) - print() + print("") print("Building the problem") iCal.buildProblem(splineOrder=6, poseKnotsPerSecond=100, @@ -199,28 +199,28 @@ def main(): timeOffsetPadding=parsed.timeoffset_padding, verbose = parsed.verbose) - print() + print("") print("Before Optimization") print("===================") util.printErrorStatistics(iCal) - print() + print("") print("Optimizing...") iCal.optimize(maxIterations=parsed.max_iter, recoverCov=parsed.recover_cov) - print() + print("") print("After Optimization (Results)") print("==================") util.printErrorStatistics(iCal) util.printResults(iCal, withCov=parsed.recover_cov) - print() + print("") bagtag = os.path.splitext(parsed.bagfile[0])[0] yamlFilename = bagtag + "-camchain-imucam.yaml" iCal.saveCamChainParametersYaml(yamlFilename) print(" Saving camera chain calibration to file: {0}".format(yamlFilename)) - print() + print("") yamlFilename = bagtag + "-imu.yaml" iCal.saveImuSetParametersYaml(yamlFilename) print(" Saving imu calibration to file: {0}".format(yamlFilename)) @@ -233,14 +233,14 @@ def main(): reportFile = bagtag + "-report-imucam.pdf" util.generateReport(iCal, filename=reportFile, showOnScreen=not parsed.dontShowReport) print(" Report written to {0}".format(reportFile)) - print() + print("") if parsed.exportPoses: print("Exporting poses...") posesFile = bagtag + "-poses-imucam-imu0.csv" util.exportPoses(iCal, filename=posesFile) print("Poses written to {0}".format(posesFile)) - print() + print("") if __name__ == "__main__": main() diff --git a/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/CameraCalibrator.py b/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/CameraCalibrator.py index 1736d69e0..4fa1490e7 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/CameraCalibrator.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/CameraCalibrator.py @@ -1,3 +1,4 @@ +from __future__ import print_function #handle print in 2.x python import sm from sm import PlotCollection from kalibr_common import ConfigReader as cr @@ -616,14 +617,14 @@ def printParameters(cself, dest=sys.stdout): def printDebugEnd(cself): - print() - print() + print("") + print("") for cidx, cam in enumerate(cself.cameras): print("cam{0}".format(cidx)) print("----------") - print() - print() + print("") + print("") corners, reprojs, rerrs = getReprojectionErrors(cself, cidx) if len(rerrs)>0: @@ -633,12 +634,12 @@ def printDebugEnd(cself): print(se[0]) print(se[1]) - print() + print("") p = cam.geometry.projection().getParameters().flatten() for temp in p: print(temp) - print() + print("") d = cam.geometry.projection().distortion().getParameters().flatten() for temp in d: print(temp) @@ -652,8 +653,8 @@ def printDebugEnd(cself): for temp in T.q(): print(temp) - print() - print() + print("") + print("") def saveChainParametersYaml(cself, resultFile, graph): cameraModels = {acvb.DistortedPinhole: 'pinhole', diff --git a/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/ObsDb.py b/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/ObsDb.py index 0fdd06dd2..811a84d87 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/ObsDb.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/ObsDb.py @@ -1,3 +1,4 @@ +from __future__ import print_function #handle print in 2.x python import sm import aslam_backend as aopt @@ -146,7 +147,7 @@ def printTable(self): print("timestamp \t", end=' ') for cam_id in range(0, self.numCameras()): print("cam{0} \t".format(cam_id), end=' ') - print() + print("") #sort for time times_sorted = np.sort(list(self.targetViews.keys())) @@ -160,5 +161,5 @@ def printTable(self): except KeyError: numCorners = "-" print("\t", numCorners, end=' ') - print() + print("") diff --git a/aslam_offline_calibration/kalibr/python/kalibr_common/ConfigReader.py b/aslam_offline_calibration/kalibr/python/kalibr_common/ConfigReader.py index 89b31de95..6cbe2fa90 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_common/ConfigReader.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_common/ConfigReader.py @@ -1,3 +1,4 @@ +from __future__ import print_function #handle print in 2.x python import yaml import sys import numpy as np diff --git a/aslam_offline_calibration/kalibr/python/kalibr_common/ImageDatasetReader.py b/aslam_offline_calibration/kalibr/python/kalibr_common/ImageDatasetReader.py index 451d90412..a2a8bb2e4 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_common/ImageDatasetReader.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_common/ImageDatasetReader.py @@ -20,6 +20,11 @@ def __init__(self, dataset, indices=None): def __iter__(self): return self + def next(self): + # required for python 2.x compatibility + idx = next(self.iter) + return self.dataset.getImage(idx) + def __next__(self): idx = next(self.iter) return self.dataset.getImage(idx) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_common/ImuDatasetReader.py b/aslam_offline_calibration/kalibr/python/kalibr_common/ImuDatasetReader.py index e1470cf97..9f0074855 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_common/ImuDatasetReader.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_common/ImuDatasetReader.py @@ -15,6 +15,10 @@ def __init__(self,dataset,indices=None): self.iter = self.indices.__iter__() def __iter__(self): return self + def next(self): + # required for python 2.x compatibility + idx = next(self.iter) + return self.dataset.getMessage(idx) def __next__(self): idx = next(self.iter) return self.dataset.getMessage(idx) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_common/TargetExtractor.py b/aslam_offline_calibration/kalibr/python/kalibr_common/TargetExtractor.py index 198b0651c..40f49c070 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_common/TargetExtractor.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_common/TargetExtractor.py @@ -3,7 +3,10 @@ import numpy as np import sys import multiprocessing -import queue +try: + import queue +except ImportError: + import Queue as queue # python 2.x import time import copy import cv2 diff --git a/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccSensors.py b/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccSensors.py index d4cbf3f34..68564f8f2 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccSensors.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccSensors.py @@ -1,3 +1,4 @@ +from __future__ import print_function #handle print in 2.x python import sm import aslam_cv as acv import aslam_cameras_april as acv_april @@ -114,7 +115,7 @@ def setupCalibrationTarget(self, targetConfig, showExtraction=False, showReproj= self.detector = acv.GridDetector(self.camera.geometry, grid, options) def findOrientationPriorCameraToImu(self, imu): - print() + print("") print("Estimating imu-camera rotation prior") # build the problem @@ -308,7 +309,7 @@ def initPoseSplineFromCamera(self, splineOrder=6, poseKnotsPerSecond=100, timeOf seconds = times[-1] - times[0] knots = int(round(seconds * poseKnotsPerSecond)) - print() + print("") print("Initializing a pose spline with %d knots (%f knots per second over %f seconds)" % ( knots, poseKnotsPerSecond, seconds)) pose.initPoseSplineSparse(times, curve, knots, 1e-4) return pose @@ -326,7 +327,7 @@ def addDesignVariables(self, problem, noExtrinsics=True, noTimeCalibration=True, problem.addDesignVariable(self.cameraTimeToImuTimeDv, ic.CALIBRATION_GROUP_ID) def addCameraErrorTerms(self, problem, poseSplineDv, T_cN_b, blakeZissermanDf=0.0, timeOffsetPadding=0.0): - print() + print("") print("Adding camera error terms ({0})".format(self.dataset.topic)) #progress bar @@ -666,7 +667,7 @@ def addDesignVariables(self, problem): def addAccelerometerErrorTerms(self, problem, poseSplineDv, g_w, mSigma=0.0, \ accelNoiseScale=1.0): - print() + print("") print("Adding accelerometer error terms ({0})".format(self.dataset.topic)) #progress bar @@ -710,7 +711,7 @@ def addAccelerometerErrorTerms(self, problem, poseSplineDv, g_w, mSigma=0.0, \ def addGyroscopeErrorTerms(self, problem, poseSplineDv, mSigma=0.0, gyroNoiseScale=1.0, \ g_w=None): - print() + print("") print("Adding gyroscope error terms ({0})".format(self.dataset.topic)) #progress bar @@ -752,7 +753,7 @@ def initBiasSplines(self, poseSpline, splineOrder, biasKnotsPerSecond): seconds = end - start; knots = int(round(seconds * biasKnotsPerSecond)) - print() + print("") print("Initializing the bias splines with %d knots" % (knots)) #initialize the bias splines @@ -778,7 +779,7 @@ def getTransformationFromBodyToImu(self): self.r_b_Dv.toEuclidean())) def findOrientationPrior(self, referenceImu): - print() + print("") print("Estimating imu-imu rotation initial guess.") # build the problem @@ -966,7 +967,7 @@ def addDesignVariables(self, problem): def addAccelerometerErrorTerms(self, problem, poseSplineDv, g_w, mSigma=0.0, \ accelNoiseScale=1.0): - print() + print("") print("Adding accelerometer error terms ({0})".format(self.dataset.topic)) #progress bar @@ -1011,7 +1012,7 @@ def addAccelerometerErrorTerms(self, problem, poseSplineDv, g_w, mSigma=0.0, \ self.accelErrors = accelErrors def addGyroscopeErrorTerms(self, problem, poseSplineDv, mSigma=0.0, gyroNoiseScale=1.0, g_w=None): - print() + print("") print("Adding gyroscope error terms ({0})".format(self.dataset.topic)) #progress bar @@ -1117,7 +1118,7 @@ def addDesignVariables(self, problem): def addAccelerometerErrorTerms(self, problem, poseSplineDv, g_w, mSigma=0.0, \ accelNoiseScale=1.0): - print() + print("") print("Adding accelerometer error terms ({0})".format(self.dataset.topic)) #progress bar diff --git a/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccUtil.py b/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccUtil.py index 58f35b097..ca6165b6a 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccUtil.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccUtil.py @@ -1,3 +1,4 @@ +from __future__ import print_function #handle print in 2.x python from sm import PlotCollection from . import IccPlots as plots import numpy as np @@ -8,6 +9,12 @@ import time from matplotlib.backends.backend_pdf import PdfPages import io +try: + # Python 2 + from cStringIO import StringIO +except ImportError: + # Python 3 + from io import StringIO import matplotlib.patches as patches def printErrorStatistics(cself, dest=sys.stdout): @@ -46,7 +53,7 @@ def printErrorStatistics(cself, dest=sys.stdout): print("Accelerometer error (imu{0}) [m/s^2]: mean {1}, median {2}, std: {3}".format(iidx, np.mean(e2), np.median(e2), np.std(e2)), file=dest) def printGravity(cself): - print() + print("") print("Gravity vector: (in target coordinates): [m/s^2]") print(cself.gravityDv.toEuclidean()) @@ -55,7 +62,7 @@ def printResults(cself, withCov=False): for camNr in range(0,nCams): T_cam_b = cself.CameraChain.getResultTrafoImuToCam(camNr) - print() + print("") print("Transformation T_cam{0}_imu0 (imu0 to cam{0}, T_ci): ".format(camNr)) if withCov and camNr==0: print("\t quaternion: ", T_cam_b.q(), " +- ", cself.std_trafo_ic[0:3]) @@ -63,16 +70,16 @@ def printResults(cself, withCov=False): print(T_cam_b.T()) if not cself.noTimeCalibration: - print() + print("") print("cam{0} to imu0 time: [s] (t_imu = t_cam + shift)".format(camNr)) print(cself.CameraChain.getResultTimeShift(camNr), end=' ') if withCov: print(" +- ", cself.std_times[camNr]) else: - print() + print("") - print() + print("") for (imuNr, imu) in enumerate(cself.ImuList): print("IMU{0}:\n".format(imuNr), "----------------------------") imu.getImuConfig().printDetails() @@ -88,7 +95,7 @@ def printBaselines(self): else: isFixed = "" - print() + print("") print("Baseline (cam{0} to cam{1}): [m] {2}".format(camNr, camNr+1, isFixed)) print(T.T()) print(baseline, "[m]") @@ -100,10 +107,10 @@ def generateReport(cself, filename="report.pdf", showOnScreen=True): offset = 3010 #Output calibration results in text form. - sstream = io.StringIO() + sstream = StringIO() printResultTxt(cself, sstream) - text = [line for line in io.StringIO(sstream.getvalue())] + text = [line for line in StringIO(sstream.getvalue())] linesPerPage = 40 while True: diff --git a/aslam_offline_calibration/kalibr/python/kalibr_rs_camera_calibration/RsCalibrator.py b/aslam_offline_calibration/kalibr/python/kalibr_rs_camera_calibration/RsCalibrator.py index 7e52caffb..cdbbd2408 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_rs_camera_calibration/RsCalibrator.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_rs_camera_calibration/RsCalibrator.py @@ -253,7 +253,7 @@ def __generateInitialSpline(self, splineOrder, timeOffsetPadding, numberOfKnots else: knots = int(round(seconds * framerate/3)) - print() + print("") print("Initializing a pose spline with %d knots (%f knots per second over %f seconds)" % ( knots, 100, seconds)) poseSpline.initPoseSplineSparse(times, curve, knots, 1e-4) @@ -455,7 +455,7 @@ def __printResults(self): shutter = self.__camera_dv.shutterDesignVariable().value() proj = self.__camera_dv.projectionDesignVariable().value() dist = self.__camera_dv.distortionDesignVariable().value() - print() + print("") if (self.__isRollingShutter()): print("LineDelay:") print(shutter.lineDelay()) From a992efad3e2dae639d5cab876ec4cd7f76ee1a80 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Fri, 22 Apr 2022 10:54:03 -0400 Subject: [PATCH 59/75] add clearer comment on adhesion issue from PR #358 --- .../python/kalibr_camera_calibration/MulticamGraph.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/MulticamGraph.py b/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/MulticamGraph.py index b090f10e5..757c8e6f2 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/MulticamGraph.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/MulticamGraph.py @@ -85,9 +85,11 @@ def initializeGraphFromObsDb(self, obs_db): #check if all cams are connected through observations def isGraphConnected(self): if self.numCams == 1: - # Since igraph 0.8, adhesion correctly returns 0 for the non-connected one cam case. - # which evaluates to false later on. So we skip the check and return true in the one camera case. + # Since igraph 0.8, adhesion correctly returns 0 instead of -2147483648 for a graph with a single vertex. + # As 0 evaluates to False later in the process, kalibr exits with the cameras unconnected error. + # So we skip the check and return true in the one camera case. # https://github.com/ethz-asl/kalibr/issues/364 + # https://github.com/ethz-asl/kalibr/pull/358 return True else: #check if all vertices are connected From b954573ff413b6e6a085c2ebdd64842ec5b4aeaf Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Mon, 2 May 2022 18:39:43 -0400 Subject: [PATCH 60/75] git ignore jetbrain editor files --- .gitignore | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/.gitignore b/.gitignore index e720f6d54..434dfde9e 100644 --- a/.gitignore +++ b/.gitignore @@ -44,3 +44,9 @@ CMakeLists.txt.user # MacOS Desktop Services Store .DS_Store + +# Jetbrains Clion +cmake-build-* +.idea +build +Build From ee6b0e6b3c4cc31e55029b5b2696e45707257fa1 Mon Sep 17 00:00:00 2001 From: stefangachter Date: Tue, 28 May 2019 09:06:05 +0200 Subject: [PATCH 61/75] Remove importing PIL not compatible with sensor messages Not needed. And "from PIL import Image" compatible with Ubuntu 18.04 conflicts with "from sensor_msgs.msg import Image" --- aslam_offline_calibration/kalibr/python/kalibr_bagcreater | 1 - 1 file changed, 1 deletion(-) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_bagcreater b/aslam_offline_calibration/kalibr/python/kalibr_bagcreater index 8598f2d03..cd0278a28 100755 --- a/aslam_offline_calibration/kalibr/python/kalibr_bagcreater +++ b/aslam_offline_calibration/kalibr/python/kalibr_bagcreater @@ -5,7 +5,6 @@ import rosbag import rospy from sensor_msgs.msg import Image from sensor_msgs.msg import Imu -import ImageFile import time, sys, os import argparse import cv2 From a4eb023cd8beeb2e03cc337fc70d47e61398db47 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Mon, 2 May 2022 19:18:17 -0400 Subject: [PATCH 62/75] fix warning, and fix not opening imu csv file in text mode (bagcreator) --- aslam_offline_calibration/kalibr/python/kalibr_bagcreater | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_bagcreater b/aslam_offline_calibration/kalibr/python/kalibr_bagcreater index cd0278a28..62511db8d 100755 --- a/aslam_offline_calibration/kalibr/python/kalibr_bagcreater +++ b/aslam_offline_calibration/kalibr/python/kalibr_bagcreater @@ -77,7 +77,7 @@ def loadImageToRosMsg(filename): rosimage.width = image_np.shape[1] rosimage.step = rosimage.width #only with mono8! (step = width * byteperpixel * numChannels) rosimage.encoding = "mono8" - rosimage.data = image_np.tostring() + rosimage.data = image_np.tobytes() return rosimage, timestamp @@ -113,7 +113,7 @@ try: imufiles = getImuCsvFiles(parsed.folder) for imufile in imufiles: topic = os.path.splitext(os.path.basename(imufile))[0] - with open(imufile, 'rb') as csvfile: + with open(imufile, 'r') as csvfile: reader = csv.reader(csvfile, delimiter=',') headers = next(reader, None) for row in reader: From 7b97eccadad1b2cb264a78c03690b1400144ff1e Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Mon, 2 May 2022 19:22:23 -0400 Subject: [PATCH 63/75] don't use byte mode for writing strings to file (bagextractor) --- aslam_offline_calibration/kalibr/python/kalibr_bagextractor | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_bagextractor b/aslam_offline_calibration/kalibr/python/kalibr_bagextractor index b8d3494c5..21f65bac5 100755 --- a/aslam_offline_calibration/kalibr/python/kalibr_bagextractor +++ b/aslam_offline_calibration/kalibr/python/kalibr_bagextractor @@ -84,7 +84,7 @@ if parsed.imu_topics is not None: iProgress.reset(numMsg) iProgress.sample() - with open( "{0}/{1}".format(parsed.output_folder, filename), 'wb') as imufile: + with open( "{0}/{1}".format(parsed.output_folder, filename), 'w') as imufile: spamwriter = csv.writer(imufile, delimiter=',') spamwriter.writerow(["timestamp", "omega_x", "omega_y", "omega_z", "alpha_x", "alpha_y", "alpha_z"]) From f7d42fd7a99cfeb1bf1c3c9cdc8605c61c3770f2 Mon Sep 17 00:00:00 2001 From: Nikhil Khedekar Date: Fri, 27 Sep 2019 16:14:53 -0700 Subject: [PATCH 64/75] Update kalibr_camera_focus solves #247 --- aslam_offline_calibration/kalibr/python/kalibr_camera_focus | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_camera_focus b/aslam_offline_calibration/kalibr/python/kalibr_camera_focus index bf6895ca3..369395766 100755 --- a/aslam_offline_calibration/kalibr/python/kalibr_camera_focus +++ b/aslam_offline_calibration/kalibr/python/kalibr_camera_focus @@ -2,7 +2,6 @@ print("importing libraries") import rospy import cv2 -import cv import numpy as np from sensor_msgs.msg import Image from cv_bridge import CvBridge, CvBridgeError @@ -12,7 +11,6 @@ class CameraFocus: def __init__(self, topic): self.topic = topic self.windowNameOrig = "Camera: {0}".format(self.topic) - cv2.namedWindow(self.windowNameOrig, 2) self.bridge = CvBridge() self.image_sub = rospy.Subscriber(self.topic, Image, self.callback) @@ -58,4 +56,4 @@ if __name__ == "__main__": except KeyboardInterrupt: print("Shutting down") - cv.DestroyAllWindows() + cv2.DestroyAllWindows() From 438f7545a303a8c8bf0699623879f149a9fec71e Mon Sep 17 00:00:00 2001 From: alejandro Date: Wed, 16 Oct 2019 11:55:51 +0200 Subject: [PATCH 65/75] Fixed azimuthal/azumithal typos and gave better control to the user over camera calibration debug windows --- .../src/GridCalibrationTargetCheckerboard.cpp | 3 ++- .../aslam_cameras/src/GridCalibrationTargetCirclegrid.cpp | 1 + aslam_cv/aslam_cameras/src/GridDetector.cpp | 1 + .../src/GridCalibrationTargetAprilgrid.cpp | 2 ++ .../python/kalibr_camera_calibration/CameraCalibrator.py | 8 ++++---- 5 files changed, 10 insertions(+), 5 deletions(-) diff --git a/aslam_cv/aslam_cameras/src/GridCalibrationTargetCheckerboard.cpp b/aslam_cv/aslam_cameras/src/GridCalibrationTargetCheckerboard.cpp index 229c11305..03d27b660 100644 --- a/aslam_cv/aslam_cameras/src/GridCalibrationTargetCheckerboard.cpp +++ b/aslam_cv/aslam_cameras/src/GridCalibrationTargetCheckerboard.cpp @@ -50,7 +50,8 @@ GridCalibrationTargetCheckerboard::GridCalibrationTargetCheckerboard( void GridCalibrationTargetCheckerboard::initialize() { if (_options.showExtractionVideo) { - cv::namedWindow("Checkerboard corners", cv::WINDOW_AUTOSIZE); + cv::namedWindow("Checkerboard corners", cv::WINDOW_NORMAL); + cv::resizeWindow("Checkerboard corners", 640, 480); cv::startWindowThread(); } } diff --git a/aslam_cv/aslam_cameras/src/GridCalibrationTargetCirclegrid.cpp b/aslam_cv/aslam_cameras/src/GridCalibrationTargetCirclegrid.cpp index 6d2695936..c8eca9856 100644 --- a/aslam_cv/aslam_cameras/src/GridCalibrationTargetCirclegrid.cpp +++ b/aslam_cv/aslam_cameras/src/GridCalibrationTargetCirclegrid.cpp @@ -31,6 +31,7 @@ void GridCalibrationTargetCirclegrid::initialize() { if (_options.showExtractionVideo) { cv::namedWindow("Circlegrid corners", cv::WINDOW_AUTOSIZE); + cv::resizeWindow("Circlegrid corners", 640, 480); cv::startWindowThread(); } } diff --git a/aslam_cv/aslam_cameras/src/GridDetector.cpp b/aslam_cv/aslam_cameras/src/GridDetector.cpp index 85f63b4ef..31d4fe9ac 100644 --- a/aslam_cv/aslam_cameras/src/GridDetector.cpp +++ b/aslam_cv/aslam_cameras/src/GridDetector.cpp @@ -33,6 +33,7 @@ void GridDetector::initializeDetector() { if (_options.plotCornerReprojection) { cv::namedWindow("Corner reprojection", cv::WINDOW_NORMAL); + cv::resizeWindow("Corner reprojection", 640, 480); } } diff --git a/aslam_cv/aslam_cameras_april/src/GridCalibrationTargetAprilgrid.cpp b/aslam_cv/aslam_cameras_april/src/GridCalibrationTargetAprilgrid.cpp index 28b0dd0dc..fec066431 100644 --- a/aslam_cv/aslam_cameras_april/src/GridCalibrationTargetAprilgrid.cpp +++ b/aslam_cv/aslam_cameras_april/src/GridCalibrationTargetAprilgrid.cpp @@ -57,7 +57,9 @@ void GridCalibrationTargetAprilgrid::initialize() { if (_options.showExtractionVideo) { cv::namedWindow("Aprilgrid: Tag detection", cv::WINDOW_NORMAL); + cv::resizeWindow("Aprilgrid: Tag detection", 640, 480); cv::namedWindow("Aprilgrid: Tag corners", cv::WINDOW_NORMAL); + cv::resizeWindow("Aprilgrid: Tag corners", 640, 480); } //create the tag detector diff --git a/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/CameraCalibrator.py b/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/CameraCalibrator.py index 4fa1490e7..07168e67e 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/CameraCalibrator.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/CameraCalibrator.py @@ -458,7 +458,7 @@ def plotAzumithalError(cself, cam_id, fno=1, clearFigure=True, stats=None, noSho if stats is None: stats = getAllPointStatistics(cself, cam_id) angleError = np.array([ [ np.degrees(s.azumithalAngle), math.sqrt(s.squaredError)] for s in stats ]) - # sort by azumithal angle + # sort by azimuthal angle sae = angleError[ angleError[:,0].argsort() ] # Now plot f = pl.figure(fno) @@ -469,12 +469,12 @@ def plotAzumithalError(cself, cam_id, fno=1, clearFigure=True, stats=None, noSho pl.subplot(121) pl.plot(sae[:,0],sae[:,1],'bx-') pl.grid('on') - pl.xlabel('azumithal angle (deg)') + pl.xlabel('azimuthal angle (deg)') pl.ylabel('reprojection error (pixels)') pl.subplot(122) pl.hist(sae[:,0]) pl.grid('on') - pl.xlabel('azumithal angle (deg)') + pl.xlabel('azimuthal angle (deg)') pl.ylabel('count') if not noShow: pl.show() @@ -772,7 +772,7 @@ def generateReport(cself, filename="report.pdf", showOnScreen=True, graph=None, plotter.add_figure(title, f) figs.append(f) f = pl.figure(cidx*10+2) - title="cam{0}: azimutal error".format(cidx) + title="cam{0}: azimuthal error".format(cidx) plotAzumithalError(cself, cidx, fno=f.number, noShow=True, title=title) plotter.add_figure(title, f) figs.append(f) From 8c660886bca3288b86eb52b65b80a046bab34bc3 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Mon, 2 May 2022 19:51:27 -0400 Subject: [PATCH 66/75] remove temp instructions from readme --- README.md | 43 ------------------------------------------- 1 file changed, 43 deletions(-) diff --git a/README.md b/README.md index 5a6736369..f33faf0db 100644 --- a/README.md +++ b/README.md @@ -17,49 +17,6 @@ Kalibr is a toolbox that solves the following calibration problems: **For questions or comments, please open an issue on Github.** -## Installation - -### Ubuntu 20.04 - -We've upgraded and fixed kalibr at ORI for 20.04. Please use our fork: `git clone https://github.com/ori-drs/kalibr.git --branch noetic-devel`. - -- Use `rosdep` to install almost all required dependencies: `rosdep install --from-paths ./ -iry`. -- Then install the two missing runtime dependencies: `sudo apt install python3-wxgtk4.0 python3-igraph` -- Unittests are currently failing on 20.04 and thus deactivated on the buildserver. - - -### Docker - -First make sure that you have install docker on your system using the official Docker [Get Docker](https://docs.docker.com/get-docker/) guide. -We can then build the docker container using: -``` -cd /src/kalibr -docker build -t kalibr -f Dockerfile_ros1_20_04 . -``` - -Then after building we can first download an example dataset and then perform following to calibrate. -``` -cd -mkdir example && cd example -wget http://robotics.ethz.ch/~asl-datasets/ijrr_euroc_mav_dataset/calibration_datasets/cam_april/april_6x6.yaml -wget http://robotics.ethz.ch/~asl-datasets/ijrr_euroc_mav_dataset/calibration_datasets/cam_april/cam_april.bag -``` - -We can now mount the data folder in the container `/data` path and enter the command prompt. -Some more details can be found on the [ROS wiki](http://wiki.ros.org/docker/Tutorials/GUI) for Docker. -``` -FOLDER=$(pwd) -xhost +local:root -docker run -it -e "DISPLAY" -e "QT_X11_NO_MITSHM=1" \ - -v "/tmp/.X11-unix:/tmp/.X11-unix:rw" \ - -v "$FOLDER:/data" \ - kalibr -source devel/setup.bash -rosrun kalibr kalibr_calibrate_cameras \ - --bag /data/cam_april.bag --target /data/april_6x6.yaml \ - --models pinhole-radtan pinhole-radtan \ - --topics /cam0/image_raw /cam1/image_raw -``` ## Tutorial: IMU-camera calibration From c04c6e93f267729ff78a019ed11cfa5e58499c45 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Mon, 2 May 2022 20:01:13 -0400 Subject: [PATCH 67/75] fix destroyAllWindows call --- aslam_offline_calibration/kalibr/python/kalibr_camera_focus | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_camera_focus b/aslam_offline_calibration/kalibr/python/kalibr_camera_focus index 369395766..84c5ab074 100755 --- a/aslam_offline_calibration/kalibr/python/kalibr_camera_focus +++ b/aslam_offline_calibration/kalibr/python/kalibr_camera_focus @@ -55,5 +55,5 @@ if __name__ == "__main__": rospy.spin() except KeyboardInterrupt: print("Shutting down") - - cv2.DestroyAllWindows() + + cv2.destroyAllWindows() From dc43f1efbfc61a60c183c9ed890394ada9cfc762 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Mon, 2 May 2022 20:15:18 -0400 Subject: [PATCH 68/75] small fix to ensure coords are ints --- .../kalibr/python/kalibr_camera_focus | 5 +++-- .../kalibr/python/kalibr_camera_validator | 14 +++++++++----- 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_camera_focus b/aslam_offline_calibration/kalibr/python/kalibr_camera_focus index 84c5ab074..646c319fa 100755 --- a/aslam_offline_calibration/kalibr/python/kalibr_camera_focus +++ b/aslam_offline_calibration/kalibr/python/kalibr_camera_focus @@ -47,10 +47,11 @@ if __name__ == "__main__": parser.add_argument('--topic', nargs='+', dest='topics', help='camera topic', required=True) parsed = parser.parse_args() + rospy.init_node('kalibr_camera_focus', anonymous=True) + for topic in parsed.topics: camval = CameraFocus(topic) - - rospy.init_node('kalibr_validator', anonymous=True) + try: rospy.spin() except KeyboardInterrupt: diff --git a/aslam_offline_calibration/kalibr/python/kalibr_camera_validator b/aslam_offline_calibration/kalibr/python/kalibr_camera_validator index 4a008dd56..28e682e96 100755 --- a/aslam_offline_calibration/kalibr/python/kalibr_camera_validator +++ b/aslam_offline_calibration/kalibr/python/kalibr_camera_validator @@ -192,7 +192,8 @@ class CameraChainValidator(object): def generatePairView(self, camAnr, camBnr): #prepare the window windowName = "Rectified view (cam{0} and cam{1})".format(camAnr, camBnr) - cv2.namedWindow(windowName, cv2.WINDOW_AUTOSIZE) + cv2.namedWindow(windowName, cv2.WINDOW_NORMAL) + cv2.resizeWindow(windowName, (640, 480)) #get the mono validators for each cam camA = self.monovalidators[camAnr] @@ -292,7 +293,7 @@ class CameraChainValidator(object): cv2.putText(np_image_rect, "{0: .4f}".format(err_val), (x+100,y), cv2.FONT_HERSHEY_SIMPLEX, fontScale=fontScale, color=(0, 0, 255), thickness=2) else: - cv2.putText(np_image_rect, "Detection failed...", (np_image_rect.shape[0]/2,np_image_rect.shape[1]/5), cv2.FONT_HERSHEY_SIMPLEX, fontScale=2, color=(0, 0, 255), thickness=3) + cv2.putText(np_image_rect, "Detection failed...", (int(np_image_rect.shape[0]/2.0),int(np_image_rect.shape[1]/5.0)), cv2.FONT_HERSHEY_SIMPLEX, fontScale=2, color=(0, 0, 255), thickness=3) cv2.imshow(windowName, np_image_rect) @@ -399,7 +400,8 @@ class MonoCameraValidator(object): def generateMonoview(self, np_image, observation, obs_valid): - cv2.namedWindow(self.windowName, cv2.WINDOW_AUTOSIZE) + cv2.namedWindow(self.windowName, cv2.WINDOW_NORMAL) + cv2.resizeWindow(self.windowName, (640, 480)) np_image = cv2.cvtColor(np_image, cv2.COLOR_GRAY2BGR) if obs_valid: @@ -451,7 +453,7 @@ class MonoCameraValidator(object): cv2.circle(np_image, (px_fix, py_fix), radius=radius_fix, color=(255,255,0), thickness=thickness, shift=shift, lineType=cv2.LINE_AA) else: - cv2.putText(np_image, "Detection failed...", (np_image.shape[0]/2,np_image.shape[1]/5), cv2.FONT_HERSHEY_SIMPLEX, fontScale=1.5, color=(0, 0, 255), thickness=2) + cv2.putText(np_image, "Detection failed...", (int(np_image.shape[0]/2.0),int(np_image.shape[1]/5.0)), cv2.FONT_HERSHEY_SIMPLEX, fontScale=1.5, color=(0, 0, 255), thickness=2) cv2.imshow(self.windowName, np_image) cv2.waitKey(1) @@ -469,6 +471,9 @@ if __name__ == "__main__": else: sm.setLoggingLevel(sm.LoggingLevel.Info) + # init the node + rospy.init_node('kalibr_camera_validator', anonymous=True) + #load the configuration yamls targetConfig = kc.ConfigReader.CalibrationTargetParameters(parsed.targetYaml) @@ -480,7 +485,6 @@ if __name__ == "__main__": chain_validator = CameraChainValidator(camchain, targetConfig) #ros message loops - rospy.init_node('kalibr_validator', anonymous=True) try: rospy.spin() except KeyboardInterrupt: From 80cfc5bcd2b5d3eae239066ef45f934987831bbb Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Mon, 2 May 2022 20:30:19 -0400 Subject: [PATCH 69/75] fixes for multi-cam validator, don't recreate windows to allow for resizing --- .../kalibr/python/kalibr_camera_focus | 4 ++- .../kalibr/python/kalibr_camera_validator | 36 +++++++++++-------- 2 files changed, 24 insertions(+), 16 deletions(-) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_camera_focus b/aslam_offline_calibration/kalibr/python/kalibr_camera_focus index 646c319fa..cfb81047f 100755 --- a/aslam_offline_calibration/kalibr/python/kalibr_camera_focus +++ b/aslam_offline_calibration/kalibr/python/kalibr_camera_focus @@ -50,7 +50,9 @@ if __name__ == "__main__": rospy.init_node('kalibr_camera_focus', anonymous=True) for topic in parsed.topics: - camval = CameraFocus(topic) + camval = CameraFocus(topic) + cv2.namedWindow(camval.windowNameOrig, cv2.WINDOW_NORMAL) + cv2.resizeWindow(camval.windowNameOrig, (640, 480)) try: rospy.spin() diff --git a/aslam_offline_calibration/kalibr/python/kalibr_camera_validator b/aslam_offline_calibration/kalibr/python/kalibr_camera_validator index 28e682e96..4df0df4bf 100755 --- a/aslam_offline_calibration/kalibr/python/kalibr_camera_validator +++ b/aslam_offline_calibration/kalibr/python/kalibr_camera_validator @@ -68,12 +68,14 @@ class CameraChainValidator(object): #initialize the cameras in the chain self.G = igraph.Graph(self.numCameras) - self.monovalidators=[] + self.monovalidators = [] for cidx in range(0, self.numCameras): camConfig = chainConfig.getCameraParameters(cidx) #create a mono instance for each cam (detection and mono view) monovalidator = MonoCameraValidator(camConfig, targetParams) + cv2.namedWindow(monovalidator.windowName, cv2.WINDOW_NORMAL) + cv2.resizeWindow(monovalidator.windowName, (640, 480)) self.monovalidators.append(monovalidator) #add edges to overlap graph @@ -89,15 +91,23 @@ class CameraChainValidator(object): for edge in self.G.es: cidx_src = edge.source cidx_dest = edge.target - - edge["rect_map"]=dict();edge["R"]=dict();edge["A"]=dict(); - + edge["rect_map"] = dict() + edge["R"] = dict() + edge["A"] = dict() edge["rect_map"][cidx_src], \ edge["rect_map"][cidx_dest], \ edge["R"][cidx_src], \ edge["R"][cidx_dest], \ edge["A"][cidx_src], \ edge["A"][cidx_dest] = self.prepareStereoRectificationMaps(cidx_src, cidx_dest) + + # generate the windows + for edge in self.G.es: + cidx_src = edge.source + cidx_dest = edge.target + windowName = "Rectified view (cam{0} and cam{1})".format(cidx_src, cidx_dest) + cv2.namedWindow(windowName, cv2.WINDOW_NORMAL) + cv2.resizeWindow(windowName, (2*640, 480)) #register the callback for the synchronized images sync_sub = message_filters.ApproximateTimeSynchronizer([val.image_sub for val in self.monovalidators], 10, 0.02) @@ -115,7 +125,7 @@ class CameraChainValidator(object): self.timeLast = timeNow #process the images of all cameras - self.observations=[]; + self.observations=[] for cam_nr, msg in enumerate(cam_msgs): #convert image to numpy @@ -190,11 +200,7 @@ class CameraChainValidator(object): return np_rect_image def generatePairView(self, camAnr, camBnr): - #prepare the window - windowName = "Rectified view (cam{0} and cam{1})".format(camAnr, camBnr) - cv2.namedWindow(windowName, cv2.WINDOW_NORMAL) - cv2.resizeWindow(windowName, (640, 480)) - + #get the mono validators for each cam camA = self.monovalidators[camAnr] camB = self.monovalidators[camBnr] @@ -242,7 +248,7 @@ class CameraChainValidator(object): reproj_errs = np.hstack((reproj_errs, reprojErr_B.flatten())) reproj_errs = reproj_errs[2:] - reproj_errs = reproj_errs.reshape(2, reproj_errs.shape[0]/2) + reproj_errs = reproj_errs.reshape(2, int(reproj_errs.shape[0]/2.0)) #rectify the undistorted images edge_idx = self.G.get_eid(camAnr, camBnr) @@ -258,7 +264,7 @@ class CameraChainValidator(object): np_image_rect = cv2.cvtColor(np_image_rect, cv2.COLOR_GRAY2BGR) n=10 for i in range(0,n): - y = np_image_rect.shape[0]*i/n + y = int(np_image_rect.shape[0]*i/n) cv2.line(np_image_rect, (0,y), (2*np_image_rect.shape[1],y),(0,255,0)); @@ -294,7 +300,8 @@ class CameraChainValidator(object): else: cv2.putText(np_image_rect, "Detection failed...", (int(np_image_rect.shape[0]/2.0),int(np_image_rect.shape[1]/5.0)), cv2.FONT_HERSHEY_SIMPLEX, fontScale=2, color=(0, 0, 255), thickness=3) - + + windowName = "Rectified view (cam{0} and cam{1})".format(camAnr, camBnr) cv2.imshow(windowName, np_image_rect) @@ -400,8 +407,7 @@ class MonoCameraValidator(object): def generateMonoview(self, np_image, observation, obs_valid): - cv2.namedWindow(self.windowName, cv2.WINDOW_NORMAL) - cv2.resizeWindow(self.windowName, (640, 480)) + np_image = cv2.cvtColor(np_image, cv2.COLOR_GRAY2BGR) if obs_valid: From 354d9b58f2c7260c94b151d77d6b78aa141680b3 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Mon, 2 May 2022 20:51:49 -0400 Subject: [PATCH 70/75] fix yaml formatting in newer PyYAML versions, and don't wrap lines --- .../kalibr/python/kalibr_common/ConfigReader.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_common/ConfigReader.py b/aslam_offline_calibration/kalibr/python/kalibr_common/ConfigReader.py index 6cbe2fa90..1cd2f4c05 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_common/ConfigReader.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_common/ConfigReader.py @@ -220,7 +220,7 @@ def writeYaml(self, filename=None): try: with open(filename, 'w') as outfile: - outfile.write( yaml.dump(self.data) ) + outfile.write( yaml.dump(self.data, default_flow_style=None, width=2147483647) ) except: self.raiseError( "Could not write configuration to {0}".format(self.yamlFile) ) From df701e4c442a385fa24d0aaa29536d00bddb327d Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Tue, 3 May 2022 14:40:52 -0400 Subject: [PATCH 71/75] specify threads more clearly --- .../kalibr/python/kalibr_calibrate_cameras | 2 ++ .../kalibr/python/kalibr_calibrate_rs_cameras | 27 +++++++------------ .../IccSensors.py | 4 +-- .../icc_calibrator.py | 7 ++--- .../RsCalibrator.py | 3 ++- 5 files changed, 19 insertions(+), 24 deletions(-) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_calibrate_cameras b/aslam_offline_calibration/kalibr/python/kalibr_calibrate_cameras index 33c60afd9..b4a7e4a6e 100755 --- a/aslam_offline_calibration/kalibr/python/kalibr_calibrate_cameras +++ b/aslam_offline_calibration/kalibr/python/kalibr_calibrate_cameras @@ -14,6 +14,7 @@ import kalibr_camera_calibration as kcc import cv2 import os import numpy as np +import multiprocessing import pylab as pl import argparse import sys @@ -245,6 +246,7 @@ def main(): optimizerOptions = calibrator.estimator.getOptimizerOptions() optimizerOptions.maxIterations = 50 + optimizerOptions.nThreads = max(1,multiprocessing.cpu_count()-1) optimizerOptions.verbose = parsed.verbose verbose = parsed.verbose diff --git a/aslam_offline_calibration/kalibr/python/kalibr_calibrate_rs_cameras b/aslam_offline_calibration/kalibr/python/kalibr_calibrate_rs_cameras index e3723f3e9..8c3b30bc3 100755 --- a/aslam_offline_calibration/kalibr/python/kalibr_calibrate_rs_cameras +++ b/aslam_offline_calibration/kalibr/python/kalibr_calibrate_rs_cameras @@ -120,22 +120,6 @@ def __parseArgs(): return parsed -# perform corner extraction from bagfile -# returns the observations -def __extractObservations(cameraGeometry, verbose=False): - - # extract the targets - multithreading = False #not verbose - observations = kc.extractCornersFromDataset( - cameraGeometry.dataset, - cameraGeometry.ctarget.detector, - multithreading=multithreading, - # transformation estimation will fail with rs cameras and significant distortions - noTransformation=True - ) - - return observations - def main(): parsed = __parseArgs() @@ -157,8 +141,15 @@ def main(): cameraModel = cameraModels[parsed.model] cameraGeometry = kcc.CameraGeometry(cameraModel, targetConfig, dataset, verbose=(parsed.verbose or parsed.showextraction)) - # extract observations - observations = __extractObservations(cameraGeometry,(parsed.verbose or parsed.showextraction)) + # extract observations of the targets + multithreading = not (parsed.verbose or parsed.showextraction) + observations = kc.extractCornersFromDataset( + cameraGeometry.dataset, + cameraGeometry.ctarget.detector, + multithreading=multithreading, + # transformation estimation will fail with rs cameras and significant distortions + noTransformation=True + ) # Calibration Configuration config = rscc.RsCalibratorConfiguration() diff --git a/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccSensors.py b/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccSensors.py index 68564f8f2..7eb468d0b 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccSensors.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccSensors.py @@ -158,7 +158,7 @@ def findOrientationPriorCameraToImu(self, imu): #define the optimization options = aopt.Optimizer2Options() options.verbose = False - options.linearSolver = aopt.BlockCholeskyLinearSystemSolver() + options.linearSolver = aopt.BlockCholeskyLinearSystemSolver() #does not have multi-threading support options.nThreads = 2 options.convergenceDeltaX = 1e-4 options.convergenceDeltaJ = 1 @@ -826,7 +826,7 @@ def findOrientationPrior(self, referenceImu): #define the optimization options = aopt.Optimizer2Options() options.verbose = False - options.linearSolver = aopt.BlockCholeskyLinearSystemSolver() + options.linearSolver = aopt.BlockCholeskyLinearSystemSolver() #does not have multi-threading support options.nThreads = 2 options.convergenceDeltaX = 1e-4 options.convergenceDeltaJ = 1 diff --git a/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/icc_calibrator.py b/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/icc_calibrator.py index d375bd79f..d79a2549f 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/icc_calibrator.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/icc_calibrator.py @@ -160,18 +160,19 @@ def optimize(self, options=None, maxIterations=30, recoverCov=False): options.convergenceDeltaJ = 1e-2 options.maxIterations = maxIterations options.trustRegionPolicy = aopt.LevenbergMarquardtTrustRegionPolicy(options.levenbergMarquardtLambdaInit) - options.linearSolver = aopt.BlockCholeskyLinearSystemSolver() + options.linearSolver = aopt.BlockCholeskyLinearSystemSolver() #does not have multi-threading support #run the optimization self.optimizer = aopt.Optimizer2(options) self.optimizer.setProblem(self.problem) optimizationFailed=False - try: + try: retval = self.optimizer.optimize() if retval.linearSolverFailure: optimizationFailed = True - except: + except Exception as e: + sm.logError(str(e)) optimizationFailed = True if optimizationFailed: diff --git a/aslam_offline_calibration/kalibr/python/kalibr_rs_camera_calibration/RsCalibrator.py b/aslam_offline_calibration/kalibr/python/kalibr_rs_camera_calibration/RsCalibrator.py index cdbbd2408..cc959039e 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_rs_camera_calibration/RsCalibrator.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_rs_camera_calibration/RsCalibrator.py @@ -430,8 +430,9 @@ def __runOptimization(self, problem ,deltaJ, deltaX, maxIt): # verbose and choldmod solving with schur complement trick options = aopt.Optimizer2Options() options.verbose = True - options.linearSolver = aopt.BlockCholeskyLinearSystemSolver() + options.nThreads = max(1,multiprocessing.cpu_count()-1) options.doSchurComplement = True + options.linearSolver = aopt.BlockCholeskyLinearSystemSolver() #does not have multi-threading support # stopping criteria options.maxIterations = maxIt From 2973a898077e3aa458e56b9991dbd5d5423cdfed Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Tue, 3 May 2022 15:11:10 -0400 Subject: [PATCH 72/75] use original IccSensors filename --- .../{icc_calibrator.py => IccCalibrator.py} | 0 .../IccSensors.py | 43 ++++++++++--------- .../kalibr_imu_camera_calibration/__init__.py | 2 +- 3 files changed, 23 insertions(+), 22 deletions(-) rename aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/{icc_calibrator.py => IccCalibrator.py} (100%) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/icc_calibrator.py b/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccCalibrator.py similarity index 100% rename from aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/icc_calibrator.py rename to aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccCalibrator.py diff --git a/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccSensors.py b/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccSensors.py index 7eb468d0b..64213c079 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccSensors.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccSensors.py @@ -7,7 +7,8 @@ import bsplines import kalibr_common as kc import kalibr_errorterms as ket -from . import icc_calibrator as ic +from . import IccCalibrator as ic +from .IccCalibrator import * import cv2 import sys @@ -314,7 +315,7 @@ def initPoseSplineFromCamera(self, splineOrder=6, poseKnotsPerSecond=100, timeOf pose.initPoseSplineSparse(times, curve, knots, 1e-4) return pose - def addDesignVariables(self, problem, noExtrinsics=True, noTimeCalibration=True, baselinedv_group_id=ic.HELPER_GROUP_ID): + def addDesignVariables(self, problem, noExtrinsics=True, noTimeCalibration=True, baselinedv_group_id=HELPER_GROUP_ID): # Add the calibration design variables. active = not noExtrinsics self.T_c_b_Dv = aopt.TransformationDv(self.T_extrinsic, rotationActive=active, translationActive=active) @@ -324,7 +325,7 @@ def addDesignVariables(self, problem, noExtrinsics=True, noTimeCalibration=True, # Add the time delay design variable. self.cameraTimeToImuTimeDv = aopt.Scalar(0.0) self.cameraTimeToImuTimeDv.setActive( not noTimeCalibration ) - problem.addDesignVariable(self.cameraTimeToImuTimeDv, ic.CALIBRATION_GROUP_ID) + problem.addDesignVariable(self.cameraTimeToImuTimeDv, CALIBRATION_GROUP_ID) def addCameraErrorTerms(self, problem, poseSplineDv, T_cN_b, blakeZissermanDf=0.0, timeOffsetPadding=0.0): print("") @@ -521,10 +522,10 @@ def addDesignVariables(self, problem, noTimeCalibration = True, noChainExtrinsic #the first "baseline" dv is between the imu and cam0 if camNr == 0: noExtrinsics = False - baselinedv_group_id = ic.CALIBRATION_GROUP_ID + baselinedv_group_id = CALIBRATION_GROUP_ID else: noExtrinsics = noChainExtrinsics - baselinedv_group_id = ic.HELPER_GROUP_ID + baselinedv_group_id = HELPER_GROUP_ID cam.addDesignVariables(problem, noExtrinsics, noTimeCalibration, baselinedv_group_id=baselinedv_group_id) #add the reprojection error terms for all cameras in the chain @@ -649,16 +650,16 @@ def addDesignVariables(self, problem): self.gyroBiasDv = asp.EuclideanBSplineDesignVariable( self.gyroBias ) self.accelBiasDv = asp.EuclideanBSplineDesignVariable( self.accelBias ) - ic.addSplineDesignVariables(problem, self.gyroBiasDv, setActive=True, \ - group_id=ic.HELPER_GROUP_ID) - ic.addSplineDesignVariables(problem, self.accelBiasDv, setActive=True, \ - group_id=ic.HELPER_GROUP_ID) + addSplineDesignVariables(problem, self.gyroBiasDv, setActive=True, + group_id=HELPER_GROUP_ID) + addSplineDesignVariables(problem, self.accelBiasDv, setActive=True, + group_id=HELPER_GROUP_ID) self.q_i_b_Dv = aopt.RotationQuaternionDv(self.q_i_b_prior) - problem.addDesignVariable(self.q_i_b_Dv, ic.HELPER_GROUP_ID) + problem.addDesignVariable(self.q_i_b_Dv, HELPER_GROUP_ID) self.q_i_b_Dv.setActive(False) self.r_b_Dv = aopt.EuclideanPointDv(np.array([0., 0., 0.])) - problem.addDesignVariable(self.r_b_Dv, ic.HELPER_GROUP_ID) + problem.addDesignVariable(self.r_b_Dv, HELPER_GROUP_ID) self.r_b_Dv.setActive(False) if not self.isReferenceImu: @@ -948,21 +949,21 @@ def addDesignVariables(self, problem): IccImu.addDesignVariables(self, problem) self.q_gyro_i_Dv = aopt.RotationQuaternionDv(np.array([0., 0., 0., 1.])) - problem.addDesignVariable(self.q_gyro_i_Dv, ic.HELPER_GROUP_ID) + problem.addDesignVariable(self.q_gyro_i_Dv, HELPER_GROUP_ID) self.q_gyro_i_Dv.setActive(True) self.M_accel_Dv = aopt.MatrixBasicDv(np.eye(3), np.array([[1, 0, 0],[1, 1, 0],[1, 1, 1]], \ dtype=int)) - problem.addDesignVariable(self.M_accel_Dv, ic.HELPER_GROUP_ID) + problem.addDesignVariable(self.M_accel_Dv, HELPER_GROUP_ID) self.M_accel_Dv.setActive(True) self.M_gyro_Dv = aopt.MatrixBasicDv(np.eye(3), np.array([[1, 0, 0],[1, 1, 0],[1, 1, 1]], \ dtype=int)) - problem.addDesignVariable(self.M_gyro_Dv, ic.HELPER_GROUP_ID) + problem.addDesignVariable(self.M_gyro_Dv, HELPER_GROUP_ID) self.M_gyro_Dv.setActive(True) self.M_accel_gyro_Dv = aopt.MatrixBasicDv(np.zeros((3,3)),np.ones((3,3),dtype=int)) - problem.addDesignVariable(self.M_accel_gyro_Dv, ic.HELPER_GROUP_ID) + problem.addDesignVariable(self.M_accel_gyro_Dv, HELPER_GROUP_ID) self.M_accel_gyro_Dv.setActive(True) def addAccelerometerErrorTerms(self, problem, poseSplineDv, g_w, mSigma=0.0, \ @@ -1095,25 +1096,25 @@ def addDesignVariables(self, problem): IccScaledMisalignedImu.addDesignVariables(self, problem) self.rx_i_Dv = aopt.EuclideanPointDv(np.array([0., 0., 0.])) - problem.addDesignVariable(self.rx_i_Dv, ic.HELPER_GROUP_ID) + problem.addDesignVariable(self.rx_i_Dv, HELPER_GROUP_ID) self.rx_i_Dv.setActive(False) self.ry_i_Dv = aopt.EuclideanPointDv(np.array([0., 0., 0.])) - problem.addDesignVariable(self.ry_i_Dv, ic.HELPER_GROUP_ID) + problem.addDesignVariable(self.ry_i_Dv, HELPER_GROUP_ID) self.ry_i_Dv.setActive(True) self.rz_i_Dv = aopt.EuclideanPointDv(np.array([0., 0., 0.])) - problem.addDesignVariable(self.rz_i_Dv, ic.HELPER_GROUP_ID) + problem.addDesignVariable(self.rz_i_Dv, HELPER_GROUP_ID) self.rz_i_Dv.setActive(True) self.Ix_Dv = aopt.MatrixBasicDv(np.diag([1.,0.,0.]), np.zeros((3,3),dtype=int)) - problem.addDesignVariable(self.Ix_Dv, ic.HELPER_GROUP_ID) + problem.addDesignVariable(self.Ix_Dv, HELPER_GROUP_ID) self.Ix_Dv.setActive(False) self.Iy_Dv = aopt.MatrixBasicDv(np.diag([0.,1.,0.]), np.zeros((3,3),dtype=int)) - problem.addDesignVariable(self.Iy_Dv, ic.HELPER_GROUP_ID) + problem.addDesignVariable(self.Iy_Dv, HELPER_GROUP_ID) self.Iy_Dv.setActive(False) self.Iz_Dv = aopt.MatrixBasicDv(np.diag([0.,0.,1.]), np.zeros((3,3),dtype=int)) - problem.addDesignVariable(self.Iz_Dv, ic.HELPER_GROUP_ID) + problem.addDesignVariable(self.Iz_Dv, HELPER_GROUP_ID) self.Iz_Dv.setActive(False) def addAccelerometerErrorTerms(self, problem, poseSplineDv, g_w, mSigma=0.0, \ diff --git a/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/__init__.py b/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/__init__.py index 17b09d7bb..1c4c3ecef 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/__init__.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/__init__.py @@ -1,4 +1,4 @@ -from .icc_calibrator import * +from .IccCalibrator import * from . import IccUtil as util from . import IccPlots as plots from . import IccSensors as sens From a91448a02a6622888c48e6d800f0d5ca8f907ea2 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Tue, 3 May 2022 15:13:48 -0400 Subject: [PATCH 73/75] for now disable the industrial ci since we have dockers now --- ...{industrial_ci_action.yml => industrial_ci_action.yml.disable} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename .github/workflows/{industrial_ci_action.yml => industrial_ci_action.yml.disable} (100%) diff --git a/.github/workflows/industrial_ci_action.yml b/.github/workflows/industrial_ci_action.yml.disable similarity index 100% rename from .github/workflows/industrial_ci_action.yml rename to .github/workflows/industrial_ci_action.yml.disable From 73df2a30d2a8ff324b17e20127f81648dd9a8c6c Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Tue, 3 May 2022 16:01:09 -0400 Subject: [PATCH 74/75] split builds so we can have seperate status badges --- .github/workflows/docker_1604_build.yaml | 23 +++++++++++ .github/workflows/docker_1804_build.yaml | 23 +++++++++++ .github/workflows/docker_2004_build.yaml | 23 +++++++++++ .github/workflows/docker_build.yaml | 52 ------------------------ 4 files changed, 69 insertions(+), 52 deletions(-) create mode 100644 .github/workflows/docker_1604_build.yaml create mode 100644 .github/workflows/docker_1804_build.yaml create mode 100644 .github/workflows/docker_2004_build.yaml delete mode 100644 .github/workflows/docker_build.yaml diff --git a/.github/workflows/docker_1604_build.yaml b/.github/workflows/docker_1604_build.yaml new file mode 100644 index 000000000..38365a2be --- /dev/null +++ b/.github/workflows/docker_1604_build.yaml @@ -0,0 +1,23 @@ +name: "ROS1 Ubuntu 16.04" + +on: + push: + branches: [ master, dockerfile ] + pull_request: + +jobs: + build: + runs-on: ubuntu-latest + steps: + - name: Code Checkout + uses: actions/checkout@v2 + - name: Create Workspace + run: | + export REPO=$(basename $GITHUB_REPOSITORY) && + cd $GITHUB_WORKSPACE/.. && mkdir src/ && + mv $REPO/ src/ && mkdir $REPO/ && mv src/ $REPO/ && cd $REPO/ + - name: Build Docker + run: | + export REPO=$(basename $GITHUB_REPOSITORY) && + docker build -t kalibr -f $GITHUB_WORKSPACE/src/$REPO/Dockerfile_ros1_16_04 . + diff --git a/.github/workflows/docker_1804_build.yaml b/.github/workflows/docker_1804_build.yaml new file mode 100644 index 000000000..0e1270aa7 --- /dev/null +++ b/.github/workflows/docker_1804_build.yaml @@ -0,0 +1,23 @@ +name: "ROS1 Ubuntu 18.04" + +on: + push: + branches: [ master, dockerfile ] + pull_request: + +jobs: + build: + runs-on: ubuntu-latest + steps: + - name: Code Checkout + uses: actions/checkout@v2 + - name: Create Workspace + run: | + export REPO=$(basename $GITHUB_REPOSITORY) && + cd $GITHUB_WORKSPACE/.. && mkdir src/ && + mv $REPO/ src/ && mkdir $REPO/ && mv src/ $REPO/ && cd $REPO/ + - name: Build Docker + run: | + export REPO=$(basename $GITHUB_REPOSITORY) && + docker build -t kalibr -f $GITHUB_WORKSPACE/src/$REPO/Dockerfile_ros1_18_04 . + diff --git a/.github/workflows/docker_2004_build.yaml b/.github/workflows/docker_2004_build.yaml new file mode 100644 index 000000000..f25ef547d --- /dev/null +++ b/.github/workflows/docker_2004_build.yaml @@ -0,0 +1,23 @@ +name: "ROS1 Ubuntu 20.04" + +on: + push: + branches: [ master, dockerfile ] + pull_request: + +jobs: + build: + runs-on: ubuntu-latest + steps: + - name: Code Checkout + uses: actions/checkout@v2 + - name: Create Workspace + run: | + export REPO=$(basename $GITHUB_REPOSITORY) && + cd $GITHUB_WORKSPACE/.. && mkdir src/ && + mv $REPO/ src/ && mkdir $REPO/ && mv src/ $REPO/ && cd $REPO/ + - name: Build Docker + run: | + export REPO=$(basename $GITHUB_REPOSITORY) && + docker build -t kalibr -f $GITHUB_WORKSPACE/src/$REPO/Dockerfile_ros1_20_04 . + diff --git a/.github/workflows/docker_build.yaml b/.github/workflows/docker_build.yaml deleted file mode 100644 index b606614fc..000000000 --- a/.github/workflows/docker_build.yaml +++ /dev/null @@ -1,52 +0,0 @@ -name: Docker - -# This determines when this workflow is run -on: [push, pull_request] # on all pushes and PRs - -jobs: - build_1604: - name: "ROS1 Ubuntu 16.04" - runs-on: ubuntu-latest - steps: - - name: Code Checkout - uses: actions/checkout@v2 - - name: Create Workspace - run: | - export REPO=$(basename $GITHUB_REPOSITORY) && - cd $GITHUB_WORKSPACE/.. && mkdir src/ && - mv $REPO/ src/ && mkdir $REPO/ && mv src/ $REPO/ && cd $REPO/ - - name: Build Docker - run: | - export REPO=$(basename $GITHUB_REPOSITORY) && - docker build -t kalibr -f $GITHUB_WORKSPACE/src/$REPO/Dockerfile_ros1_16_04 . - build_1804: - name: "ROS1 Ubuntu 18.04" - runs-on: ubuntu-latest - steps: - - name: Code Checkout - uses: actions/checkout@v2 - - name: Create Workspace - run: | - export REPO=$(basename $GITHUB_REPOSITORY) && - cd $GITHUB_WORKSPACE/.. && mkdir src/ && - mv $REPO/ src/ && mkdir $REPO/ && mv src/ $REPO/ && cd $REPO/ - - name: Build Docker - run: | - export REPO=$(basename $GITHUB_REPOSITORY) && - docker build -t kalibr -f $GITHUB_WORKSPACE/src/$REPO/Dockerfile_ros1_18_04 . - build_2004: - name: "ROS1 Ubuntu 20.04" - runs-on: ubuntu-latest - steps: - - name: Code Checkout - uses: actions/checkout@v2 - - name: Create Workspace - run: | - export REPO=$(basename $GITHUB_REPOSITORY) && - cd $GITHUB_WORKSPACE/.. && mkdir src/ && - mv $REPO/ src/ && mkdir $REPO/ && mv src/ $REPO/ && cd $REPO/ - - name: Build Docker - run: | - export REPO=$(basename $GITHUB_REPOSITORY) && - docker build -t kalibr -f $GITHUB_WORKSPACE/src/$REPO/Dockerfile_ros1_20_04 . - From b93319471a550af14ec3640a7304fe9c6265689a Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Tue, 3 May 2022 17:03:20 -0400 Subject: [PATCH 75/75] Added missing if blocks in sparse_block_matrix/sparse_helper.h --- .../include/sparse_block_matrix/sparse_helper.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/aslam_optimizer/sparse_block_matrix/include/sparse_block_matrix/sparse_helper.h b/aslam_optimizer/sparse_block_matrix/include/sparse_block_matrix/sparse_helper.h index 067092bb8..0a6a87c54 100755 --- a/aslam_optimizer/sparse_block_matrix/include/sparse_block_matrix/sparse_helper.h +++ b/aslam_optimizer/sparse_block_matrix/include/sparse_block_matrix/sparse_helper.h @@ -107,9 +107,9 @@ namespace { ~CholmodExt() { - if(p) delete[] (INTTYPE*)p; p = 0; - if(x) delete[] (double*)x; x = 0; - if(i) delete[] (INTTYPE*)i; i = 0; + if(p) { delete[] (INTTYPE*)p; p = 0; } + if(x) { delete[] (double*)x; x = 0; } + if(i) { delete[] (INTTYPE*)i; i = 0; } } size_t columnsAllocated; };