diff --git a/.github/workflows/docker-image.yml b/.github/workflows/docker-image.yml new file mode 100644 index 00000000..1d1313cd --- /dev/null +++ b/.github/workflows/docker-image.yml @@ -0,0 +1,90 @@ +name: Docker Image CI + +on: + push: + branches: [ "main" ] + pull_request: + branches: [ "main" ] + +jobs: + + build-AMD64: + + runs-on: [self-hosted, linux, x64] + + steps: + - + name: Get current date + id: date + run: echo "::set-output name=today::$(date +'%Y-%m-%d_%H-%M')" + - + name: Checkout + uses: actions/checkout@v3 + - + name: Login to Docker Hub + uses: docker/login-action@v3 + with: + username: ${{ secrets.DOCKER_HUB_ACCOUNT }} + password: ${{ secrets.DOCKER_HUB_TOKEN }} + - + name: Set up Docker Buildx + uses: docker/setup-buildx-action@v2 + - + name: Build and push + uses: docker/build-push-action@v3 + with: + context: . + file: docker/Dockerfile.x86 + push: true + tags: | + hkustswarm/d2slam:${{ steps.date.outputs.today }} + hkustswarm/d2slam:pc + hkustswarm/d2slam:latest + cache-from: type=local,src=/tmp/.buildx-cache + cache-to: type=local,dest=/tmp/.buildx-cache + + build-ARM64: + + runs-on: [self-hosted, linux, arm64] + + steps: + - + name: Get current date + id: date + run: echo "::set-output name=today::$(date +'%Y-%m-%d_%H-%M')" + - + name: Checkout + uses: actions/checkout@v3 + - + name: Login to Docker Hub + uses: docker/login-action@v3 + with: + username: ${{ secrets.DOCKER_HUB_ACCOUNT }} + password: ${{ secrets.DOCKER_HUB_TOKEN }} + - + name: Set up Docker Buildx + uses: docker/setup-buildx-action@v2 + - + name: Build-base and push + uses: docker/build-push-action@v3 + with: + context: . + file: docker/Dockerfile.jetson_orin_base_35.3.1 + push: true + tags: | + hkustswarm/d2slam:jetson_orin_base_35.3.1-${{ steps.date.outputs.today }} + hkustswarm/d2slam:jetson_orin_base_35.3.1 + cache-from: type=local,src=/tmp/.buildx-cache + cache-to: type=local,dest=/tmp/.buildx-cache + - + name: Build-app and push + uses: docker/build-push-action@v3 + with: + context: . + file: docker/Dockerfile.jetson + push: true + tags: | + hkustswarm/d2slam:jetson_orin-${{ steps.date.outputs.today }} + hkustswarm/d2slam:jetson_orin + cache-from: type=local,src=/tmp/.buildx-cache + cache-to: type=local,dest=/tmp/.buildx-cache diff --git a/.gitignore b/.gitignore index 9fc1830b..5e0ea4ec 100644 --- a/.gitignore +++ b/.gitignore @@ -14,15 +14,15 @@ d2pgo/scripts/pose_graph_partitioning/__pycache__/ d2pgo/scripts/notebooks/*.png quadcam_depth_est/build/ quadcam_tools/__pycache__/ -models/*.onnx -models/*.pb +# models/*.onnx +# models/*.pb models/*.trt -models/*.engine -models/*.profile -model.zip +# models/*.engine +# models/*.profile +# model.zip *.engine *.profile -models/ +# models/ data_analysis/.ipynb_checkpoints/ d2pgo/posegraph-graphviz.* d2pgo/scripts/notebooks/posegraph-graphviz.* diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 00000000..521e1c37 --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,8 @@ +repos: + - repo: https://github.com/cpp-linter/cpp-linter-hooks + rev: v0.4.0 # Use the ref you want to point at + hooks: + - id: clang-format + args: [--style=Google] # Other coding style: LLVM, GNU, Chromium, Microsoft, Mozilla, WebKit. + # - id: clang-tidy + # args: [--checks='all', -p, build] \ No newline at end of file diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index 6883f47e..f641a54e 100644 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -7,15 +7,16 @@ }, "includePath": [ "/opt/ros/noetic/include/**", - "/home/xuhao/d2slam_ws/devel/include/**", - "/home/xuhao/d2slam_ws/src/D2SLAM/camera_models/include/**", - "/home/xuhao/d2slam_ws/src/vision_opencv/cv_bridge/include/**", - "/home/xuhao/d2slam_ws/src/D2SLAM/d2common/include/**", - "/home/xuhao/d2slam_ws/src/D2SLAM/d2frontend/include/**", - "/home/xuhao/d2slam_ws/src/vision_opencv/image_geometry/include/**", - "/home/xuhao/d2slam_ws/src/swarm_msgs/swarm_msgs/include/**", - "/home/xuhao/d2slam_ws/src/swarm_msgs/swarmcomm_msgs/include/**", - "/usr/include/**" + "/usr/include/**", + "/usr/local/include", + "/root/swarm_ws/src/D2SLAM/**", + "/root/swarm_ws/src/D2SLAM/camera_models/include/**", + "/root/swarm_ws/src/D2SLAM/vision_opencv/cv_bridge/include/**", + "/root/swarm_ws/src/D2SLAM/d2common/include/**", + "/root/swarm_ws/src/D2SLAM/d2frontend/include/**", + "/root/swarm_ws/src/vision_opencv/image_geometry/include/**", + "/root/swarm_ws/src/swarm_msgs/swarm_msgs/include/**", + "/root/swarm_ws/src/swarm_msgs/swarmcomm_msgs/include/**" ], "name": "ROS", "intelliSenseMode": "gcc-x64", diff --git a/README.md b/README.md index 51a4e63c..219503c5 100644 --- a/README.md +++ b/README.md @@ -86,7 +86,6 @@ estimation_mode: 2 # The estimation mode. # Available options are 0 (D2VINS works as mono-robot VIO), 1 (each D2VINS instance estimates all robots in the swarm with all information it found), # 2 (distributed estimation mode, should be used in real-world experiments), and SERVER_MODE (D2VINS works as a server to collect information from the network and estimate the states, but not read data locally). double_counting_common_feature: 0 # 1 or 0. If 1, common features will be double counted. This parameter is for debugging only. -min_inv_dep: 0.01 # The minimum inverse depth of the landmark. The default value is 0.01, which is suitable for landmarks 100 meters away. #optimization parameters max_solver_time: 0.08 # The maximum time allowed for each iteration of the solver (in ms). diff --git a/camera_models/src/calib/CameraCalibration.cc b/camera_models/src/calib/CameraCalibration.cc index 10543e9c..998c1daf 100644 --- a/camera_models/src/calib/CameraCalibration.cc +++ b/camera_models/src/calib/CameraCalibration.cc @@ -1,571 +1,482 @@ #include "camodocal/calib/CameraCalibration.h" +#include #include #include +#include #include #include -#include -#include +#include #include #include #include -#include #include "camodocal/camera_models/CameraFactory.h" -#include "camodocal/sparse_graph/Transform.h" +#include "camodocal/camera_models/CostFunctionFactory.h" #include "camodocal/gpl/EigenQuaternionParameterization.h" #include "camodocal/gpl/EigenUtils.h" -#include "camodocal/camera_models/CostFunctionFactory.h" - +#include "camodocal/sparse_graph/Transform.h" #include "ceres/ceres.h" -namespace camodocal -{ +namespace camodocal { CameraCalibration::CameraCalibration() - : m_boardSize(cv::Size(0,0)) - , m_squareSize(0.0f) - , m_verbose(false) -{ - -} + : m_boardSize(cv::Size(0, 0)), m_squareSize(0.0f), m_verbose(false) {} CameraCalibration::CameraCalibration(const Camera::ModelType modelType, const std::string& cameraName, const cv::Size& imageSize, const cv::Size& boardSize, float squareSize) - : m_boardSize(boardSize) - , m_squareSize(squareSize) - , m_verbose(false) -{ - m_camera = CameraFactory::instance()->generateCamera(modelType, cameraName, imageSize); + : m_boardSize(boardSize), m_squareSize(squareSize), m_verbose(false) { + m_camera = CameraFactory::instance()->generateCamera(modelType, cameraName, + imageSize); } -void -CameraCalibration::clear(void) -{ - m_imagePoints.clear(); - m_scenePoints.clear(); +void CameraCalibration::clear(void) { + m_imagePoints.clear(); + m_scenePoints.clear(); } -void -CameraCalibration::addChessboardData(const std::vector& corners) -{ - m_imagePoints.push_back(corners); - - std::vector scenePointsInView; - for (int i = 0; i < m_boardSize.height; ++i) - { - for (int j = 0; j < m_boardSize.width; ++j) - { - scenePointsInView.push_back(cv::Point3f(i * m_squareSize, j * m_squareSize, 0.0)); - } +void CameraCalibration::addChessboardData( + const std::vector& corners) { + m_imagePoints.push_back(corners); + + std::vector scenePointsInView; + for (int i = 0; i < m_boardSize.height; ++i) { + for (int j = 0; j < m_boardSize.width; ++j) { + scenePointsInView.push_back( + cv::Point3f(i * m_squareSize, j * m_squareSize, 0.0)); } - m_scenePoints.push_back(scenePointsInView); + } + m_scenePoints.push_back(scenePointsInView); } -bool -CameraCalibration::calibrate(void) -{ - int imageCount = m_imagePoints.size(); - - // compute intrinsic camera parameters and extrinsic parameters for each of the views - std::vector rvecs; - std::vector tvecs; - bool ret = calibrateHelper(m_camera, rvecs, tvecs); - - m_cameraPoses = cv::Mat(imageCount, 6, CV_64F); - for (int i = 0; i < imageCount; ++i) - { - m_cameraPoses.at(i,0) = rvecs.at(i).at(0); - m_cameraPoses.at(i,1) = rvecs.at(i).at(1); - m_cameraPoses.at(i,2) = rvecs.at(i).at(2); - m_cameraPoses.at(i,3) = tvecs.at(i).at(0); - m_cameraPoses.at(i,4) = tvecs.at(i).at(1); - m_cameraPoses.at(i,5) = tvecs.at(i).at(2); +bool CameraCalibration::calibrate(void) { + int imageCount = m_imagePoints.size(); + + // compute intrinsic camera parameters and extrinsic parameters for each of + // the views + std::vector rvecs; + std::vector tvecs; + bool ret = calibrateHelper(m_camera, rvecs, tvecs); + + m_cameraPoses = cv::Mat(imageCount, 6, CV_64F); + for (int i = 0; i < imageCount; ++i) { + m_cameraPoses.at(i, 0) = rvecs.at(i).at(0); + m_cameraPoses.at(i, 1) = rvecs.at(i).at(1); + m_cameraPoses.at(i, 2) = rvecs.at(i).at(2); + m_cameraPoses.at(i, 3) = tvecs.at(i).at(0); + m_cameraPoses.at(i, 4) = tvecs.at(i).at(1); + m_cameraPoses.at(i, 5) = tvecs.at(i).at(2); + } + + // Compute measurement covariance. + std::vector > errVec(m_imagePoints.size()); + Eigen::Vector2d errSum = Eigen::Vector2d::Zero(); + size_t errCount = 0; + for (size_t i = 0; i < m_imagePoints.size(); ++i) { + std::vector estImagePoints; + m_camera->projectPoints(m_scenePoints.at(i), rvecs.at(i), tvecs.at(i), + estImagePoints); + + for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j) { + cv::Point2f pObs = m_imagePoints.at(i).at(j); + cv::Point2f pEst = estImagePoints.at(j); + + cv::Point2f err = pObs - pEst; + + errVec.at(i).push_back(err); + + errSum += Eigen::Vector2d(err.x, err.y); } - // Compute measurement covariance. - std::vector > errVec(m_imagePoints.size()); - Eigen::Vector2d errSum = Eigen::Vector2d::Zero(); - size_t errCount = 0; - for (size_t i = 0; i < m_imagePoints.size(); ++i) - { - std::vector estImagePoints; - m_camera->projectPoints(m_scenePoints.at(i), rvecs.at(i), tvecs.at(i), - estImagePoints); - - for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j) - { - cv::Point2f pObs = m_imagePoints.at(i).at(j); - cv::Point2f pEst = estImagePoints.at(j); - - cv::Point2f err = pObs - pEst; + errCount += m_imagePoints.at(i).size(); + } - errVec.at(i).push_back(err); + Eigen::Vector2d errMean = errSum / static_cast(errCount); - errSum += Eigen::Vector2d(err.x, err.y); - } + Eigen::Matrix2d measurementCovariance = Eigen::Matrix2d::Zero(); + for (size_t i = 0; i < errVec.size(); ++i) { + for (size_t j = 0; j < errVec.at(i).size(); ++j) { + cv::Point2f err = errVec.at(i).at(j); + double d0 = err.x - errMean(0); + double d1 = err.y - errMean(1); - errCount += m_imagePoints.at(i).size(); + measurementCovariance(0, 0) += d0 * d0; + measurementCovariance(0, 1) += d0 * d1; + measurementCovariance(1, 1) += d1 * d1; } + } + measurementCovariance /= static_cast(errCount); + measurementCovariance(1, 0) = measurementCovariance(0, 1); - Eigen::Vector2d errMean = errSum / static_cast(errCount); - - Eigen::Matrix2d measurementCovariance = Eigen::Matrix2d::Zero(); - for (size_t i = 0; i < errVec.size(); ++i) - { - for (size_t j = 0; j < errVec.at(i).size(); ++j) - { - cv::Point2f err = errVec.at(i).at(j); - double d0 = err.x - errMean(0); - double d1 = err.y - errMean(1); - - measurementCovariance(0,0) += d0 * d0; - measurementCovariance(0,1) += d0 * d1; - measurementCovariance(1,1) += d1 * d1; - } - } - measurementCovariance /= static_cast(errCount); - measurementCovariance(1,0) = measurementCovariance(0,1); - - m_measurementCovariance = measurementCovariance; + m_measurementCovariance = measurementCovariance; - return ret; + return ret; } -int -CameraCalibration::sampleCount(void) const -{ - return m_imagePoints.size(); -} +int CameraCalibration::sampleCount(void) const { return m_imagePoints.size(); } -std::vector >& -CameraCalibration::imagePoints(void) -{ - return m_imagePoints; +std::vector >& CameraCalibration::imagePoints(void) { + return m_imagePoints; } -const std::vector >& -CameraCalibration::imagePoints(void) const -{ - return m_imagePoints; +const std::vector >& CameraCalibration::imagePoints( + void) const { + return m_imagePoints; } -std::vector >& -CameraCalibration::scenePoints(void) -{ - return m_scenePoints; +std::vector >& CameraCalibration::scenePoints(void) { + return m_scenePoints; } -const std::vector >& -CameraCalibration::scenePoints(void) const -{ - return m_scenePoints; +const std::vector >& CameraCalibration::scenePoints( + void) const { + return m_scenePoints; } -CameraPtr& -CameraCalibration::camera(void) -{ - return m_camera; -} +CameraPtr& CameraCalibration::camera(void) { return m_camera; } -const CameraConstPtr -CameraCalibration::camera(void) const -{ - return m_camera; -} +const CameraConstPtr CameraCalibration::camera(void) const { return m_camera; } -Eigen::Matrix2d& -CameraCalibration::measurementCovariance(void) -{ - return m_measurementCovariance; +Eigen::Matrix2d& CameraCalibration::measurementCovariance(void) { + return m_measurementCovariance; } -const Eigen::Matrix2d& -CameraCalibration::measurementCovariance(void) const -{ - return m_measurementCovariance; +const Eigen::Matrix2d& CameraCalibration::measurementCovariance(void) const { + return m_measurementCovariance; } -cv::Mat& -CameraCalibration::cameraPoses(void) -{ - return m_cameraPoses; -} +cv::Mat& CameraCalibration::cameraPoses(void) { return m_cameraPoses; } -const cv::Mat& -CameraCalibration::cameraPoses(void) const -{ - return m_cameraPoses; +const cv::Mat& CameraCalibration::cameraPoses(void) const { + return m_cameraPoses; } -void -CameraCalibration::drawResults(std::vector& images) const -{ - std::vector rvecs, tvecs; - - for (size_t i = 0; i < images.size(); ++i) - { - cv::Mat rvec(3, 1, CV_64F); - rvec.at(0) = m_cameraPoses.at(i,0); - rvec.at(1) = m_cameraPoses.at(i,1); - rvec.at(2) = m_cameraPoses.at(i,2); - - cv::Mat tvec(3, 1, CV_64F); - tvec.at(0) = m_cameraPoses.at(i,3); - tvec.at(1) = m_cameraPoses.at(i,4); - tvec.at(2) = m_cameraPoses.at(i,5); - - rvecs.push_back(rvec); - tvecs.push_back(tvec); - } +void CameraCalibration::drawResults(std::vector& images) const { + std::vector rvecs, tvecs; - int drawShiftBits = 4; - int drawMultiplier = 1 << drawShiftBits; - - cv::Scalar green(0, 255, 0); - cv::Scalar red(0, 0, 255); - - for (size_t i = 0; i < images.size(); ++i) - { - cv::Mat& image = images.at(i); - if (image.channels() == 1) - { - cv::cvtColor(image, image, CV_GRAY2RGB); - } - - std::vector estImagePoints; - m_camera->projectPoints(m_scenePoints.at(i), rvecs.at(i), tvecs.at(i), - estImagePoints); - - float errorSum = 0.0f; - float errorMax = std::numeric_limits::min(); - - for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j) - { - cv::Point2f pObs = m_imagePoints.at(i).at(j); - cv::Point2f pEst = estImagePoints.at(j); - - cv::circle(image, - cv::Point(cvRound(pObs.x * drawMultiplier), - cvRound(pObs.y * drawMultiplier)), - 5, green, 2, CV_AA, drawShiftBits); - - cv::circle(image, - cv::Point(cvRound(pEst.x * drawMultiplier), - cvRound(pEst.y * drawMultiplier)), - 5, red, 2, CV_AA, drawShiftBits); - - float error = cv::norm(pObs - pEst); - - errorSum += error; - if (error > errorMax) - { - errorMax = error; - } - } - - std::ostringstream oss; - oss << "Reprojection error: avg = " << errorSum / m_imagePoints.at(i).size() - << " max = " << errorMax; - - cv::putText(image, oss.str(), cv::Point(10, image.rows - 10), - cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(255, 255, 255), - 1, CV_AA); - } -} + for (size_t i = 0; i < images.size(); ++i) { + cv::Mat rvec(3, 1, CV_64F); + rvec.at(0) = m_cameraPoses.at(i, 0); + rvec.at(1) = m_cameraPoses.at(i, 1); + rvec.at(2) = m_cameraPoses.at(i, 2); -void -CameraCalibration::writeParams(const std::string& filename) const -{ - m_camera->writeParametersToYamlFile(filename); -} + cv::Mat tvec(3, 1, CV_64F); + tvec.at(0) = m_cameraPoses.at(i, 3); + tvec.at(1) = m_cameraPoses.at(i, 4); + tvec.at(2) = m_cameraPoses.at(i, 5); -bool -CameraCalibration::writeChessboardData(const std::string& filename) const -{ - std::ofstream ofs(filename.c_str(), std::ios::out | std::ios::binary); - if (!ofs.is_open()) - { - return false; - } + rvecs.push_back(rvec); + tvecs.push_back(tvec); + } - writeData(ofs, m_boardSize.width); - writeData(ofs, m_boardSize.height); - writeData(ofs, m_squareSize); - - writeData(ofs, m_measurementCovariance(0,0)); - writeData(ofs, m_measurementCovariance(0,1)); - writeData(ofs, m_measurementCovariance(1,0)); - writeData(ofs, m_measurementCovariance(1,1)); - - writeData(ofs, m_cameraPoses.rows); - writeData(ofs, m_cameraPoses.cols); - writeData(ofs, m_cameraPoses.type()); - for (int i = 0; i < m_cameraPoses.rows; ++i) - { - for (int j = 0; j < m_cameraPoses.cols; ++j) - { - writeData(ofs, m_cameraPoses.at(i,j)); - } - } + int drawShiftBits = 4; + int drawMultiplier = 1 << drawShiftBits; - writeData(ofs, m_imagePoints.size()); - for (size_t i = 0; i < m_imagePoints.size(); ++i) - { - writeData(ofs, m_imagePoints.at(i).size()); - for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j) - { - const cv::Point2f& ipt = m_imagePoints.at(i).at(j); - - writeData(ofs, ipt.x); - writeData(ofs, ipt.y); - } - } + cv::Scalar green(0, 255, 0); + cv::Scalar red(0, 0, 255); - writeData(ofs, m_scenePoints.size()); - for (size_t i = 0; i < m_scenePoints.size(); ++i) - { - writeData(ofs, m_scenePoints.at(i).size()); - for (size_t j = 0; j < m_scenePoints.at(i).size(); ++j) - { - const cv::Point3f& spt = m_scenePoints.at(i).at(j); - - writeData(ofs, spt.x); - writeData(ofs, spt.y); - writeData(ofs, spt.z); - } + for (size_t i = 0; i < images.size(); ++i) { + cv::Mat& image = images.at(i); + if (image.channels() == 1) { + cv::cvtColor(image, image, CV_GRAY2RGB); } - return true; -} + std::vector estImagePoints; + m_camera->projectPoints(m_scenePoints.at(i), rvecs.at(i), tvecs.at(i), + estImagePoints); -bool -CameraCalibration::readChessboardData(const std::string& filename) -{ - std::ifstream ifs(filename.c_str(), std::ios::in | std::ios::binary); - if (!ifs.is_open()) - { - return false; - } + float errorSum = 0.0f; + float errorMax = std::numeric_limits::min(); - readData(ifs, m_boardSize.width); - readData(ifs, m_boardSize.height); - readData(ifs, m_squareSize); - - readData(ifs, m_measurementCovariance(0,0)); - readData(ifs, m_measurementCovariance(0,1)); - readData(ifs, m_measurementCovariance(1,0)); - readData(ifs, m_measurementCovariance(1,1)); - - int rows, cols, type; - readData(ifs, rows); - readData(ifs, cols); - readData(ifs, type); - m_cameraPoses = cv::Mat(rows, cols, type); - - for (int i = 0; i < m_cameraPoses.rows; ++i) - { - for (int j = 0; j < m_cameraPoses.cols; ++j) - { - readData(ifs, m_cameraPoses.at(i,j)); - } - } + for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j) { + cv::Point2f pObs = m_imagePoints.at(i).at(j); + cv::Point2f pEst = estImagePoints.at(j); - size_t nImagePointSets; - readData(ifs, nImagePointSets); - - m_imagePoints.clear(); - m_imagePoints.resize(nImagePointSets); - for (size_t i = 0; i < m_imagePoints.size(); ++i) - { - size_t nImagePoints; - readData(ifs, nImagePoints); - m_imagePoints.at(i).resize(nImagePoints); - - for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j) - { - cv::Point2f& ipt = m_imagePoints.at(i).at(j); - readData(ifs, ipt.x); - readData(ifs, ipt.y); - } - } + cv::circle(image, + cv::Point(cvRound(pObs.x * drawMultiplier), + cvRound(pObs.y * drawMultiplier)), + 5, green, 2, CV_AA, drawShiftBits); + + cv::circle(image, + cv::Point(cvRound(pEst.x * drawMultiplier), + cvRound(pEst.y * drawMultiplier)), + 5, red, 2, CV_AA, drawShiftBits); - size_t nScenePointSets; - readData(ifs, nScenePointSets); - - m_scenePoints.clear(); - m_scenePoints.resize(nScenePointSets); - for (size_t i = 0; i < m_scenePoints.size(); ++i) - { - size_t nScenePoints; - readData(ifs, nScenePoints); - m_scenePoints.at(i).resize(nScenePoints); - - for (size_t j = 0; j < m_scenePoints.at(i).size(); ++j) - { - cv::Point3f& spt = m_scenePoints.at(i).at(j); - readData(ifs, spt.x); - readData(ifs, spt.y); - readData(ifs, spt.z); - } + float error = cv::norm(pObs - pEst); + + errorSum += error; + if (error > errorMax) { + errorMax = error; + } } - return true; + std::ostringstream oss; + oss << "Reprojection error: avg = " << errorSum / m_imagePoints.at(i).size() + << " max = " << errorMax; + + cv::putText(image, oss.str(), cv::Point(10, image.rows - 10), + cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(255, 255, 255), 1, + CV_AA); + } } -void -CameraCalibration::setVerbose(bool verbose) -{ - m_verbose = verbose; +void CameraCalibration::writeParams(const std::string& filename) const { + m_camera->writeParametersToYamlFile(filename); } -bool -CameraCalibration::calibrateHelper(CameraPtr& camera, - std::vector& rvecs, std::vector& tvecs) const -{ - rvecs.assign(m_scenePoints.size(), cv::Mat()); - tvecs.assign(m_scenePoints.size(), cv::Mat()); +bool CameraCalibration::writeChessboardData(const std::string& filename) const { + std::ofstream ofs(filename.c_str(), std::ios::out | std::ios::binary); + if (!ofs.is_open()) { + return false; + } + + writeData(ofs, m_boardSize.width); + writeData(ofs, m_boardSize.height); + writeData(ofs, m_squareSize); + + writeData(ofs, m_measurementCovariance(0, 0)); + writeData(ofs, m_measurementCovariance(0, 1)); + writeData(ofs, m_measurementCovariance(1, 0)); + writeData(ofs, m_measurementCovariance(1, 1)); + + writeData(ofs, m_cameraPoses.rows); + writeData(ofs, m_cameraPoses.cols); + writeData(ofs, m_cameraPoses.type()); + for (int i = 0; i < m_cameraPoses.rows; ++i) { + for (int j = 0; j < m_cameraPoses.cols; ++j) { + writeData(ofs, m_cameraPoses.at(i, j)); + } + } - // STEP 1: Estimate intrinsics - camera->estimateIntrinsics(m_boardSize, m_scenePoints, m_imagePoints); + writeData(ofs, m_imagePoints.size()); + for (size_t i = 0; i < m_imagePoints.size(); ++i) { + writeData(ofs, m_imagePoints.at(i).size()); + for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j) { + const cv::Point2f& ipt = m_imagePoints.at(i).at(j); - // STEP 2: Estimate extrinsics - for (size_t i = 0; i < m_scenePoints.size(); ++i) - { - camera->estimateExtrinsics(m_scenePoints.at(i), m_imagePoints.at(i), rvecs.at(i), tvecs.at(i)); + writeData(ofs, ipt.x); + writeData(ofs, ipt.y); } + } - if (m_verbose) - { - std::cout << "[" << camera->cameraName() << "] " - << "# INFO: " << "Initial reprojection error: " - << std::fixed << std::setprecision(3) - << camera->reprojectionError(m_scenePoints, m_imagePoints, rvecs, tvecs) - << " pixels" << std::endl; + writeData(ofs, m_scenePoints.size()); + for (size_t i = 0; i < m_scenePoints.size(); ++i) { + writeData(ofs, m_scenePoints.at(i).size()); + for (size_t j = 0; j < m_scenePoints.at(i).size(); ++j) { + const cv::Point3f& spt = m_scenePoints.at(i).at(j); + + writeData(ofs, spt.x); + writeData(ofs, spt.y); + writeData(ofs, spt.z); } + } - // STEP 3: optimization using ceres - optimize(camera, rvecs, tvecs); + return true; +} - if (m_verbose) - { - double err = camera->reprojectionError(m_scenePoints, m_imagePoints, rvecs, tvecs); - std::cout << "[" << camera->cameraName() << "] " << "# INFO: Final reprojection error: " - << err << " pixels" << std::endl; - std::cout << "[" << camera->cameraName() << "] " << "# INFO: " - << camera->parametersToString() << std::endl; +bool CameraCalibration::readChessboardData(const std::string& filename) { + std::ifstream ifs(filename.c_str(), std::ios::in | std::ios::binary); + if (!ifs.is_open()) { + return false; + } + + readData(ifs, m_boardSize.width); + readData(ifs, m_boardSize.height); + readData(ifs, m_squareSize); + + readData(ifs, m_measurementCovariance(0, 0)); + readData(ifs, m_measurementCovariance(0, 1)); + readData(ifs, m_measurementCovariance(1, 0)); + readData(ifs, m_measurementCovariance(1, 1)); + + int rows, cols, type; + readData(ifs, rows); + readData(ifs, cols); + readData(ifs, type); + m_cameraPoses = cv::Mat(rows, cols, type); + + for (int i = 0; i < m_cameraPoses.rows; ++i) { + for (int j = 0; j < m_cameraPoses.cols; ++j) { + readData(ifs, m_cameraPoses.at(i, j)); + } + } + + size_t nImagePointSets; + readData(ifs, nImagePointSets); + + m_imagePoints.clear(); + m_imagePoints.resize(nImagePointSets); + for (size_t i = 0; i < m_imagePoints.size(); ++i) { + size_t nImagePoints; + readData(ifs, nImagePoints); + m_imagePoints.at(i).resize(nImagePoints); + + for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j) { + cv::Point2f& ipt = m_imagePoints.at(i).at(j); + readData(ifs, ipt.x); + readData(ifs, ipt.y); } + } + + size_t nScenePointSets; + readData(ifs, nScenePointSets); + + m_scenePoints.clear(); + m_scenePoints.resize(nScenePointSets); + for (size_t i = 0; i < m_scenePoints.size(); ++i) { + size_t nScenePoints; + readData(ifs, nScenePoints); + m_scenePoints.at(i).resize(nScenePoints); + + for (size_t j = 0; j < m_scenePoints.at(i).size(); ++j) { + cv::Point3f& spt = m_scenePoints.at(i).at(j); + readData(ifs, spt.x); + readData(ifs, spt.y); + readData(ifs, spt.z); + } + } - return true; + return true; } -void -CameraCalibration::optimize(CameraPtr& camera, - std::vector& rvecs, std::vector& tvecs) const -{ - // Use ceres to do optimization - ceres::Problem problem; - - std::vector > transformVec(rvecs.size()); - for (size_t i = 0; i < rvecs.size(); ++i) - { - Eigen::Vector3d rvec; - cv::cv2eigen(rvecs.at(i), rvec); - - transformVec.at(i).rotation() = Eigen::AngleAxisd(rvec.norm(), rvec.normalized()); - transformVec.at(i).translation() << tvecs[i].at(0), - tvecs[i].at(1), - tvecs[i].at(2); - } +void CameraCalibration::setVerbose(bool verbose) { m_verbose = verbose; } + +bool CameraCalibration::calibrateHelper(CameraPtr& camera, + std::vector& rvecs, + std::vector& tvecs) const { + rvecs.assign(m_scenePoints.size(), cv::Mat()); + tvecs.assign(m_scenePoints.size(), cv::Mat()); + + // STEP 1: Estimate intrinsics + camera->estimateIntrinsics(m_boardSize, m_scenePoints, m_imagePoints); + + // STEP 2: Estimate extrinsics + for (size_t i = 0; i < m_scenePoints.size(); ++i) { + camera->estimateExtrinsics(m_scenePoints.at(i), m_imagePoints.at(i), + rvecs.at(i), tvecs.at(i)); + } + + if (m_verbose) { + std::cout << "[" << camera->cameraName() << "] " + << "# INFO: " + << "Initial reprojection error: " << std::fixed + << std::setprecision(3) + << camera->reprojectionError(m_scenePoints, m_imagePoints, rvecs, + tvecs) + << " pixels" << std::endl; + } + + // STEP 3: optimization using ceres + optimize(camera, rvecs, tvecs); + + if (m_verbose) { + double err = + camera->reprojectionError(m_scenePoints, m_imagePoints, rvecs, tvecs); + std::cout << "[" << camera->cameraName() << "] " + << "# INFO: Final reprojection error: " << err << " pixels" + << std::endl; + std::cout << "[" << camera->cameraName() << "] " + << "# INFO: " << camera->parametersToString() << std::endl; + } + + return true; +} - std::vector intrinsicCameraParams; - m_camera->writeParameters(intrinsicCameraParams); - - // create residuals for each observation - for (size_t i = 0; i < m_imagePoints.size(); ++i) - { - for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j) - { - const cv::Point3f& spt = m_scenePoints.at(i).at(j); - const cv::Point2f& ipt = m_imagePoints.at(i).at(j); - - ceres::CostFunction* costFunction = - CostFunctionFactory::instance()->generateCostFunction(camera, - Eigen::Vector3d(spt.x, spt.y, spt.z), - Eigen::Vector2d(ipt.x, ipt.y), - CAMERA_INTRINSICS | CAMERA_POSE); - - ceres::LossFunction* lossFunction = new ceres::CauchyLoss(1.0); - problem.AddResidualBlock(costFunction, lossFunction, - intrinsicCameraParams.data(), - transformVec.at(i).rotationData(), - transformVec.at(i).translationData()); - } - - ceres::LocalParameterization* quaternionParameterization = - new EigenQuaternionParameterization; - - problem.SetParameterization(transformVec.at(i).rotationData(), - quaternionParameterization); +void CameraCalibration::optimize(CameraPtr& camera, std::vector& rvecs, + std::vector& tvecs) const { + // Use ceres to do optimization + ceres::Problem problem; + + std::vector > transformVec( + rvecs.size()); + for (size_t i = 0; i < rvecs.size(); ++i) { + Eigen::Vector3d rvec; + cv::cv2eigen(rvecs.at(i), rvec); + + transformVec.at(i).rotation() = + Eigen::AngleAxisd(rvec.norm(), rvec.normalized()); + transformVec.at(i).translation() << tvecs[i].at(0), + tvecs[i].at(1), tvecs[i].at(2); + } + + std::vector intrinsicCameraParams; + m_camera->writeParameters(intrinsicCameraParams); + + // create residuals for each observation + for (size_t i = 0; i < m_imagePoints.size(); ++i) { + for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j) { + const cv::Point3f& spt = m_scenePoints.at(i).at(j); + const cv::Point2f& ipt = m_imagePoints.at(i).at(j); + + ceres::CostFunction* costFunction = + CostFunctionFactory::instance()->generateCostFunction( + camera, Eigen::Vector3d(spt.x, spt.y, spt.z), + Eigen::Vector2d(ipt.x, ipt.y), CAMERA_INTRINSICS | CAMERA_POSE); + + ceres::LossFunction* lossFunction = new ceres::CauchyLoss(1.0); + problem.AddResidualBlock(costFunction, lossFunction, + intrinsicCameraParams.data(), + transformVec.at(i).rotationData(), + transformVec.at(i).translationData()); } - std::cout << "begin ceres" << std::endl; - ceres::Solver::Options options; - options.max_num_iterations = 1000; - options.num_threads = 1; + ceres::LocalParameterization* quaternionParameterization = + new EigenQuaternionParameterization; - if (m_verbose) - { - options.minimizer_progress_to_stdout = true; - } + problem.SetParameterization(transformVec.at(i).rotationData(), + quaternionParameterization); + } - ceres::Solver::Summary summary; - ceres::Solve(options, &problem, &summary); - std::cout << "end ceres" << std::endl; + std::cout << "begin ceres" << std::endl; + ceres::Solver::Options options; + options.max_num_iterations = 1000; + options.num_threads = 1; - if (m_verbose) - { - std::cout << summary.FullReport() << std::endl; - } + if (m_verbose) { + options.minimizer_progress_to_stdout = true; + } - camera->readParameters(intrinsicCameraParams); + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + std::cout << "end ceres" << std::endl; - for (size_t i = 0; i < rvecs.size(); ++i) - { - Eigen::AngleAxisd aa(transformVec.at(i).rotation()); + if (m_verbose) { + std::cout << summary.FullReport() << std::endl; + } - Eigen::Vector3d rvec = aa.angle() * aa.axis(); - cv::eigen2cv(rvec, rvecs.at(i)); + camera->readParameters(intrinsicCameraParams); - cv::Mat& tvec = tvecs.at(i); - tvec.at(0) = transformVec.at(i).translation()(0); - tvec.at(1) = transformVec.at(i).translation()(1); - tvec.at(2) = transformVec.at(i).translation()(2); - } + for (size_t i = 0; i < rvecs.size(); ++i) { + Eigen::AngleAxisd aa(transformVec.at(i).rotation()); + + Eigen::Vector3d rvec = aa.angle() * aa.axis(); + cv::eigen2cv(rvec, rvecs.at(i)); + + cv::Mat& tvec = tvecs.at(i); + tvec.at(0) = transformVec.at(i).translation()(0); + tvec.at(1) = transformVec.at(i).translation()(1); + tvec.at(2) = transformVec.at(i).translation()(2); + } } -template -void -CameraCalibration::readData(std::ifstream& ifs, T& data) const -{ - char* buffer = new char[sizeof(T)]; +template +void CameraCalibration::readData(std::ifstream& ifs, T& data) const { + char* buffer = new char[sizeof(T)]; - ifs.read(buffer, sizeof(T)); + ifs.read(buffer, sizeof(T)); - data = *(reinterpret_cast(buffer)); + data = *(reinterpret_cast(buffer)); - delete[] buffer; + delete[] buffer; } -template -void -CameraCalibration::writeData(std::ofstream& ofs, T data) const -{ - char* pData = reinterpret_cast(&data); +template +void CameraCalibration::writeData(std::ofstream& ofs, T data) const { + char* pData = reinterpret_cast(&data); - ofs.write(pData, sizeof(T)); + ofs.write(pData, sizeof(T)); } -} +} // namespace camodocal diff --git a/camera_models/src/camera_models/Camera.cc b/camera_models/src/camera_models/Camera.cc index 36acc5aa..c1c61f1b 100644 --- a/camera_models/src/camera_models/Camera.cc +++ b/camera_models/src/camera_models/Camera.cc @@ -1,332 +1,231 @@ #include "camodocal/camera_models/Camera.h" -#include "camodocal/camera_models/ScaramuzzaCamera.h" #include -namespace camodocal -{ +#include "camodocal/camera_models/ScaramuzzaCamera.h" + +namespace camodocal { Camera::Parameters::Parameters(ModelType modelType) - : m_modelType(modelType) - , m_imageWidth(0) - , m_imageHeight(0) -{ - switch (modelType) - { + : m_modelType(modelType), m_imageWidth(0), m_imageHeight(0) { + switch (modelType) { case KANNALA_BRANDT: - m_nIntrinsics = 8; - break; + m_nIntrinsics = 8; + break; case PINHOLE: - m_nIntrinsics = 8; - break; + m_nIntrinsics = 8; + break; case SCARAMUZZA: - m_nIntrinsics = SCARAMUZZA_CAMERA_NUM_PARAMS; - break; + m_nIntrinsics = SCARAMUZZA_CAMERA_NUM_PARAMS; + break; case MEI: default: - m_nIntrinsics = 9; - } + m_nIntrinsics = 9; + } } Camera::Parameters::Parameters(ModelType modelType, - const std::string& cameraName, - int w, int h) - : m_modelType(modelType) - , m_cameraName(cameraName) - , m_imageWidth(w) - , m_imageHeight(h) -{ - switch (modelType) - { + const std::string& cameraName, int w, int h) + : m_modelType(modelType), + m_cameraName(cameraName), + m_imageWidth(w), + m_imageHeight(h) { + switch (modelType) { case KANNALA_BRANDT: - m_nIntrinsics = 8; - break; + m_nIntrinsics = 8; + break; case PINHOLE: - m_nIntrinsics = 8; - break; + m_nIntrinsics = 8; + break; case SCARAMUZZA: - m_nIntrinsics = SCARAMUZZA_CAMERA_NUM_PARAMS; - break; + m_nIntrinsics = SCARAMUZZA_CAMERA_NUM_PARAMS; + break; case MEI: default: - m_nIntrinsics = 9; - } + m_nIntrinsics = 9; + } } -Camera::ModelType& -Camera::Parameters::modelType(void) -{ - return m_modelType; -} +Camera::ModelType& Camera::Parameters::modelType(void) { return m_modelType; } -std::string& -Camera::Parameters::cameraName(void) -{ - return m_cameraName; -} +std::string& Camera::Parameters::cameraName(void) { return m_cameraName; } -int& -Camera::Parameters::imageWidth(void) -{ - return m_imageWidth; -} +int& Camera::Parameters::imageWidth(void) { return m_imageWidth; } -int& -Camera::Parameters::imageHeight(void) -{ - return m_imageHeight; -} +int& Camera::Parameters::imageHeight(void) { return m_imageHeight; } -Camera::ModelType -Camera::Parameters::modelType(void) const -{ - return m_modelType; +Camera::ModelType Camera::Parameters::modelType(void) const { + return m_modelType; } -const std::string& -Camera::Parameters::cameraName(void) const -{ - return m_cameraName; +const std::string& Camera::Parameters::cameraName(void) const { + return m_cameraName; } -int -Camera::Parameters::imageWidth(void) const -{ - return m_imageWidth; -} +int Camera::Parameters::imageWidth(void) const { return m_imageWidth; } -int -Camera::Parameters::imageHeight(void) const -{ - return m_imageHeight; -} +int Camera::Parameters::imageHeight(void) const { return m_imageHeight; } -int -Camera::Parameters::nIntrinsics(void) const -{ - return m_nIntrinsics; -} +int Camera::Parameters::nIntrinsics(void) const { return m_nIntrinsics; } -cv::Mat& -Camera::mask(void) -{ - return m_mask; -} +cv::Mat& Camera::mask(void) { return m_mask; } -const cv::Mat& -Camera::mask(void) const -{ - return m_mask; -} +const cv::Mat& Camera::mask(void) const { return m_mask; } -void -Camera::estimateExtrinsics(const std::vector& objectPoints, - const std::vector& imagePoints, - cv::Mat& rvec, cv::Mat& tvec) const -{ - std::vector Ms(imagePoints.size()); - for (size_t i = 0; i < Ms.size(); ++i) - { - Eigen::Vector3d P; - liftProjective(Eigen::Vector2d(imagePoints.at(i).x, imagePoints.at(i).y), P); - - P /= P(2); - - Ms.at(i).x = P(0); - Ms.at(i).y = P(1); - } +void Camera::estimateExtrinsics(const std::vector& objectPoints, + const std::vector& imagePoints, + cv::Mat& rvec, cv::Mat& tvec) const { + std::vector Ms(imagePoints.size()); + for (size_t i = 0; i < Ms.size(); ++i) { + Eigen::Vector3d P; + liftProjective(Eigen::Vector2d(imagePoints.at(i).x, imagePoints.at(i).y), + P); + + P /= P(2); + + Ms.at(i).x = P(0); + Ms.at(i).y = P(1); + } - // assume unit focal length, zero principal point, and zero distortion - cv::solvePnP(objectPoints, Ms, cv::Mat::eye(3, 3, CV_64F), cv::noArray(), rvec, tvec); + // assume unit focal length, zero principal point, and zero distortion + cv::solvePnP(objectPoints, Ms, cv::Mat::eye(3, 3, CV_64F), cv::noArray(), + rvec, tvec); } -double -Camera::reprojectionDist(const Eigen::Vector3d& P1, const Eigen::Vector3d& P2) const -{ - Eigen::Vector2d p1, p2; +double Camera::reprojectionDist(const Eigen::Vector3d& P1, + const Eigen::Vector3d& P2) const { + Eigen::Vector2d p1, p2; - spaceToPlane(P1, p1); - spaceToPlane(P2, p2); + spaceToPlane(P1, p1); + spaceToPlane(P2, p2); - return (p1 - p2).norm(); + return (p1 - p2).norm(); } -double -Camera::reprojectionError(const std::vector< std::vector >& objectPoints, - const std::vector< std::vector >& imagePoints, - const std::vector& rvecs, - const std::vector& tvecs, - cv::OutputArray _perViewErrors) const -{ - int imageCount = objectPoints.size(); - size_t pointsSoFar = 0; - double totalErr = 0.0; - - bool computePerViewErrors = _perViewErrors.needed(); - cv::Mat perViewErrors; - if (computePerViewErrors) - { - _perViewErrors.create(imageCount, 1, CV_64F); - perViewErrors = _perViewErrors.getMat(); - } +double Camera::reprojectionError( + const std::vector >& objectPoints, + const std::vector >& imagePoints, + const std::vector& rvecs, const std::vector& tvecs, + cv::OutputArray _perViewErrors) const { + int imageCount = objectPoints.size(); + size_t pointsSoFar = 0; + double totalErr = 0.0; - for (int i = 0; i < imageCount; ++i) - { - size_t pointCount = imagePoints.at(i).size(); + bool computePerViewErrors = _perViewErrors.needed(); + cv::Mat perViewErrors; + if (computePerViewErrors) { + _perViewErrors.create(imageCount, 1, CV_64F); + perViewErrors = _perViewErrors.getMat(); + } - pointsSoFar += pointCount; + for (int i = 0; i < imageCount; ++i) { + size_t pointCount = imagePoints.at(i).size(); - std::vector estImagePoints; - projectPoints(objectPoints.at(i), rvecs.at(i), tvecs.at(i), - estImagePoints); + pointsSoFar += pointCount; - double err = 0.0; - for (size_t j = 0; j < imagePoints.at(i).size(); ++j) - { - err += cv::norm(imagePoints.at(i).at(j) - estImagePoints.at(j)); - } + std::vector estImagePoints; + projectPoints(objectPoints.at(i), rvecs.at(i), tvecs.at(i), estImagePoints); - if (computePerViewErrors) - { - perViewErrors.at(i) = err / pointCount; - } + double err = 0.0; + for (size_t j = 0; j < imagePoints.at(i).size(); ++j) { + err += cv::norm(imagePoints.at(i).at(j) - estImagePoints.at(j)); + } - totalErr += err; + if (computePerViewErrors) { + perViewErrors.at(i) = err / pointCount; } - return totalErr / pointsSoFar; + totalErr += err; + } + + return totalErr / pointsSoFar; } -double -Camera::reprojectionError(const Eigen::Vector3d& P, - const Eigen::Quaterniond& camera_q, - const Eigen::Vector3d& camera_t, - const Eigen::Vector2d& observed_p) const -{ - Eigen::Vector3d P_cam = camera_q.toRotationMatrix() * P + camera_t; +double Camera::reprojectionError(const Eigen::Vector3d& P, + const Eigen::Quaterniond& camera_q, + const Eigen::Vector3d& camera_t, + const Eigen::Vector2d& observed_p) const { + Eigen::Vector3d P_cam = camera_q.toRotationMatrix() * P + camera_t; - Eigen::Vector2d p; - spaceToPlane(P_cam, p); + Eigen::Vector2d p; + spaceToPlane(P_cam, p); - return (p - observed_p).norm(); + return (p - observed_p).norm(); } -void -Camera::projectPoints(const std::vector& objectPoints, - const cv::Mat& rvec, - const cv::Mat& tvec, - std::vector& imagePoints) const -{ - // project 3D object points to the image plane - imagePoints.reserve(objectPoints.size()); +void Camera::projectPoints(const std::vector& objectPoints, + const cv::Mat& rvec, const cv::Mat& tvec, + std::vector& imagePoints) const { + // project 3D object points to the image plane + imagePoints.reserve(objectPoints.size()); - //double - cv::Mat R0; - cv::Rodrigues(rvec, R0); + // double + cv::Mat R0; + cv::Rodrigues(rvec, R0); - Eigen::MatrixXd R(3,3); - R << R0.at(0,0), R0.at(0,1), R0.at(0,2), - R0.at(1,0), R0.at(1,1), R0.at(1,2), - R0.at(2,0), R0.at(2,1), R0.at(2,2); + Eigen::MatrixXd R(3, 3); + R << R0.at(0, 0), R0.at(0, 1), R0.at(0, 2), + R0.at(1, 0), R0.at(1, 1), R0.at(1, 2), + R0.at(2, 0), R0.at(2, 1), R0.at(2, 2); - Eigen::Vector3d t; - t << tvec.at(0), tvec.at(1), tvec.at(2); + Eigen::Vector3d t; + t << tvec.at(0), tvec.at(1), tvec.at(2); - for (size_t i = 0; i < objectPoints.size(); ++i) - { - const cv::Point3f& objectPoint = objectPoints.at(i); + for (size_t i = 0; i < objectPoints.size(); ++i) { + const cv::Point3f& objectPoint = objectPoints.at(i); - // Rotate and translate - Eigen::Vector3d P; - P << objectPoint.x, objectPoint.y, objectPoint.z; + // Rotate and translate + Eigen::Vector3d P; + P << objectPoint.x, objectPoint.y, objectPoint.z; - P = R * P + t; + P = R * P + t; - Eigen::Vector2d p; - spaceToPlane(P, p); + Eigen::Vector2d p; + spaceToPlane(P, p); - imagePoints.push_back(cv::Point2f(p(0), p(1))); - } + imagePoints.push_back(cv::Point2f(p(0), p(1))); + } } +Ray::Ray() : m_theta(0.0), m_phi(0.0) {} -Ray::Ray( ) -: m_theta( 0.0 ) -, m_phi( 0.0 ) -{ -} +Ray::Ray(double theta, double phi) : m_theta(theta), m_phi(phi) {} -Ray::Ray( double theta, double phi ) -: m_theta( theta ) -, m_phi( phi ) -{ -} +Ray::Ray(double x, double y, double z) + : m_theta(acos(z / sqrt(x * x + y * y + z * z))), m_phi(atan2(y, x)) {} -Ray::Ray( double x, double y, double z ) -: m_theta( acos( z / sqrt( x * x + y * y + z * z ) ) ) -, m_phi( atan2( y, x ) ) -{ -} +Ray::Ray(Eigen::Vector3d P) + : m_theta(acos(P(2) / P.norm())), m_phi(atan2(P(1), P(0))) {} -Ray::Ray( Eigen::Vector3d P ) -: m_theta( acos( P( 2 ) / P.norm( ) ) ) -, m_phi( atan2( P( 1 ), P( 0 ) ) ) -{ -} - -double& -Ray::theta( ) -{ - return m_theta; -} +double& Ray::theta() { return m_theta; } -double& -Ray::phi( ) -{ - return m_phi; -} +double& Ray::phi() { return m_phi; } -double -Ray::theta( ) const -{ - return m_theta; -} +double Ray::theta() const { return m_theta; } -double -Ray::phi( ) const -{ - return m_phi; -} +double Ray::phi() const { return m_phi; } -Eigen::Vector3d -Ray::toSpace( ) const -{ - return Eigen::Vector3d( sin( m_theta ) * cos( m_phi ), sin( m_theta ) * sin( m_phi ), cos( m_theta ) ); +Eigen::Vector3d Ray::toSpace() const { + return Eigen::Vector3d(sin(m_theta) * cos(m_phi), sin(m_theta) * sin(m_phi), + cos(m_theta)); } -Eigen::Vector3d -Ray::toSpace( double scale ) const -{ - return Eigen::Vector3d( sin( m_theta ) * cos( m_phi ) * scale, - sin( m_theta ) * sin( m_phi ) * scale, - cos( m_theta ) * scale ); +Eigen::Vector3d Ray::toSpace(double scale) const { + return Eigen::Vector3d(sin(m_theta) * cos(m_phi) * scale, + sin(m_theta) * sin(m_phi) * scale, + cos(m_theta) * scale); } -void -Ray::fromSpace( Eigen::Vector3d P ) -{ - m_theta = acos( P( 2 ) / P.norm( ) ); - m_phi = atan2( P( 1 ), P( 0 ) ); +void Ray::fromSpace(Eigen::Vector3d P) { + m_theta = acos(P(2) / P.norm()); + m_phi = atan2(P(1), P(0)); } -Ray& -Ray::operator=( const Ray& other ) -{ - if ( this != &other ) - { - m_theta = other.m_theta; - m_phi = other.m_phi; - } - return *this; -} +Ray& Ray::operator=(const Ray& other) { + if (this != &other) { + m_theta = other.m_theta; + m_phi = other.m_phi; + } + return *this; } +} // namespace camodocal diff --git a/camera_models/src/camera_models/CameraFactory.cc b/camera_models/src/camera_models/CameraFactory.cc index 48f9d796..b932f641 100644 --- a/camera_models/src/camera_models/CameraFactory.cc +++ b/camera_models/src/camera_models/CameraFactory.cc @@ -1,203 +1,170 @@ +#include "camodocal/camera_models/CameraFactory.h" + #include -#include "camodocal/camera_models/CameraFactory.h" #include "camodocal/camera_models/CataCamera.h" #include "camodocal/camera_models/EquidistantCamera.h" #include "camodocal/camera_models/PinholeCamera.h" #include "camodocal/camera_models/PinholeFullCamera.h" #include "camodocal/camera_models/PolyFisheyeCamera.h" #include "camodocal/camera_models/ScaramuzzaCamera.h" - #include "ceres/ceres.h" -namespace camodocal -{ +namespace camodocal { -boost::shared_ptr< CameraFactory > CameraFactory::m_instance; +boost::shared_ptr CameraFactory::m_instance; -CameraFactory::CameraFactory( ) {} +CameraFactory::CameraFactory() {} -boost::shared_ptr< CameraFactory > -CameraFactory::instance( void ) -{ - if ( m_instance.get( ) == 0 ) - { - m_instance.reset( new CameraFactory ); - } +boost::shared_ptr CameraFactory::instance(void) { + if (m_instance.get() == 0) { + m_instance.reset(new CameraFactory); + } - return m_instance; + return m_instance; } -CameraPtr -CameraFactory::generateCamera( Camera::ModelType modelType, const std::string& cameraName, cv::Size imageSize ) const -{ - switch ( modelType ) - { - case Camera::KANNALA_BRANDT: - { - EquidistantCameraPtr camera( new EquidistantCamera ); - - EquidistantCamera::Parameters params = camera->getParameters( ); - params.cameraName( ) = cameraName; - params.imageWidth( ) = imageSize.width; - params.imageHeight( ) = imageSize.height; - camera->setParameters( params ); - return camera; - } - case Camera::PINHOLE: - { - PinholeCameraPtr camera( new PinholeCamera ); - - PinholeCamera::Parameters params = camera->getParameters( ); - params.cameraName( ) = cameraName; - params.imageWidth( ) = imageSize.width; - params.imageHeight( ) = imageSize.height; - camera->setParameters( params ); - return camera; - } - case Camera::PINHOLE_FULL: - { - PinholeFullCameraPtr camera( new PinholeFullCamera ); - - PinholeFullCamera::Parameters params = camera->getParameters( ); - params.cameraName( ) = cameraName; - params.imageWidth( ) = imageSize.width; - params.imageHeight( ) = imageSize.height; - camera->setParameters( params ); - return camera; - } - case Camera::SCARAMUZZA: - { - OCAMCameraPtr camera( new OCAMCamera ); - - OCAMCamera::Parameters params = camera->getParameters( ); - params.cameraName( ) = cameraName; - params.imageWidth( ) = imageSize.width; - params.imageHeight( ) = imageSize.height; - camera->setParameters( params ); - return camera; - } - case Camera::MEI: - default: - { - CataCameraPtr camera( new CataCamera ); - - CataCamera::Parameters params = camera->getParameters( ); - params.cameraName( ) = cameraName; - params.imageWidth( ) = imageSize.width; - params.imageHeight( ) = imageSize.height; - camera->setParameters( params ); - return camera; - } +CameraPtr CameraFactory::generateCamera(Camera::ModelType modelType, + const std::string& cameraName, + cv::Size imageSize) const { + switch (modelType) { + case Camera::KANNALA_BRANDT: { + EquidistantCameraPtr camera(new EquidistantCamera); + + EquidistantCamera::Parameters params = camera->getParameters(); + params.cameraName() = cameraName; + params.imageWidth() = imageSize.width; + params.imageHeight() = imageSize.height; + camera->setParameters(params); + return camera; + } + case Camera::PINHOLE: { + PinholeCameraPtr camera(new PinholeCamera); + + PinholeCamera::Parameters params = camera->getParameters(); + params.cameraName() = cameraName; + params.imageWidth() = imageSize.width; + params.imageHeight() = imageSize.height; + camera->setParameters(params); + return camera; + } + case Camera::PINHOLE_FULL: { + PinholeFullCameraPtr camera(new PinholeFullCamera); + + PinholeFullCamera::Parameters params = camera->getParameters(); + params.cameraName() = cameraName; + params.imageWidth() = imageSize.width; + params.imageHeight() = imageSize.height; + camera->setParameters(params); + return camera; + } + case Camera::SCARAMUZZA: { + OCAMCameraPtr camera(new OCAMCamera); + + OCAMCamera::Parameters params = camera->getParameters(); + params.cameraName() = cameraName; + params.imageWidth() = imageSize.width; + params.imageHeight() = imageSize.height; + camera->setParameters(params); + return camera; + } + case Camera::MEI: + default: { + CataCameraPtr camera(new CataCamera); + + CataCamera::Parameters params = camera->getParameters(); + params.cameraName() = cameraName; + params.imageWidth() = imageSize.width; + params.imageHeight() = imageSize.height; + camera->setParameters(params); + return camera; } + } } -CameraPtr -CameraFactory::generateCameraFromYamlFile( const std::string& filename ) -{ - cv::FileStorage fs( filename, cv::FileStorage::READ ); +CameraPtr CameraFactory::generateCameraFromYamlFile( + const std::string& filename) { + cv::FileStorage fs(filename, cv::FileStorage::READ); + + if (!fs.isOpened()) { + return CameraPtr(); + } + + Camera::ModelType modelType = Camera::MEI; + if (!fs["model_type"].isNone()) { + std::string sModelType; + fs["model_type"] >> sModelType; + + if (boost::iequals(sModelType, "KANNALA_BRANDT")) { + modelType = Camera::KANNALA_BRANDT; + } else if (boost::iequals(sModelType, "MEI")) { + modelType = Camera::MEI; + } else if (boost::iequals(sModelType, "SCARAMUZZA")) { + modelType = Camera::SCARAMUZZA; + } else if (boost::iequals(sModelType, "PINHOLE")) { + modelType = Camera::PINHOLE; + } else if (boost::iequals(sModelType, "PINHOLE_FULL")) { + modelType = Camera::PINHOLE_FULL; + } else if (boost::iequals(sModelType, "POLYFISHEYE")) { + modelType = Camera::POLYFISHEYE; + } else { + std::cerr << "# ERROR: Unknown camera model: " << sModelType << std::endl; + return CameraPtr(); + } + } + + switch (modelType) { + case Camera::KANNALA_BRANDT: { + EquidistantCameraPtr camera(new EquidistantCamera); - if ( !fs.isOpened( ) ) - { - return CameraPtr( ); + EquidistantCamera::Parameters params = camera->getParameters(); + params.readFromYamlFile(filename); + camera->setParameters(params); + return camera; } + case Camera::PINHOLE: { + PinholeCameraPtr camera(new PinholeCamera); - Camera::ModelType modelType = Camera::MEI; - if ( !fs["model_type"].isNone( ) ) - { - std::string sModelType; - fs["model_type"] >> sModelType; - - if ( boost::iequals( sModelType, "KANNALA_BRANDT" ) ) - { - modelType = Camera::KANNALA_BRANDT; - } - else if ( boost::iequals( sModelType, "MEI" ) ) - { - modelType = Camera::MEI; - } - else if ( boost::iequals( sModelType, "SCARAMUZZA" ) ) - { - modelType = Camera::SCARAMUZZA; - } - else if ( boost::iequals( sModelType, "PINHOLE" ) ) - { - modelType = Camera::PINHOLE; - } - else if ( boost::iequals( sModelType, "PINHOLE_FULL" ) ) - { - modelType = Camera::PINHOLE_FULL; - } - else if ( boost::iequals( sModelType, "POLYFISHEYE" ) ) - { - modelType = Camera::POLYFISHEYE; - } - else - { - std::cerr << "# ERROR: Unknown camera model: " << sModelType << std::endl; - return CameraPtr( ); - } + PinholeCamera::Parameters params = camera->getParameters(); + params.readFromYamlFile(filename); + camera->setParameters(params); + return camera; } + case Camera::PINHOLE_FULL: { + PinholeFullCameraPtr camera(new PinholeFullCamera); - switch ( modelType ) - { - case Camera::KANNALA_BRANDT: - { - EquidistantCameraPtr camera( new EquidistantCamera ); - - EquidistantCamera::Parameters params = camera->getParameters( ); - params.readFromYamlFile( filename ); - camera->setParameters( params ); - return camera; - } - case Camera::PINHOLE: - { - PinholeCameraPtr camera( new PinholeCamera ); - - PinholeCamera::Parameters params = camera->getParameters( ); - params.readFromYamlFile( filename ); - camera->setParameters( params ); - return camera; - } - case Camera::PINHOLE_FULL: - { - PinholeFullCameraPtr camera( new PinholeFullCamera ); - - PinholeFullCamera::Parameters params = camera->getParameters( ); - params.readFromYamlFile( filename ); - camera->setParameters( params ); - return camera; - } - case Camera::SCARAMUZZA: - { - OCAMCameraPtr camera( new OCAMCamera ); - - OCAMCamera::Parameters params = camera->getParameters( ); - params.readFromYamlFile( filename ); - camera->setParameters( params ); - return camera; - } - case Camera::POLYFISHEYE: - { - PolyFisheyeCameraPtr camera( new PolyFisheyeCamera ); - - PolyFisheyeCamera::Parameters params = camera->getParameters( ); - params.readFromYamlFile( filename ); - camera->setParameters( params ); - return camera; - } - case Camera::MEI: - default: - { - CataCameraPtr camera( new CataCamera ); - - CataCamera::Parameters params = camera->getParameters( ); - params.readFromYamlFile( filename ); - camera->setParameters( params ); - return camera; - } + PinholeFullCamera::Parameters params = camera->getParameters(); + params.readFromYamlFile(filename); + camera->setParameters(params); + return camera; } + case Camera::SCARAMUZZA: { + OCAMCameraPtr camera(new OCAMCamera); - return CameraPtr( ); -} + OCAMCamera::Parameters params = camera->getParameters(); + params.readFromYamlFile(filename); + camera->setParameters(params); + return camera; + } + case Camera::POLYFISHEYE: { + PolyFisheyeCameraPtr camera(new PolyFisheyeCamera); + + PolyFisheyeCamera::Parameters params = camera->getParameters(); + params.readFromYamlFile(filename); + camera->setParameters(params); + return camera; + } + case Camera::MEI: + default: { + CataCameraPtr camera(new CataCamera); + + CataCamera::Parameters params = camera->getParameters(); + params.readFromYamlFile(filename); + camera->setParameters(params); + return camera; + } + } + + return CameraPtr(); } +} // namespace camodocal diff --git a/camera_models/src/camera_models/CataCamera.cc b/camera_models/src/camera_models/CataCamera.cc index 7eacaa52..5103c6de 100644 --- a/camera_models/src/camera_models/CataCamera.cc +++ b/camera_models/src/camera_models/CataCamera.cc @@ -11,651 +11,507 @@ #include "camodocal/gpl/gpl.h" -namespace camodocal -{ +namespace camodocal { CataCamera::Parameters::Parameters() - : Camera::Parameters(MEI) - , m_xi(0.0) - , m_k1(0.0) - , m_k2(0.0) - , m_p1(0.0) - , m_p2(0.0) - , m_gamma1(0.0) - , m_gamma2(0.0) - , m_u0(0.0) - , m_v0(0.0) -{ - -} - -CataCamera::Parameters::Parameters(const std::string& cameraName, - int w, int h, - double xi, - double k1, double k2, - double p1, double p2, - double gamma1, double gamma2, + : Camera::Parameters(MEI), + m_xi(0.0), + m_k1(0.0), + m_k2(0.0), + m_p1(0.0), + m_p2(0.0), + m_gamma1(0.0), + m_gamma2(0.0), + m_u0(0.0), + m_v0(0.0) {} + +CataCamera::Parameters::Parameters(const std::string& cameraName, int w, int h, + double xi, double k1, double k2, double p1, + double p2, double gamma1, double gamma2, double u0, double v0) - : Camera::Parameters(MEI, cameraName, w, h) - , m_xi(xi) - , m_k1(k1) - , m_k2(k2) - , m_p1(p1) - , m_p2(p2) - , m_gamma1(gamma1) - , m_gamma2(gamma2) - , m_u0(u0) - , m_v0(v0) -{ -} + : Camera::Parameters(MEI, cameraName, w, h), + m_xi(xi), + m_k1(k1), + m_k2(k2), + m_p1(p1), + m_p2(p2), + m_gamma1(gamma1), + m_gamma2(gamma2), + m_u0(u0), + m_v0(v0) {} -double& -CataCamera::Parameters::xi(void) -{ - return m_xi; -} +double& CataCamera::Parameters::xi(void) { return m_xi; } -double& -CataCamera::Parameters::k1(void) -{ - return m_k1; -} +double& CataCamera::Parameters::k1(void) { return m_k1; } -double& -CataCamera::Parameters::k2(void) -{ - return m_k2; -} +double& CataCamera::Parameters::k2(void) { return m_k2; } -double& -CataCamera::Parameters::p1(void) -{ - return m_p1; -} +double& CataCamera::Parameters::p1(void) { return m_p1; } -double& -CataCamera::Parameters::p2(void) -{ - return m_p2; -} +double& CataCamera::Parameters::p2(void) { return m_p2; } -double& -CataCamera::Parameters::gamma1(void) -{ - return m_gamma1; -} +double& CataCamera::Parameters::gamma1(void) { return m_gamma1; } -double& -CataCamera::Parameters::gamma2(void) -{ - return m_gamma2; -} +double& CataCamera::Parameters::gamma2(void) { return m_gamma2; } -double& -CataCamera::Parameters::u0(void) -{ - return m_u0; -} +double& CataCamera::Parameters::u0(void) { return m_u0; } -double& -CataCamera::Parameters::v0(void) -{ - return m_v0; -} +double& CataCamera::Parameters::v0(void) { return m_v0; } -double -CataCamera::Parameters::xi(void) const -{ - return m_xi; -} +double CataCamera::Parameters::xi(void) const { return m_xi; } -double -CataCamera::Parameters::k1(void) const -{ - return m_k1; -} +double CataCamera::Parameters::k1(void) const { return m_k1; } -double -CataCamera::Parameters::k2(void) const -{ - return m_k2; -} +double CataCamera::Parameters::k2(void) const { return m_k2; } -double -CataCamera::Parameters::p1(void) const -{ - return m_p1; -} +double CataCamera::Parameters::p1(void) const { return m_p1; } -double -CataCamera::Parameters::p2(void) const -{ - return m_p2; -} +double CataCamera::Parameters::p2(void) const { return m_p2; } -double -CataCamera::Parameters::gamma1(void) const -{ - return m_gamma1; -} +double CataCamera::Parameters::gamma1(void) const { return m_gamma1; } -double -CataCamera::Parameters::gamma2(void) const -{ - return m_gamma2; -} +double CataCamera::Parameters::gamma2(void) const { return m_gamma2; } -double -CataCamera::Parameters::u0(void) const -{ - return m_u0; -} +double CataCamera::Parameters::u0(void) const { return m_u0; } -double -CataCamera::Parameters::v0(void) const -{ - return m_v0; -} +double CataCamera::Parameters::v0(void) const { return m_v0; } -bool -CataCamera::Parameters::readFromYamlFile(const std::string& filename) -{ - cv::FileStorage fs(filename, cv::FileStorage::READ); +bool CataCamera::Parameters::readFromYamlFile(const std::string& filename) { + cv::FileStorage fs(filename, cv::FileStorage::READ); - if (!fs.isOpened()) - { - return false; - } + if (!fs.isOpened()) { + return false; + } - if (!fs["model_type"].isNone()) - { - std::string sModelType; - fs["model_type"] >> sModelType; + if (!fs["model_type"].isNone()) { + std::string sModelType; + fs["model_type"] >> sModelType; - if (sModelType.compare("MEI") != 0) - { - return false; - } + if (sModelType.compare("MEI") != 0) { + return false; } + } - m_modelType = MEI; - fs["camera_name"] >> m_cameraName; - m_imageWidth = static_cast(fs["image_width"]); - m_imageHeight = static_cast(fs["image_height"]); + m_modelType = MEI; + fs["camera_name"] >> m_cameraName; + m_imageWidth = static_cast(fs["image_width"]); + m_imageHeight = static_cast(fs["image_height"]); - cv::FileNode n = fs["mirror_parameters"]; - m_xi = static_cast(n["xi"]); + cv::FileNode n = fs["mirror_parameters"]; + m_xi = static_cast(n["xi"]); - n = fs["distortion_parameters"]; - m_k1 = static_cast(n["k1"]); - m_k2 = static_cast(n["k2"]); - m_p1 = static_cast(n["p1"]); - m_p2 = static_cast(n["p2"]); + n = fs["distortion_parameters"]; + m_k1 = static_cast(n["k1"]); + m_k2 = static_cast(n["k2"]); + m_p1 = static_cast(n["p1"]); + m_p2 = static_cast(n["p2"]); - n = fs["projection_parameters"]; - m_gamma1 = static_cast(n["gamma1"]); - m_gamma2 = static_cast(n["gamma2"]); - m_u0 = static_cast(n["u0"]); - m_v0 = static_cast(n["v0"]); + n = fs["projection_parameters"]; + m_gamma1 = static_cast(n["gamma1"]); + m_gamma2 = static_cast(n["gamma2"]); + m_u0 = static_cast(n["u0"]); + m_v0 = static_cast(n["v0"]); - return true; + return true; } -void -CataCamera::Parameters::writeToYamlFile(const std::string& filename) const -{ - cv::FileStorage fs(filename, cv::FileStorage::WRITE); - - fs << "model_type" << "MEI"; - fs << "camera_name" << m_cameraName; - fs << "image_width" << m_imageWidth; - fs << "image_height" << m_imageHeight; - - // mirror: xi - fs << "mirror_parameters"; - fs << "{" << "xi" << m_xi << "}"; - - // radial distortion: k1, k2 - // tangential distortion: p1, p2 - fs << "distortion_parameters"; - fs << "{" << "k1" << m_k1 - << "k2" << m_k2 - << "p1" << m_p1 - << "p2" << m_p2 << "}"; - - // projection: gamma1, gamma2, u0, v0 - fs << "projection_parameters"; - fs << "{" << "gamma1" << m_gamma1 - << "gamma2" << m_gamma2 - << "u0" << m_u0 - << "v0" << m_v0 << "}"; - - fs.release(); -} +void CataCamera::Parameters::writeToYamlFile( + const std::string& filename) const { + cv::FileStorage fs(filename, cv::FileStorage::WRITE); -CataCamera::Parameters& -CataCamera::Parameters::operator=(const CataCamera::Parameters& other) -{ - if (this != &other) - { - m_modelType = other.m_modelType; - m_cameraName = other.m_cameraName; - m_imageWidth = other.m_imageWidth; - m_imageHeight = other.m_imageHeight; - m_xi = other.m_xi; - m_k1 = other.m_k1; - m_k2 = other.m_k2; - m_p1 = other.m_p1; - m_p2 = other.m_p2; - m_gamma1 = other.m_gamma1; - m_gamma2 = other.m_gamma2; - m_u0 = other.m_u0; - m_v0 = other.m_v0; - } + fs << "model_type" + << "MEI"; + fs << "camera_name" << m_cameraName; + fs << "image_width" << m_imageWidth; + fs << "image_height" << m_imageHeight; - return *this; -} + // mirror: xi + fs << "mirror_parameters"; + fs << "{" + << "xi" << m_xi << "}"; -std::ostream& -operator<< (std::ostream& out, const CataCamera::Parameters& params) -{ - out << "Camera Parameters:" << std::endl; - out << " model_type " << "MEI" << std::endl; - out << " camera_name " << params.m_cameraName << std::endl; - out << " image_width " << params.m_imageWidth << std::endl; - out << " image_height " << params.m_imageHeight << std::endl; - - out << "Mirror Parameters" << std::endl; - out << std::fixed << std::setprecision(10); - out << " xi " << params.m_xi << std::endl; - - // radial distortion: k1, k2 - // tangential distortion: p1, p2 - out << "Distortion Parameters" << std::endl; - out << " k1 " << params.m_k1 << std::endl - << " k2 " << params.m_k2 << std::endl - << " p1 " << params.m_p1 << std::endl - << " p2 " << params.m_p2 << std::endl; - - // projection: gamma1, gamma2, u0, v0 - out << "Projection Parameters" << std::endl; - out << " gamma1 " << params.m_gamma1 << std::endl - << " gamma2 " << params.m_gamma2 << std::endl - << " u0 " << params.m_u0 << std::endl - << " v0 " << params.m_v0 << std::endl; - - return out; + // radial distortion: k1, k2 + // tangential distortion: p1, p2 + fs << "distortion_parameters"; + fs << "{" + << "k1" << m_k1 << "k2" << m_k2 << "p1" << m_p1 << "p2" << m_p2 << "}"; + + // projection: gamma1, gamma2, u0, v0 + fs << "projection_parameters"; + fs << "{" + << "gamma1" << m_gamma1 << "gamma2" << m_gamma2 << "u0" << m_u0 << "v0" + << m_v0 << "}"; + + fs.release(); } -CataCamera::CataCamera() - : m_inv_K11(1.0) - , m_inv_K13(0.0) - , m_inv_K22(1.0) - , m_inv_K23(0.0) - , m_noDistortion(true) -{ +CataCamera::Parameters& CataCamera::Parameters::operator=( + const CataCamera::Parameters& other) { + if (this != &other) { + m_modelType = other.m_modelType; + m_cameraName = other.m_cameraName; + m_imageWidth = other.m_imageWidth; + m_imageHeight = other.m_imageHeight; + m_xi = other.m_xi; + m_k1 = other.m_k1; + m_k2 = other.m_k2; + m_p1 = other.m_p1; + m_p2 = other.m_p2; + m_gamma1 = other.m_gamma1; + m_gamma2 = other.m_gamma2; + m_u0 = other.m_u0; + m_v0 = other.m_v0; + } + return *this; } -CataCamera::CataCamera(const std::string& cameraName, - int imageWidth, int imageHeight, - double xi, double k1, double k2, double p1, double p2, - double gamma1, double gamma2, double u0, double v0) - : mParameters(cameraName, imageWidth, imageHeight, - xi, k1, k2, p1, p2, gamma1, gamma2, u0, v0) -{ - if ((mParameters.k1() == 0.0) && - (mParameters.k2() == 0.0) && - (mParameters.p1() == 0.0) && - (mParameters.p2() == 0.0)) - { - m_noDistortion = true; - } - else - { - m_noDistortion = false; - } +std::ostream& operator<<(std::ostream& out, + const CataCamera::Parameters& params) { + out << "Camera Parameters:" << std::endl; + out << " model_type " + << "MEI" << std::endl; + out << " camera_name " << params.m_cameraName << std::endl; + out << " image_width " << params.m_imageWidth << std::endl; + out << " image_height " << params.m_imageHeight << std::endl; + + out << "Mirror Parameters" << std::endl; + out << std::fixed << std::setprecision(10); + out << " xi " << params.m_xi << std::endl; + + // radial distortion: k1, k2 + // tangential distortion: p1, p2 + out << "Distortion Parameters" << std::endl; + out << " k1 " << params.m_k1 << std::endl + << " k2 " << params.m_k2 << std::endl + << " p1 " << params.m_p1 << std::endl + << " p2 " << params.m_p2 << std::endl; + + // projection: gamma1, gamma2, u0, v0 + out << "Projection Parameters" << std::endl; + out << " gamma1 " << params.m_gamma1 << std::endl + << " gamma2 " << params.m_gamma2 << std::endl + << " u0 " << params.m_u0 << std::endl + << " v0 " << params.m_v0 << std::endl; + + return out; +} - // Inverse camera projection matrix parameters - m_inv_K11 = 1.0 / mParameters.gamma1(); - m_inv_K13 = -mParameters.u0() / mParameters.gamma1(); - m_inv_K22 = 1.0 / mParameters.gamma2(); - m_inv_K23 = -mParameters.v0() / mParameters.gamma2(); +CataCamera::CataCamera() + : m_inv_K11(1.0), + m_inv_K13(0.0), + m_inv_K22(1.0), + m_inv_K23(0.0), + m_noDistortion(true) {} + +CataCamera::CataCamera(const std::string& cameraName, int imageWidth, + int imageHeight, double xi, double k1, double k2, + double p1, double p2, double gamma1, double gamma2, + double u0, double v0) + : mParameters(cameraName, imageWidth, imageHeight, xi, k1, k2, p1, p2, + gamma1, gamma2, u0, v0) { + if ((mParameters.k1() == 0.0) && (mParameters.k2() == 0.0) && + (mParameters.p1() == 0.0) && (mParameters.p2() == 0.0)) { + m_noDistortion = true; + } else { + m_noDistortion = false; + } + + // Inverse camera projection matrix parameters + m_inv_K11 = 1.0 / mParameters.gamma1(); + m_inv_K13 = -mParameters.u0() / mParameters.gamma1(); + m_inv_K22 = 1.0 / mParameters.gamma2(); + m_inv_K23 = -mParameters.v0() / mParameters.gamma2(); } CataCamera::CataCamera(const CataCamera::Parameters& params) - : mParameters(params) -{ - if ((mParameters.k1() == 0.0) && - (mParameters.k2() == 0.0) && - (mParameters.p1() == 0.0) && - (mParameters.p2() == 0.0)) - { - m_noDistortion = true; - } - else - { - m_noDistortion = false; - } + : mParameters(params) { + if ((mParameters.k1() == 0.0) && (mParameters.k2() == 0.0) && + (mParameters.p1() == 0.0) && (mParameters.p2() == 0.0)) { + m_noDistortion = true; + } else { + m_noDistortion = false; + } + + // Inverse camera projection matrix parameters + m_inv_K11 = 1.0 / mParameters.gamma1(); + m_inv_K13 = -mParameters.u0() / mParameters.gamma1(); + m_inv_K22 = 1.0 / mParameters.gamma2(); + m_inv_K23 = -mParameters.v0() / mParameters.gamma2(); +} - // Inverse camera projection matrix parameters - m_inv_K11 = 1.0 / mParameters.gamma1(); - m_inv_K13 = -mParameters.u0() / mParameters.gamma1(); - m_inv_K22 = 1.0 / mParameters.gamma2(); - m_inv_K23 = -mParameters.v0() / mParameters.gamma2(); -} +Camera::ModelType CataCamera::modelType(void) const { + return mParameters.modelType(); +} -Camera::ModelType -CataCamera::modelType(void) const -{ - return mParameters.modelType(); +const std::string& CataCamera::cameraName(void) const { + return mParameters.cameraName(); } -const std::string& -CataCamera::cameraName(void) const -{ - return mParameters.cameraName(); -} +int CataCamera::imageWidth(void) const { return mParameters.imageWidth(); } -int -CataCamera::imageWidth(void) const -{ - return mParameters.imageWidth(); -} +int CataCamera::imageHeight(void) const { return mParameters.imageHeight(); } -int -CataCamera::imageHeight(void) const -{ - return mParameters.imageHeight(); -} +void CataCamera::estimateIntrinsics( + const cv::Size& boardSize, + const std::vector >& objectPoints, + const std::vector >& imagePoints) { + Parameters params = getParameters(); -void -CataCamera::estimateIntrinsics(const cv::Size& boardSize, - const std::vector< std::vector >& objectPoints, - const std::vector< std::vector >& imagePoints) -{ - Parameters params = getParameters(); + double u0 = params.imageWidth() / 2.0; + double v0 = params.imageHeight() / 2.0; - double u0 = params.imageWidth() / 2.0; - double v0 = params.imageHeight() / 2.0; + double gamma0 = 0.0; + double minReprojErr = std::numeric_limits::max(); - double gamma0 = 0.0; - double minReprojErr = std::numeric_limits::max(); + std::vector rvecs, tvecs; + rvecs.assign(objectPoints.size(), cv::Mat()); + tvecs.assign(objectPoints.size(), cv::Mat()); - std::vector rvecs, tvecs; - rvecs.assign(objectPoints.size(), cv::Mat()); - tvecs.assign(objectPoints.size(), cv::Mat()); + params.xi() = 1.0; + params.k1() = 0.0; + params.k2() = 0.0; + params.p1() = 0.0; + params.p2() = 0.0; + params.u0() = u0; + params.v0() = v0; - params.xi() = 1.0; - params.k1() = 0.0; - params.k2() = 0.0; - params.p1() = 0.0; - params.p2() = 0.0; - params.u0() = u0; - params.v0() = v0; + // Initialize gamma (focal length) + // Use non-radial line image and xi = 1 + for (size_t i = 0; i < imagePoints.size(); ++i) { + for (int r = 0; r < boardSize.height; ++r) { + cv::Mat P(boardSize.width, 4, CV_64F); + for (int c = 0; c < boardSize.width; ++c) { + const cv::Point2f& imagePoint = + imagePoints.at(i).at(r * boardSize.width + c); - // Initialize gamma (focal length) - // Use non-radial line image and xi = 1 - for (size_t i = 0; i < imagePoints.size(); ++i) - { - for (int r = 0; r < boardSize.height; ++r) - { - cv::Mat P(boardSize.width, 4, CV_64F); - for (int c = 0; c < boardSize.width; ++c) - { - const cv::Point2f& imagePoint = imagePoints.at(i).at(r * boardSize.width + c); - - double u = imagePoint.x - u0; - double v = imagePoint.y - v0; - - P.at(c, 0) = u; - P.at(c, 1) = v; - P.at(c, 2) = 0.5; - P.at(c, 3) = -0.5 * (square(u) + square(v)); - } - - cv::Mat C; - cv::SVD::solveZ(P, C); - - double t = square(C.at(0)) + square(C.at(1)) + C.at(2) * C.at(3); - if (t < 0.0) - { - continue; - } - - // check that line image is not radial - double d = sqrt(1.0 / t); - double nx = C.at(0) * d; - double ny = C.at(1) * d; - if (hypot(nx, ny) > 0.95) - { - continue; - } - - double gamma = sqrt(C.at(2) / C.at(3)); - - params.gamma1() = gamma; - params.gamma2() = gamma; - setParameters(params); - - for (size_t j = 0; j < objectPoints.size(); ++j) - { - estimateExtrinsics(objectPoints.at(j), imagePoints.at(j), rvecs.at(j), tvecs.at(j)); - } - - double reprojErr = reprojectionError(objectPoints, imagePoints, rvecs, tvecs, cv::noArray()); - - if (reprojErr < minReprojErr) - { - minReprojErr = reprojErr; - gamma0 = gamma; - } - } - } + double u = imagePoint.x - u0; + double v = imagePoint.y - v0; - if (gamma0 <= 0.0 && minReprojErr >= std::numeric_limits::max()) - { - std::cout << "[" << params.cameraName() << "] " - << "# INFO: CataCamera model fails with given data. " << std::endl; + P.at(c, 0) = u; + P.at(c, 1) = v; + P.at(c, 2) = 0.5; + P.at(c, 3) = -0.5 * (square(u) + square(v)); + } + + cv::Mat C; + cv::SVD::solveZ(P, C); + + double t = square(C.at(0)) + square(C.at(1)) + + C.at(2) * C.at(3); + if (t < 0.0) { + continue; + } - return; + // check that line image is not radial + double d = sqrt(1.0 / t); + double nx = C.at(0) * d; + double ny = C.at(1) * d; + if (hypot(nx, ny) > 0.95) { + continue; + } + + double gamma = sqrt(C.at(2) / C.at(3)); + + params.gamma1() = gamma; + params.gamma2() = gamma; + setParameters(params); + + for (size_t j = 0; j < objectPoints.size(); ++j) { + estimateExtrinsics(objectPoints.at(j), imagePoints.at(j), rvecs.at(j), + tvecs.at(j)); + } + + double reprojErr = reprojectionError(objectPoints, imagePoints, rvecs, + tvecs, cv::noArray()); + + if (reprojErr < minReprojErr) { + minReprojErr = reprojErr; + gamma0 = gamma; + } } + } + + if (gamma0 <= 0.0 && minReprojErr >= std::numeric_limits::max()) { + std::cout << "[" << params.cameraName() << "] " + << "# INFO: CataCamera model fails with given data. " + << std::endl; + + return; + } - params.gamma1() = gamma0; - params.gamma2() = gamma0; - setParameters(params); + params.gamma1() = gamma0; + params.gamma2() = gamma0; + setParameters(params); } -/** +/** * \brief Lifts a point from the image plane to the unit sphere * * \param p image coordinates * \param P coordinates of the point on the sphere */ -void -CataCamera::liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const -{ - double mx_d, my_d,mx2_d, mxy_d, my2_d, mx_u, my_u; - double rho2_d, rho4_d, radDist_d, Dx_d, Dy_d, inv_denom_d; - double lambda; - - // Lift points to normalised plane - mx_d = m_inv_K11 * p(0) + m_inv_K13; - my_d = m_inv_K22 * p(1) + m_inv_K23; - - if (m_noDistortion) - { - mx_u = mx_d; - my_u = my_d; - } - else - { - // Apply inverse distortion model - if (0) - { - double k1 = mParameters.k1(); - double k2 = mParameters.k2(); - double p1 = mParameters.p1(); - double p2 = mParameters.p2(); - - // Inverse distortion model - // proposed by Heikkila - mx2_d = mx_d*mx_d; - my2_d = my_d*my_d; - mxy_d = mx_d*my_d; - rho2_d = mx2_d+my2_d; - rho4_d = rho2_d*rho2_d; - radDist_d = k1*rho2_d+k2*rho4_d; - Dx_d = mx_d*radDist_d + p2*(rho2_d+2*mx2_d) + 2*p1*mxy_d; - Dy_d = my_d*radDist_d + p1*(rho2_d+2*my2_d) + 2*p2*mxy_d; - inv_denom_d = 1/(1+4*k1*rho2_d+6*k2*rho4_d+8*p1*my_d+8*p2*mx_d); - - mx_u = mx_d - inv_denom_d*Dx_d; - my_u = my_d - inv_denom_d*Dy_d; - } - else - { - // Recursive distortion model - int n = 6; - Eigen::Vector2d d_u; - distortion(Eigen::Vector2d(mx_d, my_d), d_u); - // Approximate value - mx_u = mx_d - d_u(0); - my_u = my_d - d_u(1); - - for (int i = 1; i < n; ++i) - { - distortion(Eigen::Vector2d(mx_u, my_u), d_u); - mx_u = mx_d - d_u(0); - my_u = my_d - d_u(1); - } - } - } - - // Lift normalised points to the sphere (inv_hslash) - double xi = mParameters.xi(); - if (xi == 1.0) - { - lambda = 2.0 / (mx_u * mx_u + my_u * my_u + 1.0); - P << lambda * mx_u, lambda * my_u, lambda - 1.0; - } - else - { - lambda = (xi + sqrt(1.0 + (1.0 - xi * xi) * (mx_u * mx_u + my_u * my_u))) / (1.0 + mx_u * mx_u + my_u * my_u); - P << lambda * mx_u, lambda * my_u, lambda - xi; - } -} - -/** +void CataCamera::liftSphere(const Eigen::Vector2d& p, + Eigen::Vector3d& P) const { + double mx_d, my_d, mx2_d, mxy_d, my2_d, mx_u, my_u; + double rho2_d, rho4_d, radDist_d, Dx_d, Dy_d, inv_denom_d; + double lambda; + + // Lift points to normalised plane + mx_d = m_inv_K11 * p(0) + m_inv_K13; + my_d = m_inv_K22 * p(1) + m_inv_K23; + + if (m_noDistortion) { + mx_u = mx_d; + my_u = my_d; + } else { + // Apply inverse distortion model + if (0) { + double k1 = mParameters.k1(); + double k2 = mParameters.k2(); + double p1 = mParameters.p1(); + double p2 = mParameters.p2(); + + // Inverse distortion model + // proposed by Heikkila + mx2_d = mx_d * mx_d; + my2_d = my_d * my_d; + mxy_d = mx_d * my_d; + rho2_d = mx2_d + my2_d; + rho4_d = rho2_d * rho2_d; + radDist_d = k1 * rho2_d + k2 * rho4_d; + Dx_d = mx_d * radDist_d + p2 * (rho2_d + 2 * mx2_d) + 2 * p1 * mxy_d; + Dy_d = my_d * radDist_d + p1 * (rho2_d + 2 * my2_d) + 2 * p2 * mxy_d; + inv_denom_d = 1 / (1 + 4 * k1 * rho2_d + 6 * k2 * rho4_d + 8 * p1 * my_d + + 8 * p2 * mx_d); + + mx_u = mx_d - inv_denom_d * Dx_d; + my_u = my_d - inv_denom_d * Dy_d; + } else { + // Recursive distortion model + int n = 6; + Eigen::Vector2d d_u; + distortion(Eigen::Vector2d(mx_d, my_d), d_u); + // Approximate value + mx_u = mx_d - d_u(0); + my_u = my_d - d_u(1); + + for (int i = 1; i < n; ++i) { + distortion(Eigen::Vector2d(mx_u, my_u), d_u); + mx_u = mx_d - d_u(0); + my_u = my_d - d_u(1); + } + } + } + + // Lift normalised points to the sphere (inv_hslash) + double xi = mParameters.xi(); + if (xi == 1.0) { + lambda = 2.0 / (mx_u * mx_u + my_u * my_u + 1.0); + P << lambda * mx_u, lambda * my_u, lambda - 1.0; + } else { + lambda = (xi + sqrt(1.0 + (1.0 - xi * xi) * (mx_u * mx_u + my_u * my_u))) / + (1.0 + mx_u * mx_u + my_u * my_u); + P << lambda * mx_u, lambda * my_u, lambda - xi; + } +} + +/** * \brief Lifts a point from the image plane to its projective ray * * \param p image coordinates * \param P coordinates of the projective ray */ -void -CataCamera::liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const -{ - double mx_d, my_d,mx2_d, mxy_d, my2_d, mx_u, my_u; - double rho2_d, rho4_d, radDist_d, Dx_d, Dy_d, inv_denom_d; - //double lambda; - - // Lift points to normalised plane - mx_d = m_inv_K11 * p(0) + m_inv_K13; - my_d = m_inv_K22 * p(1) + m_inv_K23; - - if (m_noDistortion) - { - mx_u = mx_d; - my_u = my_d; - } - else - { - if (0) - { - double k1 = mParameters.k1(); - double k2 = mParameters.k2(); - double p1 = mParameters.p1(); - double p2 = mParameters.p2(); - - // Apply inverse distortion model - // proposed by Heikkila - mx2_d = mx_d*mx_d; - my2_d = my_d*my_d; - mxy_d = mx_d*my_d; - rho2_d = mx2_d+my2_d; - rho4_d = rho2_d*rho2_d; - radDist_d = k1*rho2_d+k2*rho4_d; - Dx_d = mx_d*radDist_d + p2*(rho2_d+2*mx2_d) + 2*p1*mxy_d; - Dy_d = my_d*radDist_d + p1*(rho2_d+2*my2_d) + 2*p2*mxy_d; - inv_denom_d = 1/(1+4*k1*rho2_d+6*k2*rho4_d+8*p1*my_d+8*p2*mx_d); - - mx_u = mx_d - inv_denom_d*Dx_d; - my_u = my_d - inv_denom_d*Dy_d; - } - else - { - // Recursive distortion model - int n = 8; - Eigen::Vector2d d_u; - distortion(Eigen::Vector2d(mx_d, my_d), d_u); - // Approximate value - mx_u = mx_d - d_u(0); - my_u = my_d - d_u(1); - - for (int i = 1; i < n; ++i) - { - distortion(Eigen::Vector2d(mx_u, my_u), d_u); - mx_u = mx_d - d_u(0); - my_u = my_d - d_u(1); - } - } - } - - // Obtain a projective ray - double xi = mParameters.xi(); - if (xi == 1.0) - { - P << mx_u, my_u, (1.0 - mx_u * mx_u - my_u * my_u) / 2.0; - } - else - { - // Reuse variable - rho2_d = mx_u * mx_u + my_u * my_u; - P << mx_u, my_u, 1.0 - xi * (rho2_d + 1.0) / (xi + sqrt(1.0 + (1.0 - xi * xi) * rho2_d)); - } -} - - -/** +void CataCamera::liftProjective(const Eigen::Vector2d& p, + Eigen::Vector3d& P) const { + double mx_d, my_d, mx2_d, mxy_d, my2_d, mx_u, my_u; + double rho2_d, rho4_d, radDist_d, Dx_d, Dy_d, inv_denom_d; + // double lambda; + + // Lift points to normalised plane + mx_d = m_inv_K11 * p(0) + m_inv_K13; + my_d = m_inv_K22 * p(1) + m_inv_K23; + + if (m_noDistortion) { + mx_u = mx_d; + my_u = my_d; + } else { + if (0) { + double k1 = mParameters.k1(); + double k2 = mParameters.k2(); + double p1 = mParameters.p1(); + double p2 = mParameters.p2(); + + // Apply inverse distortion model + // proposed by Heikkila + mx2_d = mx_d * mx_d; + my2_d = my_d * my_d; + mxy_d = mx_d * my_d; + rho2_d = mx2_d + my2_d; + rho4_d = rho2_d * rho2_d; + radDist_d = k1 * rho2_d + k2 * rho4_d; + Dx_d = mx_d * radDist_d + p2 * (rho2_d + 2 * mx2_d) + 2 * p1 * mxy_d; + Dy_d = my_d * radDist_d + p1 * (rho2_d + 2 * my2_d) + 2 * p2 * mxy_d; + inv_denom_d = 1 / (1 + 4 * k1 * rho2_d + 6 * k2 * rho4_d + 8 * p1 * my_d + + 8 * p2 * mx_d); + + mx_u = mx_d - inv_denom_d * Dx_d; + my_u = my_d - inv_denom_d * Dy_d; + } else { + // Recursive distortion model + int n = 8; + Eigen::Vector2d d_u; + distortion(Eigen::Vector2d(mx_d, my_d), d_u); + // Approximate value + mx_u = mx_d - d_u(0); + my_u = my_d - d_u(1); + + for (int i = 1; i < n; ++i) { + distortion(Eigen::Vector2d(mx_u, my_u), d_u); + mx_u = mx_d - d_u(0); + my_u = my_d - d_u(1); + } + } + } + + // Obtain a projective ray + double xi = mParameters.xi(); + if (xi == 1.0) { + P << mx_u, my_u, (1.0 - mx_u * mx_u - my_u * my_u) / 2.0; + } else { + // Reuse variable + rho2_d = mx_u * mx_u + my_u * my_u; + P << mx_u, my_u, + 1.0 - xi * (rho2_d + 1.0) / (xi + sqrt(1.0 + (1.0 - xi * xi) * rho2_d)); + } +} + +/** * \brief Project a 3D point (\a x,\a y,\a z) to the image plane in (\a u,\a v) * * \param P 3D point coordinates * \param p return value, contains the image point coordinates */ -void -CataCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const -{ - Eigen::Vector2d p_u, p_d; +void CataCamera::spaceToPlane(const Eigen::Vector3d& P, + Eigen::Vector2d& p) const { + Eigen::Vector2d p_u, p_d; - // Project points to the normalised plane - double z = P(2) + mParameters.xi() * P.norm(); - p_u << P(0) / z, P(1) / z; + // Project points to the normalised plane + double z = P(2) + mParameters.xi() * P.norm(); + p_u << P(0) / z, P(1) / z; - if (m_noDistortion) - { - p_d = p_u; - } - else - { - // Apply distortion - Eigen::Vector2d d_u; - distortion(p_u, d_u); - p_d = p_u + d_u; - } + if (m_noDistortion) { + p_d = p_u; + } else { + // Apply distortion + Eigen::Vector2d d_u; + distortion(p_u, d_u); + p_d = p_u + d_u; + } - // Apply generalised projection matrix - p << mParameters.gamma1() * p_d(0) + mParameters.u0(), - mParameters.gamma2() * p_d(1) + mParameters.v0(); + // Apply generalised projection matrix + p << mParameters.gamma1() * p_d(0) + mParameters.u0(), + mParameters.gamma2() * p_d(1) + mParameters.v0(); } #if 0 @@ -728,278 +584,238 @@ CataCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, } #endif -/** +/** * \brief Projects an undistorted 2D point p_u to the image plane * * \param p_u 2D point coordinates * \return image point coordinates */ -void -CataCamera::undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const -{ - Eigen::Vector2d p_d; +void CataCamera::undistToPlane(const Eigen::Vector2d& p_u, + Eigen::Vector2d& p) const { + Eigen::Vector2d p_d; - if (m_noDistortion) - { - p_d = p_u; - } - else - { - // Apply distortion - Eigen::Vector2d d_u; - distortion(p_u, d_u); - p_d = p_u + d_u; - } + if (m_noDistortion) { + p_d = p_u; + } else { + // Apply distortion + Eigen::Vector2d d_u; + distortion(p_u, d_u); + p_d = p_u + d_u; + } - // Apply generalised projection matrix - p << mParameters.gamma1() * p_d(0) + mParameters.u0(), - mParameters.gamma2() * p_d(1) + mParameters.v0(); + // Apply generalised projection matrix + p << mParameters.gamma1() * p_d(0) + mParameters.u0(), + mParameters.gamma2() * p_d(1) + mParameters.v0(); } -/** +/** * \brief Apply distortion to input point (from the normalised plane) - * + * * \param p_u undistorted coordinates of point on the normalised plane * \return to obtain the distorted point: p_d = p_u + d_u */ -void -CataCamera::distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const -{ - double k1 = mParameters.k1(); - double k2 = mParameters.k2(); - double p1 = mParameters.p1(); - double p2 = mParameters.p2(); - - double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u; - - mx2_u = p_u(0) * p_u(0); - my2_u = p_u(1) * p_u(1); - mxy_u = p_u(0) * p_u(1); - rho2_u = mx2_u + my2_u; - rad_dist_u = k1 * rho2_u + k2 * rho2_u * rho2_u; - d_u << p_u(0) * rad_dist_u + 2.0 * p1 * mxy_u + p2 * (rho2_u + 2.0 * mx2_u), - p_u(1) * rad_dist_u + 2.0 * p2 * mxy_u + p1 * (rho2_u + 2.0 * my2_u); +void CataCamera::distortion(const Eigen::Vector2d& p_u, + Eigen::Vector2d& d_u) const { + double k1 = mParameters.k1(); + double k2 = mParameters.k2(); + double p1 = mParameters.p1(); + double p2 = mParameters.p2(); + + double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u; + + mx2_u = p_u(0) * p_u(0); + my2_u = p_u(1) * p_u(1); + mxy_u = p_u(0) * p_u(1); + rho2_u = mx2_u + my2_u; + rad_dist_u = k1 * rho2_u + k2 * rho2_u * rho2_u; + d_u << p_u(0) * rad_dist_u + 2.0 * p1 * mxy_u + p2 * (rho2_u + 2.0 * mx2_u), + p_u(1) * rad_dist_u + 2.0 * p2 * mxy_u + p1 * (rho2_u + 2.0 * my2_u); } -/** +/** * \brief Apply distortion to input point (from the normalised plane) * and calculate Jacobian * * \param p_u undistorted coordinates of point on the normalised plane * \return to obtain the distorted point: p_d = p_u + d_u */ -void -CataCamera::distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u, - Eigen::Matrix2d& J) const -{ - double k1 = mParameters.k1(); - double k2 = mParameters.k2(); - double p1 = mParameters.p1(); - double p2 = mParameters.p2(); - - double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u; - - mx2_u = p_u(0) * p_u(0); - my2_u = p_u(1) * p_u(1); - mxy_u = p_u(0) * p_u(1); - rho2_u = mx2_u + my2_u; - rad_dist_u = k1 * rho2_u + k2 * rho2_u * rho2_u; - d_u << p_u(0) * rad_dist_u + 2.0 * p1 * mxy_u + p2 * (rho2_u + 2.0 * mx2_u), - p_u(1) * rad_dist_u + 2.0 * p2 * mxy_u + p1 * (rho2_u + 2.0 * my2_u); - - double dxdmx = 1.0 + rad_dist_u + k1 * 2.0 * mx2_u + k2 * rho2_u * 4.0 * mx2_u + 2.0 * p1 * p_u(1) + 6.0 * p2 * p_u(0); - double dydmx = k1 * 2.0 * p_u(0) * p_u(1) + k2 * 4.0 * rho2_u * p_u(0) * p_u(1) + p1 * 2.0 * p_u(0) + 2.0 * p2 * p_u(1); - double dxdmy = dydmx; - double dydmy = 1.0 + rad_dist_u + k1 * 2.0 * my2_u + k2 * rho2_u * 4.0 * my2_u + 6.0 * p1 * p_u(1) + 2.0 * p2 * p_u(0); - - J << dxdmx, dxdmy, - dydmx, dydmy; +void CataCamera::distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u, + Eigen::Matrix2d& J) const { + double k1 = mParameters.k1(); + double k2 = mParameters.k2(); + double p1 = mParameters.p1(); + double p2 = mParameters.p2(); + + double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u; + + mx2_u = p_u(0) * p_u(0); + my2_u = p_u(1) * p_u(1); + mxy_u = p_u(0) * p_u(1); + rho2_u = mx2_u + my2_u; + rad_dist_u = k1 * rho2_u + k2 * rho2_u * rho2_u; + d_u << p_u(0) * rad_dist_u + 2.0 * p1 * mxy_u + p2 * (rho2_u + 2.0 * mx2_u), + p_u(1) * rad_dist_u + 2.0 * p2 * mxy_u + p1 * (rho2_u + 2.0 * my2_u); + + double dxdmx = 1.0 + rad_dist_u + k1 * 2.0 * mx2_u + + k2 * rho2_u * 4.0 * mx2_u + 2.0 * p1 * p_u(1) + + 6.0 * p2 * p_u(0); + double dydmx = k1 * 2.0 * p_u(0) * p_u(1) + + k2 * 4.0 * rho2_u * p_u(0) * p_u(1) + p1 * 2.0 * p_u(0) + + 2.0 * p2 * p_u(1); + double dxdmy = dydmx; + double dydmy = 1.0 + rad_dist_u + k1 * 2.0 * my2_u + + k2 * rho2_u * 4.0 * my2_u + 6.0 * p1 * p_u(1) + + 2.0 * p2 * p_u(0); + + J << dxdmx, dxdmy, dydmx, dydmy; } -void -CataCamera::initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale) const -{ - cv::Size imageSize(mParameters.imageWidth(), mParameters.imageHeight()); +void CataCamera::initUndistortMap(cv::Mat& map1, cv::Mat& map2, + double fScale) const { + cv::Size imageSize(mParameters.imageWidth(), mParameters.imageHeight()); - cv::Mat mapX = cv::Mat::zeros(imageSize, CV_32F); - cv::Mat mapY = cv::Mat::zeros(imageSize, CV_32F); + cv::Mat mapX = cv::Mat::zeros(imageSize, CV_32F); + cv::Mat mapY = cv::Mat::zeros(imageSize, CV_32F); - for (int v = 0; v < imageSize.height; ++v) - { - for (int u = 0; u < imageSize.width; ++u) - { - double mx_u = m_inv_K11 / fScale * u + m_inv_K13 / fScale; - double my_u = m_inv_K22 / fScale * v + m_inv_K23 / fScale; + for (int v = 0; v < imageSize.height; ++v) { + for (int u = 0; u < imageSize.width; ++u) { + double mx_u = m_inv_K11 / fScale * u + m_inv_K13 / fScale; + double my_u = m_inv_K22 / fScale * v + m_inv_K23 / fScale; - double xi = mParameters.xi(); - double d2 = mx_u * mx_u + my_u * my_u; + double xi = mParameters.xi(); + double d2 = mx_u * mx_u + my_u * my_u; - Eigen::Vector3d P; - P << mx_u, my_u, 1.0 - xi * (d2 + 1.0) / (xi + sqrt(1.0 + (1.0 - xi * xi) * d2)); + Eigen::Vector3d P; + P << mx_u, my_u, + 1.0 - xi * (d2 + 1.0) / (xi + sqrt(1.0 + (1.0 - xi * xi) * d2)); - Eigen::Vector2d p; - spaceToPlane(P, p); + Eigen::Vector2d p; + spaceToPlane(P, p); - mapX.at(v,u) = p(0); - mapY.at(v,u) = p(1); - } + mapX.at(v, u) = p(0); + mapY.at(v, u) = p(1); } + } - cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false); + cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false); } -cv::Mat -CataCamera::initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, - float fx, float fy, - cv::Size imageSize, - float cx, float cy, - cv::Mat rmat) const -{ - if (imageSize == cv::Size(0, 0)) - { - imageSize = cv::Size(mParameters.imageWidth(), mParameters.imageHeight()); - } +cv::Mat CataCamera::initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, + float fx, float fy, + cv::Size imageSize, float cx, + float cy, cv::Mat rmat) const { + if (imageSize == cv::Size(0, 0)) { + imageSize = cv::Size(mParameters.imageWidth(), mParameters.imageHeight()); + } - cv::Mat mapX = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F); - cv::Mat mapY = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F); + cv::Mat mapX = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F); + cv::Mat mapY = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F); - Eigen::Matrix3f K_rect; + Eigen::Matrix3f K_rect; - if (cx == -1.0f && cy == -1.0f) - { - K_rect << fx, 0, imageSize.width / 2, - 0, fy, imageSize.height / 2, - 0, 0, 1; - } - else - { - K_rect << fx, 0, cx, - 0, fy, cy, - 0, 0, 1; - } + if (cx == -1.0f && cy == -1.0f) { + K_rect << fx, 0, imageSize.width / 2, 0, fy, imageSize.height / 2, 0, 0, 1; + } else { + K_rect << fx, 0, cx, 0, fy, cy, 0, 0, 1; + } - if (fx == -1.0f || fy == -1.0f) - { - K_rect(0,0) = mParameters.gamma1(); - K_rect(1,1) = mParameters.gamma2(); - } + if (fx == -1.0f || fy == -1.0f) { + K_rect(0, 0) = mParameters.gamma1(); + K_rect(1, 1) = mParameters.gamma2(); + } - Eigen::Matrix3f K_rect_inv = K_rect.inverse(); + Eigen::Matrix3f K_rect_inv = K_rect.inverse(); - Eigen::Matrix3f R, R_inv; - cv::cv2eigen(rmat, R); - R_inv = R.inverse(); + Eigen::Matrix3f R, R_inv; + cv::cv2eigen(rmat, R); + R_inv = R.inverse(); - for (int v = 0; v < imageSize.height; ++v) - { - for (int u = 0; u < imageSize.width; ++u) - { - Eigen::Vector3f xo; - xo << u, v, 1; + for (int v = 0; v < imageSize.height; ++v) { + for (int u = 0; u < imageSize.width; ++u) { + Eigen::Vector3f xo; + xo << u, v, 1; - Eigen::Vector3f uo = R_inv * K_rect_inv * xo; + Eigen::Vector3f uo = R_inv * K_rect_inv * xo; - Eigen::Vector2d p; - spaceToPlane(uo.cast(), p); + Eigen::Vector2d p; + spaceToPlane(uo.cast(), p); - mapX.at(v,u) = p(0); - mapY.at(v,u) = p(1); - } + mapX.at(v, u) = p(0); + mapY.at(v, u) = p(1); } + } - cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false); + cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false); - cv::Mat K_rect_cv; - cv::eigen2cv(K_rect, K_rect_cv); - return K_rect_cv; + cv::Mat K_rect_cv; + cv::eigen2cv(K_rect, K_rect_cv); + return K_rect_cv; } -int -CataCamera::parameterCount(void) const -{ - return 9; -} +int CataCamera::parameterCount(void) const { return 9; } -const CataCamera::Parameters& -CataCamera::getParameters(void) const -{ - return mParameters; +const CataCamera::Parameters& CataCamera::getParameters(void) const { + return mParameters; } -void -CataCamera::setParameters(const CataCamera::Parameters& parameters) -{ - mParameters = parameters; +void CataCamera::setParameters(const CataCamera::Parameters& parameters) { + mParameters = parameters; - if ((mParameters.k1() == 0.0) && - (mParameters.k2() == 0.0) && - (mParameters.p1() == 0.0) && - (mParameters.p2() == 0.0)) - { - m_noDistortion = true; - } - else - { - m_noDistortion = false; - } + if ((mParameters.k1() == 0.0) && (mParameters.k2() == 0.0) && + (mParameters.p1() == 0.0) && (mParameters.p2() == 0.0)) { + m_noDistortion = true; + } else { + m_noDistortion = false; + } - m_inv_K11 = 1.0 / mParameters.gamma1(); - m_inv_K13 = -mParameters.u0() / mParameters.gamma1(); - m_inv_K22 = 1.0 / mParameters.gamma2(); - m_inv_K23 = -mParameters.v0() / mParameters.gamma2(); + m_inv_K11 = 1.0 / mParameters.gamma1(); + m_inv_K13 = -mParameters.u0() / mParameters.gamma1(); + m_inv_K22 = 1.0 / mParameters.gamma2(); + m_inv_K23 = -mParameters.v0() / mParameters.gamma2(); } -void -CataCamera::readParameters(const std::vector& parameterVec) -{ - if ((int)parameterVec.size() != parameterCount()) - { - return; - } +void CataCamera::readParameters(const std::vector& parameterVec) { + if ((int)parameterVec.size() != parameterCount()) { + return; + } - Parameters params = getParameters(); + Parameters params = getParameters(); - params.xi() = parameterVec.at(0); - params.k1() = parameterVec.at(1); - params.k2() = parameterVec.at(2); - params.p1() = parameterVec.at(3); - params.p2() = parameterVec.at(4); - params.gamma1() = parameterVec.at(5); - params.gamma2() = parameterVec.at(6); - params.u0() = parameterVec.at(7); - params.v0() = parameterVec.at(8); + params.xi() = parameterVec.at(0); + params.k1() = parameterVec.at(1); + params.k2() = parameterVec.at(2); + params.p1() = parameterVec.at(3); + params.p2() = parameterVec.at(4); + params.gamma1() = parameterVec.at(5); + params.gamma2() = parameterVec.at(6); + params.u0() = parameterVec.at(7); + params.v0() = parameterVec.at(8); - setParameters(params); + setParameters(params); } -void -CataCamera::writeParameters(std::vector& parameterVec) const -{ - parameterVec.resize(parameterCount()); - parameterVec.at(0) = mParameters.xi(); - parameterVec.at(1) = mParameters.k1(); - parameterVec.at(2) = mParameters.k2(); - parameterVec.at(3) = mParameters.p1(); - parameterVec.at(4) = mParameters.p2(); - parameterVec.at(5) = mParameters.gamma1(); - parameterVec.at(6) = mParameters.gamma2(); - parameterVec.at(7) = mParameters.u0(); - parameterVec.at(8) = mParameters.v0(); +void CataCamera::writeParameters(std::vector& parameterVec) const { + parameterVec.resize(parameterCount()); + parameterVec.at(0) = mParameters.xi(); + parameterVec.at(1) = mParameters.k1(); + parameterVec.at(2) = mParameters.k2(); + parameterVec.at(3) = mParameters.p1(); + parameterVec.at(4) = mParameters.p2(); + parameterVec.at(5) = mParameters.gamma1(); + parameterVec.at(6) = mParameters.gamma2(); + parameterVec.at(7) = mParameters.u0(); + parameterVec.at(8) = mParameters.v0(); } -void -CataCamera::writeParametersToYamlFile(const std::string& filename) const -{ - mParameters.writeToYamlFile(filename); +void CataCamera::writeParametersToYamlFile(const std::string& filename) const { + mParameters.writeToYamlFile(filename); } -std::string -CataCamera::parametersToString(void) const -{ - std::ostringstream oss; - oss << mParameters; +std::string CataCamera::parametersToString(void) const { + std::ostringstream oss; + oss << mParameters; - return oss.str(); + return oss.str(); } -} +} // namespace camodocal diff --git a/camera_models/src/camera_models/CostFunctionFactory.cc b/camera_models/src/camera_models/CostFunctionFactory.cc index 2bcdb146..d141e472 100644 --- a/camera_models/src/camera_models/CostFunctionFactory.cc +++ b/camera_models/src/camera_models/CostFunctionFactory.cc @@ -7,1378 +7,1317 @@ #include "camodocal/camera_models/ScaramuzzaCamera.h" #include "ceres/ceres.h" -namespace camodocal -{ +namespace camodocal { -template< typename T > -void -worldToCameraTransform( const T* const q_cam_odo, - const T* const t_cam_odo, - const T* const p_odo, - const T* const att_odo, - T* q, - T* t, - bool optimize_cam_odo_z = true ) -{ - Eigen::Quaternion< T > q_z_inv( cos( att_odo[0] / T( 2 ) ), T( 0 ), T( 0 ), -sin( att_odo[0] / T( 2 ) ) ); - Eigen::Quaternion< T > q_y_inv( cos( att_odo[1] / T( 2 ) ), T( 0 ), -sin( att_odo[1] / T( 2 ) ), T( 0 ) ); - Eigen::Quaternion< T > q_x_inv( cos( att_odo[2] / T( 2 ) ), -sin( att_odo[2] / T( 2 ) ), T( 0 ), T( 0 ) ); +template +void worldToCameraTransform(const T* const q_cam_odo, const T* const t_cam_odo, + const T* const p_odo, const T* const att_odo, T* q, + T* t, bool optimize_cam_odo_z = true) { + Eigen::Quaternion q_z_inv(cos(att_odo[0] / T(2)), T(0), T(0), + -sin(att_odo[0] / T(2))); + Eigen::Quaternion q_y_inv(cos(att_odo[1] / T(2)), T(0), + -sin(att_odo[1] / T(2)), T(0)); + Eigen::Quaternion q_x_inv(cos(att_odo[2] / T(2)), -sin(att_odo[2] / T(2)), + T(0), T(0)); - Eigen::Quaternion< T > q_zyx_inv = q_x_inv * q_y_inv * q_z_inv; + Eigen::Quaternion q_zyx_inv = q_x_inv * q_y_inv * q_z_inv; - T q_odo[4] = { q_zyx_inv.w( ), q_zyx_inv.x( ), q_zyx_inv.y( ), q_zyx_inv.z( ) }; + T q_odo[4] = {q_zyx_inv.w(), q_zyx_inv.x(), q_zyx_inv.y(), q_zyx_inv.z()}; - T q_odo_cam[4] = { q_cam_odo[3], -q_cam_odo[0], -q_cam_odo[1], -q_cam_odo[2] }; + T q_odo_cam[4] = {q_cam_odo[3], -q_cam_odo[0], -q_cam_odo[1], -q_cam_odo[2]}; - T q0[4]; - ceres::QuaternionProduct( q_odo_cam, q_odo, q0 ); + T q0[4]; + ceres::QuaternionProduct(q_odo_cam, q_odo, q0); - T t0[3]; - T t_odo[3] = { p_odo[0], p_odo[1], p_odo[2] }; + T t0[3]; + T t_odo[3] = {p_odo[0], p_odo[1], p_odo[2]}; - ceres::QuaternionRotatePoint( q_odo, t_odo, t0 ); + ceres::QuaternionRotatePoint(q_odo, t_odo, t0); - t0[0] += t_cam_odo[0]; - t0[1] += t_cam_odo[1]; + t0[0] += t_cam_odo[0]; + t0[1] += t_cam_odo[1]; - if ( optimize_cam_odo_z ) - { - t0[2] += t_cam_odo[2]; - } + if (optimize_cam_odo_z) { + t0[2] += t_cam_odo[2]; + } + + ceres::QuaternionRotatePoint(q_odo_cam, t0, t); + t[0] = -t[0]; + t[1] = -t[1]; + t[2] = -t[2]; - ceres::QuaternionRotatePoint( q_odo_cam, t0, t ); - t[0] = -t[0]; - t[1] = -t[1]; - t[2] = -t[2]; - - // Convert quaternion from Ceres convention (w, x, y, z) - // to Eigen convention (x, y, z, w) - q[0] = q0[1]; - q[1] = q0[2]; - q[2] = q0[3]; - q[3] = q0[0]; + // Convert quaternion from Ceres convention (w, x, y, z) + // to Eigen convention (x, y, z, w) + q[0] = q0[1]; + q[1] = q0[2]; + q[2] = q0[3]; + q[3] = q0[0]; } -template< class CameraT > -class ReprojectionError1 -{ - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW +template +class ReprojectionError1 { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW - ReprojectionError1( const Eigen::Vector3d& observed_P, const Eigen::Vector2d& observed_p ) - : m_observed_P( observed_P ) - , m_observed_p( observed_p ) - , m_sqrtPrecisionMat( Eigen::Matrix2d::Identity( ) ) - { - } + ReprojectionError1(const Eigen::Vector3d& observed_P, + const Eigen::Vector2d& observed_p) + : m_observed_P(observed_P), + m_observed_p(observed_p), + m_sqrtPrecisionMat(Eigen::Matrix2d::Identity()) {} - ReprojectionError1( const Eigen::Vector3d& observed_P, - const Eigen::Vector2d& observed_p, - const Eigen::Matrix2d& sqrtPrecisionMat ) - : m_observed_P( observed_P ) - , m_observed_p( observed_p ) - , m_sqrtPrecisionMat( sqrtPrecisionMat ) - { - } + ReprojectionError1(const Eigen::Vector3d& observed_P, + const Eigen::Vector2d& observed_p, + const Eigen::Matrix2d& sqrtPrecisionMat) + : m_observed_P(observed_P), + m_observed_p(observed_p), + m_sqrtPrecisionMat(sqrtPrecisionMat) {} - ReprojectionError1( const std::vector< double >& intrinsic_params, - const Eigen::Vector3d& observed_P, - const Eigen::Vector2d& observed_p ) - : m_intrinsic_params( intrinsic_params ) - , m_observed_P( observed_P ) - , m_observed_p( observed_p ) - { - } + ReprojectionError1(const std::vector& intrinsic_params, + const Eigen::Vector3d& observed_P, + const Eigen::Vector2d& observed_p) + : m_intrinsic_params(intrinsic_params), + m_observed_P(observed_P), + m_observed_p(observed_p) {} - // variables: camera intrinsics and camera extrinsics - template< typename T > - bool operator( )( const T* const intrinsic_params, const T* const q, const T* const t, T* residuals ) const - { - Eigen::Matrix< T, 3, 1 > P = m_observed_P.cast< T >( ); + // variables: camera intrinsics and camera extrinsics + template + bool operator()(const T* const intrinsic_params, const T* const q, + const T* const t, T* residuals) const { + Eigen::Matrix P = m_observed_P.cast(); - Eigen::Matrix< T, 2, 1 > predicted_p; - CameraT::spaceToPlane( intrinsic_params, q, t, P, predicted_p ); + Eigen::Matrix predicted_p; + CameraT::spaceToPlane(intrinsic_params, q, t, P, predicted_p); - Eigen::Matrix< T, 2, 1 > e = predicted_p - m_observed_p.cast< T >( ); + Eigen::Matrix e = predicted_p - m_observed_p.cast(); - Eigen::Matrix< T, 2, 1 > e_weighted = m_sqrtPrecisionMat.cast< T >( ) * e; + Eigen::Matrix e_weighted = m_sqrtPrecisionMat.cast() * e; - residuals[0] = e_weighted( 0 ); - residuals[1] = e_weighted( 1 ); + residuals[0] = e_weighted(0); + residuals[1] = e_weighted(1); - return true; - } + return true; + } - // variables: camera-odometry transforms and odometry poses - template< typename T > - bool operator( )( const T* const q_cam_odo, - const T* const t_cam_odo, - const T* const p_odo, - const T* const att_odo, - T* residuals ) const - { - T q[4], t[3]; - worldToCameraTransform( q_cam_odo, t_cam_odo, p_odo, att_odo, q, t ); + // variables: camera-odometry transforms and odometry poses + template + bool operator()(const T* const q_cam_odo, const T* const t_cam_odo, + const T* const p_odo, const T* const att_odo, + T* residuals) const { + T q[4], t[3]; + worldToCameraTransform(q_cam_odo, t_cam_odo, p_odo, att_odo, q, t); - Eigen::Matrix< T, 3, 1 > P = m_observed_P.cast< T >( ); + Eigen::Matrix P = m_observed_P.cast(); - std::vector< T > intrinsic_params( m_intrinsic_params.begin( ), m_intrinsic_params.end( ) ); + std::vector intrinsic_params(m_intrinsic_params.begin(), + m_intrinsic_params.end()); - // project 3D object point to the image plane - Eigen::Matrix< T, 2, 1 > predicted_p; - CameraT::spaceToPlane( intrinsic_params.data( ), q, t, P, predicted_p ); + // project 3D object point to the image plane + Eigen::Matrix predicted_p; + CameraT::spaceToPlane(intrinsic_params.data(), q, t, P, predicted_p); - residuals[0] = predicted_p( 0 ) - T( m_observed_p( 0 ) ); - residuals[1] = predicted_p( 1 ) - T( m_observed_p( 1 ) ); + residuals[0] = predicted_p(0) - T(m_observed_p(0)); + residuals[1] = predicted_p(1) - T(m_observed_p(1)); - return true; - } + return true; + } - // private: - // camera intrinsics - std::vector< double > m_intrinsic_params; + // private: + // camera intrinsics + std::vector m_intrinsic_params; - // observed 3D point - Eigen::Vector3d m_observed_P; + // observed 3D point + Eigen::Vector3d m_observed_P; - // observed 2D point - Eigen::Vector2d m_observed_p; + // observed 2D point + Eigen::Vector2d m_observed_p; - // square root of precision matrix - Eigen::Matrix2d m_sqrtPrecisionMat; + // square root of precision matrix + Eigen::Matrix2d m_sqrtPrecisionMat; }; // variables: camera extrinsics, 3D point -template< class CameraT > -class ReprojectionError2 -{ - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - ReprojectionError2( const std::vector< double >& intrinsic_params, const Eigen::Vector2d& observed_p ) - : m_intrinsic_params( intrinsic_params ) - , m_observed_p( observed_p ) - { - } - - template< typename T > - bool operator( )( const T* const q, const T* const t, const T* const point, T* residuals ) const - { - Eigen::Matrix< T, 3, 1 > P; - P( 0 ) = T( point[0] ); - P( 1 ) = T( point[1] ); - P( 2 ) = T( point[2] ); - - std::vector< T > intrinsic_params( m_intrinsic_params.begin( ), m_intrinsic_params.end( ) ); - - // project 3D object point to the image plane - Eigen::Matrix< T, 2, 1 > predicted_p; - CameraT::spaceToPlane( intrinsic_params.data( ), q, t, P, predicted_p ); - - residuals[0] = predicted_p( 0 ) - T( m_observed_p( 0 ) ); - residuals[1] = predicted_p( 1 ) - T( m_observed_p( 1 ) ); - - return true; - } - - private: - // camera intrinsics - std::vector< double > m_intrinsic_params; - - // observed 2D point - Eigen::Vector2d m_observed_p; +template +class ReprojectionError2 { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + ReprojectionError2(const std::vector& intrinsic_params, + const Eigen::Vector2d& observed_p) + : m_intrinsic_params(intrinsic_params), m_observed_p(observed_p) {} + + template + bool operator()(const T* const q, const T* const t, const T* const point, + T* residuals) const { + Eigen::Matrix P; + P(0) = T(point[0]); + P(1) = T(point[1]); + P(2) = T(point[2]); + + std::vector intrinsic_params(m_intrinsic_params.begin(), + m_intrinsic_params.end()); + + // project 3D object point to the image plane + Eigen::Matrix predicted_p; + CameraT::spaceToPlane(intrinsic_params.data(), q, t, P, predicted_p); + + residuals[0] = predicted_p(0) - T(m_observed_p(0)); + residuals[1] = predicted_p(1) - T(m_observed_p(1)); + + return true; + } + + private: + // camera intrinsics + std::vector m_intrinsic_params; + + // observed 2D point + Eigen::Vector2d m_observed_p; }; -template< class CameraT > -class ReprojectionError3 -{ - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - ReprojectionError3( const Eigen::Vector2d& observed_p ) - : m_observed_p( observed_p ) - , m_sqrtPrecisionMat( Eigen::Matrix2d::Identity( ) ) - , m_optimize_cam_odo_z( true ) - { - } - - ReprojectionError3( const Eigen::Vector2d& observed_p, const Eigen::Matrix2d& sqrtPrecisionMat ) - : m_observed_p( observed_p ) - , m_sqrtPrecisionMat( sqrtPrecisionMat ) - , m_optimize_cam_odo_z( true ) - { - } - - ReprojectionError3( const std::vector< double >& intrinsic_params, const Eigen::Vector2d& observed_p ) - : m_intrinsic_params( intrinsic_params ) - , m_observed_p( observed_p ) - , m_sqrtPrecisionMat( Eigen::Matrix2d::Identity( ) ) - , m_optimize_cam_odo_z( true ) - { - } - - ReprojectionError3( const std::vector< double >& intrinsic_params, - const Eigen::Vector2d& observed_p, - const Eigen::Matrix2d& sqrtPrecisionMat ) - : m_intrinsic_params( intrinsic_params ) - , m_observed_p( observed_p ) - , m_sqrtPrecisionMat( sqrtPrecisionMat ) - , m_optimize_cam_odo_z( true ) - { - } - - ReprojectionError3( const std::vector< double >& intrinsic_params, - const Eigen::Vector3d& odo_pos, - const Eigen::Vector3d& odo_att, - const Eigen::Vector2d& observed_p, - bool optimize_cam_odo_z ) - : m_intrinsic_params( intrinsic_params ) - , m_odo_pos( odo_pos ) - , m_odo_att( odo_att ) - , m_observed_p( observed_p ) - , m_optimize_cam_odo_z( optimize_cam_odo_z ) - { - } - - ReprojectionError3( const std::vector< double >& intrinsic_params, - const Eigen::Quaterniond& cam_odo_q, - const Eigen::Vector3d& cam_odo_t, - const Eigen::Vector3d& odo_pos, - const Eigen::Vector3d& odo_att, - const Eigen::Vector2d& observed_p ) - : m_intrinsic_params( intrinsic_params ) - , m_cam_odo_q( cam_odo_q ) - , m_cam_odo_t( cam_odo_t ) - , m_odo_pos( odo_pos ) - , m_odo_att( odo_att ) - , m_observed_p( observed_p ) - , m_optimize_cam_odo_z( true ) - { - } - - // variables: camera intrinsics, camera-to-odometry transform, - // odometry extrinsics, 3D point - template< typename T > - bool operator( )( const T* const intrinsic_params, - const T* const q_cam_odo, - const T* const t_cam_odo, - const T* const p_odo, - const T* const att_odo, - const T* const point, - T* residuals ) const - { - T q[4], t[3]; - worldToCameraTransform( q_cam_odo, t_cam_odo, p_odo, att_odo, q, t, m_optimize_cam_odo_z ); - - Eigen::Matrix< T, 3, 1 > P( point[0], point[1], point[2] ); - - // project 3D object point to the image plane - Eigen::Matrix< T, 2, 1 > predicted_p; - CameraT::spaceToPlane( intrinsic_params, q, t, P, predicted_p ); +template +class ReprojectionError3 { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + ReprojectionError3(const Eigen::Vector2d& observed_p) + : m_observed_p(observed_p), + m_sqrtPrecisionMat(Eigen::Matrix2d::Identity()), + m_optimize_cam_odo_z(true) {} + + ReprojectionError3(const Eigen::Vector2d& observed_p, + const Eigen::Matrix2d& sqrtPrecisionMat) + : m_observed_p(observed_p), + m_sqrtPrecisionMat(sqrtPrecisionMat), + m_optimize_cam_odo_z(true) {} + + ReprojectionError3(const std::vector& intrinsic_params, + const Eigen::Vector2d& observed_p) + : m_intrinsic_params(intrinsic_params), + m_observed_p(observed_p), + m_sqrtPrecisionMat(Eigen::Matrix2d::Identity()), + m_optimize_cam_odo_z(true) {} + + ReprojectionError3(const std::vector& intrinsic_params, + const Eigen::Vector2d& observed_p, + const Eigen::Matrix2d& sqrtPrecisionMat) + : m_intrinsic_params(intrinsic_params), + m_observed_p(observed_p), + m_sqrtPrecisionMat(sqrtPrecisionMat), + m_optimize_cam_odo_z(true) {} + + ReprojectionError3(const std::vector& intrinsic_params, + const Eigen::Vector3d& odo_pos, + const Eigen::Vector3d& odo_att, + const Eigen::Vector2d& observed_p, bool optimize_cam_odo_z) + : m_intrinsic_params(intrinsic_params), + m_odo_pos(odo_pos), + m_odo_att(odo_att), + m_observed_p(observed_p), + m_optimize_cam_odo_z(optimize_cam_odo_z) {} + + ReprojectionError3(const std::vector& intrinsic_params, + const Eigen::Quaterniond& cam_odo_q, + const Eigen::Vector3d& cam_odo_t, + const Eigen::Vector3d& odo_pos, + const Eigen::Vector3d& odo_att, + const Eigen::Vector2d& observed_p) + : m_intrinsic_params(intrinsic_params), + m_cam_odo_q(cam_odo_q), + m_cam_odo_t(cam_odo_t), + m_odo_pos(odo_pos), + m_odo_att(odo_att), + m_observed_p(observed_p), + m_optimize_cam_odo_z(true) {} + + // variables: camera intrinsics, camera-to-odometry transform, + // odometry extrinsics, 3D point + template + bool operator()(const T* const intrinsic_params, const T* const q_cam_odo, + const T* const t_cam_odo, const T* const p_odo, + const T* const att_odo, const T* const point, + T* residuals) const { + T q[4], t[3]; + worldToCameraTransform(q_cam_odo, t_cam_odo, p_odo, att_odo, q, t, + m_optimize_cam_odo_z); + + Eigen::Matrix P(point[0], point[1], point[2]); + + // project 3D object point to the image plane + Eigen::Matrix predicted_p; + CameraT::spaceToPlane(intrinsic_params, q, t, P, predicted_p); + + Eigen::Matrix err = predicted_p - m_observed_p.cast(); + Eigen::Matrix err_weighted = m_sqrtPrecisionMat.cast() * err; + + residuals[0] = err_weighted(0); + residuals[1] = err_weighted(1); + + return true; + } + + // variables: camera-to-odometry transform, 3D point + template + bool operator()(const T* const q_cam_odo, const T* const t_cam_odo, + const T* const point, T* residuals) const { + T p_odo[3] = {T(m_odo_pos(0)), T(m_odo_pos(1)), T(m_odo_pos(2))}; + T att_odo[3] = {T(m_odo_att(0)), T(m_odo_att(1)), T(m_odo_att(2))}; + T q[4], t[3]; + + worldToCameraTransform(q_cam_odo, t_cam_odo, p_odo, att_odo, q, t, + m_optimize_cam_odo_z); + + std::vector intrinsic_params(m_intrinsic_params.begin(), + m_intrinsic_params.end()); + Eigen::Matrix P(point[0], point[1], point[2]); + + // project 3D object point to the image plane + Eigen::Matrix predicted_p; + CameraT::spaceToPlane(intrinsic_params.data(), q, t, P, predicted_p); + + residuals[0] = predicted_p(0) - T(m_observed_p(0)); + residuals[1] = predicted_p(1) - T(m_observed_p(1)); + + return true; + } + + // variables: camera-to-odometry transform, odometry extrinsics, 3D point + template + bool operator()(const T* const q_cam_odo, const T* const t_cam_odo, + const T* const p_odo, const T* const att_odo, + const T* const point, T* residuals) const { + T q[4], t[3]; + worldToCameraTransform(q_cam_odo, t_cam_odo, p_odo, att_odo, q, t, + m_optimize_cam_odo_z); + + std::vector intrinsic_params(m_intrinsic_params.begin(), + m_intrinsic_params.end()); + Eigen::Matrix P(point[0], point[1], point[2]); + + // project 3D object point to the image plane + Eigen::Matrix predicted_p; + CameraT::spaceToPlane(intrinsic_params.data(), q, t, P, predicted_p); + + Eigen::Matrix err = predicted_p - m_observed_p.cast(); + Eigen::Matrix err_weighted = m_sqrtPrecisionMat.cast() * err; + + residuals[0] = err_weighted(0); + residuals[1] = err_weighted(1); + + return true; + } + + // variables: 3D point + template + bool operator()(const T* const point, T* residuals) const { + T q_cam_odo[4] = {T(m_cam_odo_q.coeffs()(0)), T(m_cam_odo_q.coeffs()(1)), + T(m_cam_odo_q.coeffs()(2)), T(m_cam_odo_q.coeffs()(3))}; + T t_cam_odo[3] = {T(m_cam_odo_t(0)), T(m_cam_odo_t(1)), T(m_cam_odo_t(2))}; + T p_odo[3] = {T(m_odo_pos(0)), T(m_odo_pos(1)), T(m_odo_pos(2))}; + T att_odo[3] = {T(m_odo_att(0)), T(m_odo_att(1)), T(m_odo_att(2))}; + T q[4], t[3]; + + worldToCameraTransform(q_cam_odo, t_cam_odo, p_odo, att_odo, q, t, + m_optimize_cam_odo_z); + + std::vector intrinsic_params(m_intrinsic_params.begin(), + m_intrinsic_params.end()); + Eigen::Matrix P(point[0], point[1], point[2]); + + // project 3D object point to the image plane + Eigen::Matrix predicted_p; + CameraT::spaceToPlane(intrinsic_params.data(), q, t, P, predicted_p); + + residuals[0] = predicted_p(0) - T(m_observed_p(0)); + residuals[1] = predicted_p(1) - T(m_observed_p(1)); + + return true; + } - Eigen::Matrix< T, 2, 1 > err = predicted_p - m_observed_p.cast< T >( ); - Eigen::Matrix< T, 2, 1 > err_weighted = m_sqrtPrecisionMat.cast< T >( ) * err; + private: + // camera intrinsics + std::vector m_intrinsic_params; - residuals[0] = err_weighted( 0 ); - residuals[1] = err_weighted( 1 ); + // observed camera-odometry transform + Eigen::Quaterniond m_cam_odo_q; + Eigen::Vector3d m_cam_odo_t; - return true; - } - - // variables: camera-to-odometry transform, 3D point - template< typename T > - bool operator( )( const T* const q_cam_odo, const T* const t_cam_odo, const T* const point, T* residuals ) const - { - T p_odo[3] = { T( m_odo_pos( 0 ) ), T( m_odo_pos( 1 ) ), T( m_odo_pos( 2 ) ) }; - T att_odo[3] = { T( m_odo_att( 0 ) ), T( m_odo_att( 1 ) ), T( m_odo_att( 2 ) ) }; - T q[4], t[3]; - - worldToCameraTransform( q_cam_odo, t_cam_odo, p_odo, att_odo, q, t, m_optimize_cam_odo_z ); - - std::vector< T > intrinsic_params( m_intrinsic_params.begin( ), m_intrinsic_params.end( ) ); - Eigen::Matrix< T, 3, 1 > P( point[0], point[1], point[2] ); - - // project 3D object point to the image plane - Eigen::Matrix< T, 2, 1 > predicted_p; - CameraT::spaceToPlane( intrinsic_params.data( ), q, t, P, predicted_p ); - - residuals[0] = predicted_p( 0 ) - T( m_observed_p( 0 ) ); - residuals[1] = predicted_p( 1 ) - T( m_observed_p( 1 ) ); - - return true; - } - - // variables: camera-to-odometry transform, odometry extrinsics, 3D point - template< typename T > - bool operator( )( const T* const q_cam_odo, - const T* const t_cam_odo, - const T* const p_odo, - const T* const att_odo, - const T* const point, - T* residuals ) const - { - T q[4], t[3]; - worldToCameraTransform( q_cam_odo, t_cam_odo, p_odo, att_odo, q, t, m_optimize_cam_odo_z ); - - std::vector< T > intrinsic_params( m_intrinsic_params.begin( ), m_intrinsic_params.end( ) ); - Eigen::Matrix< T, 3, 1 > P( point[0], point[1], point[2] ); - - // project 3D object point to the image plane - Eigen::Matrix< T, 2, 1 > predicted_p; - CameraT::spaceToPlane( intrinsic_params.data( ), q, t, P, predicted_p ); - - Eigen::Matrix< T, 2, 1 > err = predicted_p - m_observed_p.cast< T >( ); - Eigen::Matrix< T, 2, 1 > err_weighted = m_sqrtPrecisionMat.cast< T >( ) * err; - - residuals[0] = err_weighted( 0 ); - residuals[1] = err_weighted( 1 ); - - return true; - } - - // variables: 3D point - template< typename T > - bool operator( )( const T* const point, T* residuals ) const - { - T q_cam_odo[4] = { T( m_cam_odo_q.coeffs( )( 0 ) ), - T( m_cam_odo_q.coeffs( )( 1 ) ), - T( m_cam_odo_q.coeffs( )( 2 ) ), - T( m_cam_odo_q.coeffs( )( 3 ) ) }; - T t_cam_odo[3] = { T( m_cam_odo_t( 0 ) ), T( m_cam_odo_t( 1 ) ), T( m_cam_odo_t( 2 ) ) }; - T p_odo[3] = { T( m_odo_pos( 0 ) ), T( m_odo_pos( 1 ) ), T( m_odo_pos( 2 ) ) }; - T att_odo[3] = { T( m_odo_att( 0 ) ), T( m_odo_att( 1 ) ), T( m_odo_att( 2 ) ) }; - T q[4], t[3]; - - worldToCameraTransform( q_cam_odo, t_cam_odo, p_odo, att_odo, q, t, m_optimize_cam_odo_z ); - - std::vector< T > intrinsic_params( m_intrinsic_params.begin( ), m_intrinsic_params.end( ) ); - Eigen::Matrix< T, 3, 1 > P( point[0], point[1], point[2] ); - - // project 3D object point to the image plane - Eigen::Matrix< T, 2, 1 > predicted_p; - CameraT::spaceToPlane( intrinsic_params.data( ), q, t, P, predicted_p ); - - residuals[0] = predicted_p( 0 ) - T( m_observed_p( 0 ) ); - residuals[1] = predicted_p( 1 ) - T( m_observed_p( 1 ) ); + // observed odometry + Eigen::Vector3d m_odo_pos; + Eigen::Vector3d m_odo_att; - return true; - } - - private: - // camera intrinsics - std::vector< double > m_intrinsic_params; - - // observed camera-odometry transform - Eigen::Quaterniond m_cam_odo_q; - Eigen::Vector3d m_cam_odo_t; + // observed 2D point + Eigen::Vector2d m_observed_p; - // observed odometry - Eigen::Vector3d m_odo_pos; - Eigen::Vector3d m_odo_att; + Eigen::Matrix2d m_sqrtPrecisionMat; - // observed 2D point - Eigen::Vector2d m_observed_p; - - Eigen::Matrix2d m_sqrtPrecisionMat; - - bool m_optimize_cam_odo_z; + bool m_optimize_cam_odo_z; }; // variables: camera intrinsics and camera extrinsics -template< class CameraT > -class StereoReprojectionError -{ - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - StereoReprojectionError( const Eigen::Vector3d& observed_P, - const Eigen::Vector2d& observed_p_l, - const Eigen::Vector2d& observed_p_r ) - : m_observed_P( observed_P ) - , m_observed_p_l( observed_p_l ) - , m_observed_p_r( observed_p_r ) - { - } - - template< typename T > - bool operator( )( const T* const intrinsic_params_l, - const T* const intrinsic_params_r, - const T* const q_l, - const T* const t_l, - const T* const q_l_r, - const T* const t_l_r, - T* residuals ) const - { - Eigen::Matrix< T, 3, 1 > P; - P( 0 ) = T( m_observed_P( 0 ) ); - P( 1 ) = T( m_observed_P( 1 ) ); - P( 2 ) = T( m_observed_P( 2 ) ); - - Eigen::Matrix< T, 2, 1 > predicted_p_l; - CameraT::spaceToPlane( intrinsic_params_l, q_l, t_l, P, predicted_p_l ); - - Eigen::Quaternion< T > q_r = Eigen::Quaternion< T >( q_l_r ) * Eigen::Quaternion< T >( q_l ); - - Eigen::Matrix< T, 3, 1 > t_r; - t_r( 0 ) = t_l[0]; - t_r( 1 ) = t_l[1]; - t_r( 2 ) = t_l[2]; - - t_r = Eigen::Quaternion< T >( q_l_r ) * t_r; - t_r( 0 ) += t_l_r[0]; - t_r( 1 ) += t_l_r[1]; - t_r( 2 ) += t_l_r[2]; - - Eigen::Matrix< T, 2, 1 > predicted_p_r; - CameraT::spaceToPlane( intrinsic_params_r, q_r.coeffs( ).data( ), t_r.data( ), P, predicted_p_r ); - - residuals[0] = predicted_p_l( 0 ) - T( m_observed_p_l( 0 ) ); - residuals[1] = predicted_p_l( 1 ) - T( m_observed_p_l( 1 ) ); - residuals[2] = predicted_p_r( 0 ) - T( m_observed_p_r( 0 ) ); - residuals[3] = predicted_p_r( 1 ) - T( m_observed_p_r( 1 ) ); - - return true; - } - - private: - // observed 3D point - Eigen::Vector3d m_observed_P; - - // observed 2D point - Eigen::Vector2d m_observed_p_l; - Eigen::Vector2d m_observed_p_r; +template +class StereoReprojectionError { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + StereoReprojectionError(const Eigen::Vector3d& observed_P, + const Eigen::Vector2d& observed_p_l, + const Eigen::Vector2d& observed_p_r) + : m_observed_P(observed_P), + m_observed_p_l(observed_p_l), + m_observed_p_r(observed_p_r) {} + + template + bool operator()(const T* const intrinsic_params_l, + const T* const intrinsic_params_r, const T* const q_l, + const T* const t_l, const T* const q_l_r, + const T* const t_l_r, T* residuals) const { + Eigen::Matrix P; + P(0) = T(m_observed_P(0)); + P(1) = T(m_observed_P(1)); + P(2) = T(m_observed_P(2)); + + Eigen::Matrix predicted_p_l; + CameraT::spaceToPlane(intrinsic_params_l, q_l, t_l, P, predicted_p_l); + + Eigen::Quaternion q_r = + Eigen::Quaternion(q_l_r) * Eigen::Quaternion(q_l); + + Eigen::Matrix t_r; + t_r(0) = t_l[0]; + t_r(1) = t_l[1]; + t_r(2) = t_l[2]; + + t_r = Eigen::Quaternion(q_l_r) * t_r; + t_r(0) += t_l_r[0]; + t_r(1) += t_l_r[1]; + t_r(2) += t_l_r[2]; + + Eigen::Matrix predicted_p_r; + CameraT::spaceToPlane(intrinsic_params_r, q_r.coeffs().data(), t_r.data(), + P, predicted_p_r); + + residuals[0] = predicted_p_l(0) - T(m_observed_p_l(0)); + residuals[1] = predicted_p_l(1) - T(m_observed_p_l(1)); + residuals[2] = predicted_p_r(0) - T(m_observed_p_r(0)); + residuals[3] = predicted_p_r(1) - T(m_observed_p_r(1)); + + return true; + } + + private: + // observed 3D point + Eigen::Vector3d m_observed_P; + + // observed 2D point + Eigen::Vector2d m_observed_p_l; + Eigen::Vector2d m_observed_p_r; }; -template< class CameraT > -class ComprehensionError -{ - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW +template +class ComprehensionError { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW - ComprehensionError( const Eigen::Vector3d& observed_P, const Eigen::Vector2d& observed_p ) - : m_observed_P( observed_P ) - , m_observed_p( observed_p ) - , m_sqrtPrecisionMat( Eigen::Matrix2d::Identity( ) ) - { - } + ComprehensionError(const Eigen::Vector3d& observed_P, + const Eigen::Vector2d& observed_p) + : m_observed_P(observed_P), + m_observed_p(observed_p), + m_sqrtPrecisionMat(Eigen::Matrix2d::Identity()) {} - template< typename T > - bool operator( )( const T* const intrinsic_params, const T* const q, const T* const t, T* residuals ) const + template + bool operator()(const T* const intrinsic_params, const T* const q, + const T* const t, T* residuals) const { { - { - Eigen::Matrix< T, 2, 1 > p = m_observed_p.cast< T >( ); + Eigen::Matrix p = m_observed_p.cast(); - Eigen::Matrix< T, 3, 1 > predicted_img_P; - CameraT::LiftToSphere( intrinsic_params, p, predicted_img_P ); + Eigen::Matrix predicted_img_P; + CameraT::LiftToSphere(intrinsic_params, p, predicted_img_P); - Eigen::Matrix< T, 2, 1 > predicted_p; - CameraT::SphereToPlane( intrinsic_params, predicted_img_P, predicted_p ); + Eigen::Matrix predicted_p; + CameraT::SphereToPlane(intrinsic_params, predicted_img_P, predicted_p); - Eigen::Matrix< T, 2, 1 > e = predicted_p - m_observed_p.cast< T >( ); + Eigen::Matrix e = predicted_p - m_observed_p.cast(); - Eigen::Matrix< T, 2, 1 > e_weighted = m_sqrtPrecisionMat.cast< T >( ) * e; + Eigen::Matrix e_weighted = m_sqrtPrecisionMat.cast() * e; - residuals[0] = e_weighted( 0 ); - residuals[1] = e_weighted( 1 ); - } - - { - Eigen::Matrix< T, 3, 1 > P = m_observed_P.cast< T >( ); + residuals[0] = e_weighted(0); + residuals[1] = e_weighted(1); + } - Eigen::Matrix< T, 2, 1 > predicted_p; - CameraT::spaceToPlane( intrinsic_params, q, t, P, predicted_p ); + { + Eigen::Matrix P = m_observed_P.cast(); - Eigen::Matrix< T, 2, 1 > e = predicted_p - m_observed_p.cast< T >( ); + Eigen::Matrix predicted_p; + CameraT::spaceToPlane(intrinsic_params, q, t, P, predicted_p); - Eigen::Matrix< T, 2, 1 > e_weighted = m_sqrtPrecisionMat.cast< T >( ) * e; + Eigen::Matrix e = predicted_p - m_observed_p.cast(); - residuals[2] = e_weighted( 0 ); - residuals[3] = e_weighted( 1 ); - } + Eigen::Matrix e_weighted = m_sqrtPrecisionMat.cast() * e; - return true; + residuals[2] = e_weighted(0); + residuals[3] = e_weighted(1); } - // private: - // camera intrinsics - // std::vector m_intrinsic_params; + return true; + } + + // private: + // camera intrinsics + // std::vector m_intrinsic_params; - // observed 3D point - Eigen::Vector3d m_observed_P; + // observed 3D point + Eigen::Vector3d m_observed_P; - // observed 2D point - Eigen::Vector2d m_observed_p; + // observed 2D point + Eigen::Vector2d m_observed_p; - // square root of precision matrix - Eigen::Matrix2d m_sqrtPrecisionMat; + // square root of precision matrix + Eigen::Matrix2d m_sqrtPrecisionMat; }; -boost::shared_ptr< CostFunctionFactory > CostFunctionFactory::m_instance; +boost::shared_ptr CostFunctionFactory::m_instance; -CostFunctionFactory::CostFunctionFactory( ) {} +CostFunctionFactory::CostFunctionFactory() {} -boost::shared_ptr< CostFunctionFactory > -CostFunctionFactory::instance( void ) -{ - if ( m_instance.get( ) == 0 ) - { - m_instance.reset( new CostFunctionFactory ); - } +boost::shared_ptr CostFunctionFactory::instance(void) { + if (m_instance.get() == 0) { + m_instance.reset(new CostFunctionFactory); + } - return m_instance; + return m_instance; } -ceres::CostFunction* -CostFunctionFactory::generateCostFunction( const CameraConstPtr& camera, - const Eigen::Vector3d& observed_P, - const Eigen::Vector2d& observed_p, - int flags ) const -{ - ceres::CostFunction* costFunction = 0; - - std::vector< double > intrinsic_params; - camera->writeParameters( intrinsic_params ); +ceres::CostFunction* CostFunctionFactory::generateCostFunction( + const CameraConstPtr& camera, const Eigen::Vector3d& observed_P, + const Eigen::Vector2d& observed_p, int flags) const { + ceres::CostFunction* costFunction = 0; - switch ( flags ) - { - case CAMERA_INTRINSICS | CAMERA_POSE: - switch ( camera->modelType( ) ) - { - case Camera::KANNALA_BRANDT: - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError1< EquidistantCamera >, 2, 8, 4, 3 >( - new ReprojectionError1< EquidistantCamera >( observed_P, observed_p ) ); - break; - case Camera::PINHOLE: - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError1< PinholeCamera >, 2, 8, 4, 3 >( - new ReprojectionError1< PinholeCamera >( observed_P, observed_p ) ); - break; - case Camera::PINHOLE_FULL: - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError1< PinholeFullCamera >, 2, 12, 4, 3 >( - new ReprojectionError1< PinholeFullCamera >( observed_P, observed_p ) ); - break; - case Camera::MEI: - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError1< CataCamera >, 2, 9, 4, 3 >( - new ReprojectionError1< CataCamera >( observed_P, observed_p ) ); - break; - case Camera::SCARAMUZZA: - costFunction - = new ceres::AutoDiffCostFunction< ComprehensionError< OCAMCamera >, 4, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3 >( - new ComprehensionError< OCAMCamera >( observed_P, observed_p ) ); - // new ceres::AutoDiffCostFunction, 2, - // SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3>( new - // ReprojectionError1(observed_P, observed_p)); - break; - } - break; - case CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE: - switch ( camera->modelType( ) ) - { - case Camera::KANNALA_BRANDT: - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError1< EquidistantCamera >, 2, 4, 3, 3, 3 >( - new ReprojectionError1< EquidistantCamera >( intrinsic_params, observed_P, observed_p ) ); - break; - case Camera::PINHOLE: - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError1< PinholeCamera >, 2, 4, 3, 3, 3 >( - new ReprojectionError1< PinholeCamera >( intrinsic_params, observed_P, observed_p ) ); - break; - case Camera::PINHOLE_FULL: - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError1< PinholeFullCamera >, 2, 4, 3, 3, 3 >( - new ReprojectionError1< PinholeFullCamera >( intrinsic_params, observed_P, observed_p ) ); - break; - case Camera::MEI: - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError1< CataCamera >, 2, 4, 3, 3, 3 >( - new ReprojectionError1< CataCamera >( intrinsic_params, observed_P, observed_p ) ); - break; - case Camera::SCARAMUZZA: - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError1< OCAMCamera >, 2, 4, 3, 3, 3 >( - new ReprojectionError1< OCAMCamera >( intrinsic_params, observed_P, observed_p ) ); - break; - } - break; - } + std::vector intrinsic_params; + camera->writeParameters(intrinsic_params); - return costFunction; + switch (flags) { + case CAMERA_INTRINSICS | CAMERA_POSE: + switch (camera->modelType()) { + case Camera::KANNALA_BRANDT: + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError1, 2, 8, 4, 3>( + new ReprojectionError1(observed_P, + observed_p)); + break; + case Camera::PINHOLE: + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError1, 2, 8, 4, 3>( + new ReprojectionError1(observed_P, observed_p)); + break; + case Camera::PINHOLE_FULL: + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError1, 2, 12, 4, 3>( + new ReprojectionError1(observed_P, + observed_p)); + break; + case Camera::MEI: + costFunction = + new ceres::AutoDiffCostFunction, 2, + 9, 4, 3>( + new ReprojectionError1(observed_P, observed_p)); + break; + case Camera::SCARAMUZZA: + costFunction = + new ceres::AutoDiffCostFunction, 4, + SCARAMUZZA_CAMERA_NUM_PARAMS, 4, + 3>( + new ComprehensionError(observed_P, observed_p)); + // new ceres::AutoDiffCostFunction, 2, + // SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3>( new + // ReprojectionError1(observed_P, observed_p)); + break; + } + break; + case CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE: + switch (camera->modelType()) { + case Camera::KANNALA_BRANDT: + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError1, 2, 4, 3, 3, 3>( + new ReprojectionError1( + intrinsic_params, observed_P, observed_p)); + break; + case Camera::PINHOLE: + costFunction = + new ceres::AutoDiffCostFunction, + 2, 4, 3, 3, 3>( + new ReprojectionError1( + intrinsic_params, observed_P, observed_p)); + break; + case Camera::PINHOLE_FULL: + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError1, 2, 4, 3, 3, 3>( + new ReprojectionError1( + intrinsic_params, observed_P, observed_p)); + break; + case Camera::MEI: + costFunction = + new ceres::AutoDiffCostFunction, 2, + 4, 3, 3, 3>( + new ReprojectionError1(intrinsic_params, + observed_P, observed_p)); + break; + case Camera::SCARAMUZZA: + costFunction = + new ceres::AutoDiffCostFunction, 2, + 4, 3, 3, 3>( + new ReprojectionError1(intrinsic_params, + observed_P, observed_p)); + break; + } + break; + } + + return costFunction; } -ceres::CostFunction* -CostFunctionFactory::generateCostFunction( const CameraConstPtr& camera, - const Eigen::Vector3d& observed_P, - const Eigen::Vector2d& observed_p, - const Eigen::Matrix2d& sqrtPrecisionMat, - int flags ) const -{ - ceres::CostFunction* costFunction = 0; +ceres::CostFunction* CostFunctionFactory::generateCostFunction( + const CameraConstPtr& camera, const Eigen::Vector3d& observed_P, + const Eigen::Vector2d& observed_p, const Eigen::Matrix2d& sqrtPrecisionMat, + int flags) const { + ceres::CostFunction* costFunction = 0; - std::vector< double > intrinsic_params; - camera->writeParameters( intrinsic_params ); + std::vector intrinsic_params; + camera->writeParameters(intrinsic_params); - switch ( flags ) - { - case CAMERA_INTRINSICS | CAMERA_POSE: - switch ( camera->modelType( ) ) - { - case Camera::KANNALA_BRANDT: - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError1< EquidistantCamera >, 2, 8, 4, 3 >( - new ReprojectionError1< EquidistantCamera >( observed_P, observed_p, sqrtPrecisionMat ) ); - break; - case Camera::PINHOLE: - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError1< PinholeCamera >, 2, 8, 4, 3 >( - new ReprojectionError1< PinholeCamera >( observed_P, observed_p, sqrtPrecisionMat ) ); - break; - case Camera::PINHOLE_FULL: - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError1< PinholeFullCamera >, 2, 12, 4, 3 >( - new ReprojectionError1< PinholeFullCamera >( observed_P, observed_p, sqrtPrecisionMat ) ); - break; - case Camera::MEI: - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError1< CataCamera >, 2, 9, 4, 3 >( - new ReprojectionError1< CataCamera >( observed_P, observed_p, sqrtPrecisionMat ) ); - break; - case Camera::SCARAMUZZA: - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError1< OCAMCamera >, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3 >( - new ReprojectionError1< OCAMCamera >( observed_P, observed_p, sqrtPrecisionMat ) ); - break; - } - break; - } - - return costFunction; + switch (flags) { + case CAMERA_INTRINSICS | CAMERA_POSE: + switch (camera->modelType()) { + case Camera::KANNALA_BRANDT: + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError1, 2, 8, 4, 3>( + new ReprojectionError1(observed_P, observed_p, + sqrtPrecisionMat)); + break; + case Camera::PINHOLE: + costFunction = + new ceres::AutoDiffCostFunction, + 2, 8, 4, 3>( + new ReprojectionError1(observed_P, observed_p, + sqrtPrecisionMat)); + break; + case Camera::PINHOLE_FULL: + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError1, 2, 12, 4, 3>( + new ReprojectionError1(observed_P, observed_p, + sqrtPrecisionMat)); + break; + case Camera::MEI: + costFunction = + new ceres::AutoDiffCostFunction, 2, + 9, 4, 3>( + new ReprojectionError1(observed_P, observed_p, + sqrtPrecisionMat)); + break; + case Camera::SCARAMUZZA: + costFunction = + new ceres::AutoDiffCostFunction, 2, + SCARAMUZZA_CAMERA_NUM_PARAMS, 4, + 3>( + new ReprojectionError1(observed_P, observed_p, + sqrtPrecisionMat)); + break; + } + break; + } + + return costFunction; } -ceres::CostFunction* -CostFunctionFactory::generateCostFunction( const CameraConstPtr& camera, - const Eigen::Vector2d& observed_p, - int flags, - bool optimize_cam_odo_z ) const -{ - ceres::CostFunction* costFunction = 0; - - std::vector< double > intrinsic_params; - camera->writeParameters( intrinsic_params ); +ceres::CostFunction* CostFunctionFactory::generateCostFunction( + const CameraConstPtr& camera, const Eigen::Vector2d& observed_p, int flags, + bool optimize_cam_odo_z) const { + ceres::CostFunction* costFunction = 0; - switch ( flags ) - { - case CAMERA_POSE | POINT_3D: - switch ( camera->modelType( ) ) - { - case Camera::KANNALA_BRANDT: - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError2< EquidistantCamera >, 2, 4, 3, 3 >( - new ReprojectionError2< EquidistantCamera >( intrinsic_params, observed_p ) ); - break; - case Camera::PINHOLE: - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError2< PinholeCamera >, 2, 4, 3, 3 >( - new ReprojectionError2< PinholeCamera >( intrinsic_params, observed_p ) ); - break; - case Camera::PINHOLE_FULL: - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError2< PinholeFullCamera >, 2, 4, 3, 3 >( - new ReprojectionError2< PinholeFullCamera >( intrinsic_params, observed_p ) ); - break; - case Camera::MEI: - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError2< CataCamera >, 2, 4, 3, 3 >( - new ReprojectionError2< CataCamera >( intrinsic_params, observed_p ) ); - break; - case Camera::SCARAMUZZA: - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError2< OCAMCamera >, 2, 4, 3, 3 >( - new ReprojectionError2< OCAMCamera >( intrinsic_params, observed_p ) ); - break; - } - break; - case CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_3D_POSE | POINT_3D: - switch ( camera->modelType( ) ) - { - case Camera::KANNALA_BRANDT: - if ( optimize_cam_odo_z ) - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< EquidistantCamera >, 2, 4, 3, 2, 1, 3 >( - new ReprojectionError3< EquidistantCamera >( intrinsic_params, observed_p ) ); - } - else - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeCamera >, 2, 4, 2, 2, 1, 3 >( - new ReprojectionError3< PinholeCamera >( intrinsic_params, observed_p ) ); - } - break; - case Camera::PINHOLE: - if ( optimize_cam_odo_z ) - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeCamera >, 2, 4, 3, 2, 1, 3 >( - new ReprojectionError3< PinholeCamera >( intrinsic_params, observed_p ) ); - } - else - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeCamera >, 2, 4, 2, 2, 1, 3 >( - new ReprojectionError3< PinholeCamera >( intrinsic_params, observed_p ) ); - } - break; - case Camera::PINHOLE_FULL: - if ( optimize_cam_odo_z ) - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeFullCamera >, 2, 4, 3, 2, 1, 3 >( - new ReprojectionError3< PinholeFullCamera >( intrinsic_params, observed_p ) ); - } - else - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeFullCamera >, 2, 4, 2, 2, 1, 3 >( - new ReprojectionError3< PinholeFullCamera >( intrinsic_params, observed_p ) ); - } - break; - case Camera::MEI: - if ( optimize_cam_odo_z ) - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< CataCamera >, 2, 4, 3, 2, 1, 3 >( - new ReprojectionError3< CataCamera >( intrinsic_params, observed_p ) ); - } - else - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< CataCamera >, 2, 4, 2, 2, 1, 3 >( - new ReprojectionError3< CataCamera >( intrinsic_params, observed_p ) ); - } - break; - case Camera::SCARAMUZZA: - if ( optimize_cam_odo_z ) - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< OCAMCamera >, 2, 4, 3, 2, 1, 3 >( - new ReprojectionError3< OCAMCamera >( intrinsic_params, observed_p ) ); - } - else - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< OCAMCamera >, 2, 4, 2, 2, 1, 3 >( - new ReprojectionError3< OCAMCamera >( intrinsic_params, observed_p ) ); - } - break; - } - break; - case CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE | POINT_3D: - switch ( camera->modelType( ) ) - { - case Camera::KANNALA_BRANDT: - if ( optimize_cam_odo_z ) - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< EquidistantCamera >, 2, 4, 3, 3, 3, 3 >( - new ReprojectionError3< EquidistantCamera >( intrinsic_params, observed_p ) ); - } - else - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeCamera >, 2, 4, 2, 3, 3, 3 >( - new ReprojectionError3< PinholeCamera >( intrinsic_params, observed_p ) ); - } - break; - case Camera::PINHOLE: - if ( optimize_cam_odo_z ) - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeCamera >, 2, 4, 3, 3, 3, 3 >( - new ReprojectionError3< PinholeCamera >( intrinsic_params, observed_p ) ); - } - else - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeCamera >, 2, 4, 2, 3, 3, 3 >( - new ReprojectionError3< PinholeCamera >( intrinsic_params, observed_p ) ); - } - break; - case Camera::PINHOLE_FULL: - if ( optimize_cam_odo_z ) - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeFullCamera >, 2, 4, 3, 3, 3, 3 >( - new ReprojectionError3< PinholeFullCamera >( intrinsic_params, observed_p ) ); - } - else - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeFullCamera >, 2, 4, 2, 3, 3, 3 >( - new ReprojectionError3< PinholeFullCamera >( intrinsic_params, observed_p ) ); - } - break; - case Camera::MEI: - if ( optimize_cam_odo_z ) - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< CataCamera >, 2, 4, 3, 3, 3, 3 >( - new ReprojectionError3< CataCamera >( intrinsic_params, observed_p ) ); - } - else - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< CataCamera >, 2, 4, 2, 3, 3, 3 >( - new ReprojectionError3< CataCamera >( intrinsic_params, observed_p ) ); - } - break; - case Camera::SCARAMUZZA: - if ( optimize_cam_odo_z ) - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< OCAMCamera >, 2, 4, 3, 3, 3, 3 >( - new ReprojectionError3< OCAMCamera >( intrinsic_params, observed_p ) ); - } - else - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< OCAMCamera >, 2, 4, 2, 3, 3, 3 >( - new ReprojectionError3< OCAMCamera >( intrinsic_params, observed_p ) ); - } - break; - } - break; - case CAMERA_INTRINSICS | CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_3D_POSE | POINT_3D: - switch ( camera->modelType( ) ) - { - case Camera::KANNALA_BRANDT: - if ( optimize_cam_odo_z ) - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< EquidistantCamera >, 2, 8, 4, 3, 2, 1, 3 >( - new ReprojectionError3< EquidistantCamera >( observed_p ) ); - } - else - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeCamera >, 2, 8, 4, 2, 2, 1, 3 >( - new ReprojectionError3< PinholeCamera >( observed_p ) ); - } - break; - case Camera::PINHOLE: - if ( optimize_cam_odo_z ) - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeCamera >, 2, 8, 4, 3, 2, 1, 3 >( - new ReprojectionError3< PinholeCamera >( observed_p ) ); - } - else - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeCamera >, 2, 8, 4, 2, 2, 1, 3 >( - new ReprojectionError3< PinholeCamera >( observed_p ) ); - } - break; - case Camera::PINHOLE_FULL: - if ( optimize_cam_odo_z ) - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeFullCamera >, 2, 12, 4, 3, 2, 1, 3 >( - new ReprojectionError3< PinholeFullCamera >( observed_p ) ); - } - else - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeFullCamera >, 2, 12, 4, 2, 2, 1, 3 >( - new ReprojectionError3< PinholeFullCamera >( observed_p ) ); - } - break; - case Camera::MEI: - if ( optimize_cam_odo_z ) - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< CataCamera >, 2, 9, 4, 3, 2, 1, 3 >( - new ReprojectionError3< CataCamera >( observed_p ) ); - } - else - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< CataCamera >, 2, 9, 4, 2, 2, 1, 3 >( - new ReprojectionError3< CataCamera >( observed_p ) ); - } - break; - case Camera::SCARAMUZZA: - if ( optimize_cam_odo_z ) - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< OCAMCamera >, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3, 2, 1, 3 >( - new ReprojectionError3< OCAMCamera >( observed_p ) ); - } - else - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< OCAMCamera >, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 2, 2, 1, 3 >( - new ReprojectionError3< OCAMCamera >( observed_p ) ); - } - break; - } - break; - case CAMERA_INTRINSICS | CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE | POINT_3D: - switch ( camera->modelType( ) ) - { - case Camera::KANNALA_BRANDT: - if ( optimize_cam_odo_z ) - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< EquidistantCamera >, 2, 8, 4, 3, 3, 3, 3 >( - new ReprojectionError3< EquidistantCamera >( observed_p ) ); - } - else - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeCamera >, 2, 8, 4, 2, 3, 3, 3 >( - new ReprojectionError3< PinholeCamera >( observed_p ) ); - } - break; - case Camera::PINHOLE: - if ( optimize_cam_odo_z ) - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeCamera >, 2, 8, 4, 3, 3, 3, 3 >( - new ReprojectionError3< PinholeCamera >( observed_p ) ); - } - else - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeCamera >, 2, 8, 4, 2, 3, 3, 3 >( - new ReprojectionError3< PinholeCamera >( observed_p ) ); - } - break; - case Camera::PINHOLE_FULL: - if ( optimize_cam_odo_z ) - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeFullCamera >, 2, 12, 4, 3, 3, 3, 3 >( - new ReprojectionError3< PinholeFullCamera >( observed_p ) ); - } - else - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeFullCamera >, 2, 12, 4, 2, 3, 3, 3 >( - new ReprojectionError3< PinholeFullCamera >( observed_p ) ); - } - break; - case Camera::MEI: - if ( optimize_cam_odo_z ) - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< CataCamera >, 2, 9, 4, 3, 3, 3, 3 >( - new ReprojectionError3< CataCamera >( observed_p ) ); - } - else - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< CataCamera >, 2, 9, 4, 2, 3, 3, 3 >( - new ReprojectionError3< CataCamera >( observed_p ) ); - } - break; - case Camera::SCARAMUZZA: - if ( optimize_cam_odo_z ) - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< OCAMCamera >, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3, 3, 3, 3 >( - new ReprojectionError3< OCAMCamera >( observed_p ) ); - } - else - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< OCAMCamera >, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 2, 3, 3, 3 >( - new ReprojectionError3< OCAMCamera >( observed_p ) ); - } - break; - } - break; - } + std::vector intrinsic_params; + camera->writeParameters(intrinsic_params); - return costFunction; + switch (flags) { + case CAMERA_POSE | POINT_3D: + switch (camera->modelType()) { + case Camera::KANNALA_BRANDT: + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError2, 2, 4, 3, 3>( + new ReprojectionError2(intrinsic_params, + observed_p)); + break; + case Camera::PINHOLE: + costFunction = + new ceres::AutoDiffCostFunction, + 2, 4, 3, 3>( + new ReprojectionError2(intrinsic_params, + observed_p)); + break; + case Camera::PINHOLE_FULL: + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError2, 2, 4, 3, 3>( + new ReprojectionError2(intrinsic_params, + observed_p)); + break; + case Camera::MEI: + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError2, 2, 4, 3, 3>( + new ReprojectionError2(intrinsic_params, observed_p)); + break; + case Camera::SCARAMUZZA: + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError2, 2, 4, 3, 3>( + new ReprojectionError2(intrinsic_params, observed_p)); + break; + } + break; + case CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_3D_POSE | POINT_3D: + switch (camera->modelType()) { + case Camera::KANNALA_BRANDT: + if (optimize_cam_odo_z) { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 4, 3, 2, 1, 3>( + new ReprojectionError3(intrinsic_params, + observed_p)); + } else { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 4, 2, 2, 1, 3>( + new ReprojectionError3(intrinsic_params, + observed_p)); + } + break; + case Camera::PINHOLE: + if (optimize_cam_odo_z) { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 4, 3, 2, 1, 3>( + new ReprojectionError3(intrinsic_params, + observed_p)); + } else { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 4, 2, 2, 1, 3>( + new ReprojectionError3(intrinsic_params, + observed_p)); + } + break; + case Camera::PINHOLE_FULL: + if (optimize_cam_odo_z) { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 4, 3, 2, 1, 3>( + new ReprojectionError3(intrinsic_params, + observed_p)); + } else { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 4, 2, 2, 1, 3>( + new ReprojectionError3(intrinsic_params, + observed_p)); + } + break; + case Camera::MEI: + if (optimize_cam_odo_z) { + costFunction = + new ceres::AutoDiffCostFunction, + 2, 4, 3, 2, 1, 3>( + new ReprojectionError3(intrinsic_params, + observed_p)); + } else { + costFunction = + new ceres::AutoDiffCostFunction, + 2, 4, 2, 2, 1, 3>( + new ReprojectionError3(intrinsic_params, + observed_p)); + } + break; + case Camera::SCARAMUZZA: + if (optimize_cam_odo_z) { + costFunction = + new ceres::AutoDiffCostFunction, + 2, 4, 3, 2, 1, 3>( + new ReprojectionError3(intrinsic_params, + observed_p)); + } else { + costFunction = + new ceres::AutoDiffCostFunction, + 2, 4, 2, 2, 1, 3>( + new ReprojectionError3(intrinsic_params, + observed_p)); + } + break; + } + break; + case CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE | POINT_3D: + switch (camera->modelType()) { + case Camera::KANNALA_BRANDT: + if (optimize_cam_odo_z) { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 4, 3, 3, 3, 3>( + new ReprojectionError3(intrinsic_params, + observed_p)); + } else { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 4, 2, 3, 3, 3>( + new ReprojectionError3(intrinsic_params, + observed_p)); + } + break; + case Camera::PINHOLE: + if (optimize_cam_odo_z) { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 4, 3, 3, 3, 3>( + new ReprojectionError3(intrinsic_params, + observed_p)); + } else { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 4, 2, 3, 3, 3>( + new ReprojectionError3(intrinsic_params, + observed_p)); + } + break; + case Camera::PINHOLE_FULL: + if (optimize_cam_odo_z) { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 4, 3, 3, 3, 3>( + new ReprojectionError3(intrinsic_params, + observed_p)); + } else { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 4, 2, 3, 3, 3>( + new ReprojectionError3(intrinsic_params, + observed_p)); + } + break; + case Camera::MEI: + if (optimize_cam_odo_z) { + costFunction = + new ceres::AutoDiffCostFunction, + 2, 4, 3, 3, 3, 3>( + new ReprojectionError3(intrinsic_params, + observed_p)); + } else { + costFunction = + new ceres::AutoDiffCostFunction, + 2, 4, 2, 3, 3, 3>( + new ReprojectionError3(intrinsic_params, + observed_p)); + } + break; + case Camera::SCARAMUZZA: + if (optimize_cam_odo_z) { + costFunction = + new ceres::AutoDiffCostFunction, + 2, 4, 3, 3, 3, 3>( + new ReprojectionError3(intrinsic_params, + observed_p)); + } else { + costFunction = + new ceres::AutoDiffCostFunction, + 2, 4, 2, 3, 3, 3>( + new ReprojectionError3(intrinsic_params, + observed_p)); + } + break; + } + break; + case CAMERA_INTRINSICS | CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_3D_POSE | + POINT_3D: + switch (camera->modelType()) { + case Camera::KANNALA_BRANDT: + if (optimize_cam_odo_z) { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 8, 4, 3, 2, 1, 3>( + new ReprojectionError3(observed_p)); + } else { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 8, 4, 2, 2, 1, 3>( + new ReprojectionError3(observed_p)); + } + break; + case Camera::PINHOLE: + if (optimize_cam_odo_z) { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 8, 4, 3, 2, 1, 3>( + new ReprojectionError3(observed_p)); + } else { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 8, 4, 2, 2, 1, 3>( + new ReprojectionError3(observed_p)); + } + break; + case Camera::PINHOLE_FULL: + if (optimize_cam_odo_z) { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 12, 4, 3, 2, 1, 3>( + new ReprojectionError3(observed_p)); + } else { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 12, 4, 2, 2, 1, 3>( + new ReprojectionError3(observed_p)); + } + break; + case Camera::MEI: + if (optimize_cam_odo_z) { + costFunction = + new ceres::AutoDiffCostFunction, + 2, 9, 4, 3, 2, 1, 3>( + new ReprojectionError3(observed_p)); + } else { + costFunction = + new ceres::AutoDiffCostFunction, + 2, 9, 4, 2, 2, 1, 3>( + new ReprojectionError3(observed_p)); + } + break; + case Camera::SCARAMUZZA: + if (optimize_cam_odo_z) { + costFunction = + new ceres::AutoDiffCostFunction, + 2, SCARAMUZZA_CAMERA_NUM_PARAMS, + 4, 3, 2, 1, 3>( + new ReprojectionError3(observed_p)); + } else { + costFunction = + new ceres::AutoDiffCostFunction, + 2, SCARAMUZZA_CAMERA_NUM_PARAMS, + 4, 2, 2, 1, 3>( + new ReprojectionError3(observed_p)); + } + break; + } + break; + case CAMERA_INTRINSICS | CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE | + POINT_3D: + switch (camera->modelType()) { + case Camera::KANNALA_BRANDT: + if (optimize_cam_odo_z) { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 8, 4, 3, 3, 3, 3>( + new ReprojectionError3(observed_p)); + } else { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 8, 4, 2, 3, 3, 3>( + new ReprojectionError3(observed_p)); + } + break; + case Camera::PINHOLE: + if (optimize_cam_odo_z) { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 8, 4, 3, 3, 3, 3>( + new ReprojectionError3(observed_p)); + } else { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 8, 4, 2, 3, 3, 3>( + new ReprojectionError3(observed_p)); + } + break; + case Camera::PINHOLE_FULL: + if (optimize_cam_odo_z) { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 12, 4, 3, 3, 3, 3>( + new ReprojectionError3(observed_p)); + } else { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 12, 4, 2, 3, 3, 3>( + new ReprojectionError3(observed_p)); + } + break; + case Camera::MEI: + if (optimize_cam_odo_z) { + costFunction = + new ceres::AutoDiffCostFunction, + 2, 9, 4, 3, 3, 3, 3>( + new ReprojectionError3(observed_p)); + } else { + costFunction = + new ceres::AutoDiffCostFunction, + 2, 9, 4, 2, 3, 3, 3>( + new ReprojectionError3(observed_p)); + } + break; + case Camera::SCARAMUZZA: + if (optimize_cam_odo_z) { + costFunction = + new ceres::AutoDiffCostFunction, + 2, SCARAMUZZA_CAMERA_NUM_PARAMS, + 4, 3, 3, 3, 3>( + new ReprojectionError3(observed_p)); + } else { + costFunction = + new ceres::AutoDiffCostFunction, + 2, SCARAMUZZA_CAMERA_NUM_PARAMS, + 4, 2, 3, 3, 3>( + new ReprojectionError3(observed_p)); + } + break; + } + break; + } + + return costFunction; } -ceres::CostFunction* -CostFunctionFactory::generateCostFunction( const CameraConstPtr& camera, - const Eigen::Vector2d& observed_p, - const Eigen::Matrix2d& sqrtPrecisionMat, - int flags, - bool optimize_cam_odo_z ) const -{ - ceres::CostFunction* costFunction = 0; +ceres::CostFunction* CostFunctionFactory::generateCostFunction( + const CameraConstPtr& camera, const Eigen::Vector2d& observed_p, + const Eigen::Matrix2d& sqrtPrecisionMat, int flags, + bool optimize_cam_odo_z) const { + ceres::CostFunction* costFunction = 0; - std::vector< double > intrinsic_params; - camera->writeParameters( intrinsic_params ); + std::vector intrinsic_params; + camera->writeParameters(intrinsic_params); - switch ( flags ) - { - case CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE | POINT_3D: - switch ( camera->modelType( ) ) - { - case Camera::KANNALA_BRANDT: - if ( optimize_cam_odo_z ) - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< EquidistantCamera >, 2, 4, 3, 3, 3, 3 >( - new ReprojectionError3< EquidistantCamera >( intrinsic_params, observed_p, sqrtPrecisionMat ) ); - } - else - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeCamera >, 2, 4, 2, 3, 3, 3 >( - new ReprojectionError3< PinholeCamera >( intrinsic_params, observed_p, sqrtPrecisionMat ) ); - } - break; - case Camera::PINHOLE: - if ( optimize_cam_odo_z ) - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeCamera >, 2, 4, 3, 3, 3, 3 >( - new ReprojectionError3< PinholeCamera >( intrinsic_params, observed_p, sqrtPrecisionMat ) ); - } - else - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeCamera >, 2, 4, 2, 3, 3, 3 >( - new ReprojectionError3< PinholeCamera >( intrinsic_params, observed_p, sqrtPrecisionMat ) ); - } - break; - case Camera::PINHOLE_FULL: - if ( optimize_cam_odo_z ) - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeFullCamera >, 2, 4, 3, 3, 3, 3 >( - new ReprojectionError3< PinholeFullCamera >( intrinsic_params, observed_p, sqrtPrecisionMat ) ); - } - else - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeFullCamera >, 2, 4, 2, 3, 3, 3 >( - new ReprojectionError3< PinholeFullCamera >( intrinsic_params, observed_p, sqrtPrecisionMat ) ); - } - break; - case Camera::MEI: - if ( optimize_cam_odo_z ) - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< CataCamera >, 2, 4, 3, 3, 3, 3 >( - new ReprojectionError3< CataCamera >( intrinsic_params, observed_p, sqrtPrecisionMat ) ); - } - else - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< CataCamera >, 2, 4, 2, 3, 3, 3 >( - new ReprojectionError3< CataCamera >( intrinsic_params, observed_p, sqrtPrecisionMat ) ); - } - break; - case Camera::SCARAMUZZA: - if ( optimize_cam_odo_z ) - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< OCAMCamera >, 2, 4, 3, 3, 3, 3 >( - new ReprojectionError3< OCAMCamera >( intrinsic_params, observed_p, sqrtPrecisionMat ) ); - } - else - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< OCAMCamera >, 2, 4, 2, 3, 3, 3 >( - new ReprojectionError3< OCAMCamera >( intrinsic_params, observed_p, sqrtPrecisionMat ) ); - } - break; - } - break; - case CAMERA_INTRINSICS | CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE | POINT_3D: - switch ( camera->modelType( ) ) - { - case Camera::KANNALA_BRANDT: - if ( optimize_cam_odo_z ) - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< EquidistantCamera >, 2, 8, 4, 3, 3, 3, 3 >( - new ReprojectionError3< EquidistantCamera >( observed_p, sqrtPrecisionMat ) ); - } - else - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeCamera >, 2, 8, 4, 2, 3, 3, 3 >( - new ReprojectionError3< PinholeCamera >( observed_p, sqrtPrecisionMat ) ); - } - break; - case Camera::PINHOLE: - if ( optimize_cam_odo_z ) - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeCamera >, 2, 8, 4, 3, 3, 3, 3 >( - new ReprojectionError3< PinholeCamera >( observed_p, sqrtPrecisionMat ) ); - } - else - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeCamera >, 2, 8, 4, 2, 3, 3, 3 >( - new ReprojectionError3< PinholeCamera >( observed_p, sqrtPrecisionMat ) ); - } - break; - case Camera::PINHOLE_FULL: - if ( optimize_cam_odo_z ) - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeFullCamera >, 2, 12, 4, 3, 3, 3, 3 >( - new ReprojectionError3< PinholeFullCamera >( observed_p, sqrtPrecisionMat ) ); - } - else - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeFullCamera >, 2, 12, 4, 2, 3, 3, 3 >( - new ReprojectionError3< PinholeFullCamera >( observed_p, sqrtPrecisionMat ) ); - } - break; - case Camera::MEI: - if ( optimize_cam_odo_z ) - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< CataCamera >, 2, 9, 4, 3, 3, 3, 3 >( - new ReprojectionError3< CataCamera >( observed_p, sqrtPrecisionMat ) ); - } - else - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< CataCamera >, 2, 9, 4, 2, 3, 3, 3 >( - new ReprojectionError3< CataCamera >( observed_p, sqrtPrecisionMat ) ); - } - break; - case Camera::SCARAMUZZA: - if ( optimize_cam_odo_z ) - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< OCAMCamera >, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3, 3, 3, 3 >( - new ReprojectionError3< OCAMCamera >( observed_p, sqrtPrecisionMat ) ); - } - else - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< OCAMCamera >, 2, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 2, 3, 3, 3 >( - new ReprojectionError3< OCAMCamera >( observed_p, sqrtPrecisionMat ) ); - } - break; - } - break; - } - - return costFunction; + switch (flags) { + case CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE | POINT_3D: + switch (camera->modelType()) { + case Camera::KANNALA_BRANDT: + if (optimize_cam_odo_z) { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 4, 3, 3, 3, 3>( + new ReprojectionError3( + intrinsic_params, observed_p, sqrtPrecisionMat)); + } else { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 4, 2, 3, 3, 3>( + new ReprojectionError3( + intrinsic_params, observed_p, sqrtPrecisionMat)); + } + break; + case Camera::PINHOLE: + if (optimize_cam_odo_z) { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 4, 3, 3, 3, 3>( + new ReprojectionError3( + intrinsic_params, observed_p, sqrtPrecisionMat)); + } else { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 4, 2, 3, 3, 3>( + new ReprojectionError3( + intrinsic_params, observed_p, sqrtPrecisionMat)); + } + break; + case Camera::PINHOLE_FULL: + if (optimize_cam_odo_z) { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 4, 3, 3, 3, 3>( + new ReprojectionError3( + intrinsic_params, observed_p, sqrtPrecisionMat)); + } else { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 4, 2, 3, 3, 3>( + new ReprojectionError3( + intrinsic_params, observed_p, sqrtPrecisionMat)); + } + break; + case Camera::MEI: + if (optimize_cam_odo_z) { + costFunction = + new ceres::AutoDiffCostFunction, + 2, 4, 3, 3, 3, 3>( + new ReprojectionError3( + intrinsic_params, observed_p, sqrtPrecisionMat)); + } else { + costFunction = + new ceres::AutoDiffCostFunction, + 2, 4, 2, 3, 3, 3>( + new ReprojectionError3( + intrinsic_params, observed_p, sqrtPrecisionMat)); + } + break; + case Camera::SCARAMUZZA: + if (optimize_cam_odo_z) { + costFunction = + new ceres::AutoDiffCostFunction, + 2, 4, 3, 3, 3, 3>( + new ReprojectionError3( + intrinsic_params, observed_p, sqrtPrecisionMat)); + } else { + costFunction = + new ceres::AutoDiffCostFunction, + 2, 4, 2, 3, 3, 3>( + new ReprojectionError3( + intrinsic_params, observed_p, sqrtPrecisionMat)); + } + break; + } + break; + case CAMERA_INTRINSICS | CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE | + POINT_3D: + switch (camera->modelType()) { + case Camera::KANNALA_BRANDT: + if (optimize_cam_odo_z) { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 8, 4, 3, 3, 3, 3>( + new ReprojectionError3(observed_p, + sqrtPrecisionMat)); + } else { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 8, 4, 2, 3, 3, 3>( + new ReprojectionError3(observed_p, + sqrtPrecisionMat)); + } + break; + case Camera::PINHOLE: + if (optimize_cam_odo_z) { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 8, 4, 3, 3, 3, 3>( + new ReprojectionError3(observed_p, + sqrtPrecisionMat)); + } else { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 8, 4, 2, 3, 3, 3>( + new ReprojectionError3(observed_p, + sqrtPrecisionMat)); + } + break; + case Camera::PINHOLE_FULL: + if (optimize_cam_odo_z) { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 12, 4, 3, 3, 3, 3>( + new ReprojectionError3(observed_p, + sqrtPrecisionMat)); + } else { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 12, 4, 2, 3, 3, 3>( + new ReprojectionError3(observed_p, + sqrtPrecisionMat)); + } + break; + case Camera::MEI: + if (optimize_cam_odo_z) { + costFunction = + new ceres::AutoDiffCostFunction, + 2, 9, 4, 3, 3, 3, 3>( + new ReprojectionError3(observed_p, + sqrtPrecisionMat)); + } else { + costFunction = + new ceres::AutoDiffCostFunction, + 2, 9, 4, 2, 3, 3, 3>( + new ReprojectionError3(observed_p, + sqrtPrecisionMat)); + } + break; + case Camera::SCARAMUZZA: + if (optimize_cam_odo_z) { + costFunction = + new ceres::AutoDiffCostFunction, + 2, SCARAMUZZA_CAMERA_NUM_PARAMS, + 4, 3, 3, 3, 3>( + new ReprojectionError3(observed_p, + sqrtPrecisionMat)); + } else { + costFunction = + new ceres::AutoDiffCostFunction, + 2, SCARAMUZZA_CAMERA_NUM_PARAMS, + 4, 2, 3, 3, 3>( + new ReprojectionError3(observed_p, + sqrtPrecisionMat)); + } + break; + } + break; + } + + return costFunction; } -ceres::CostFunction* -CostFunctionFactory::generateCostFunction( const CameraConstPtr& camera, - const Eigen::Vector3d& odo_pos, - const Eigen::Vector3d& odo_att, - const Eigen::Vector2d& observed_p, - int flags, - bool optimize_cam_odo_z ) const -{ - ceres::CostFunction* costFunction = 0; - - std::vector< double > intrinsic_params; - camera->writeParameters( intrinsic_params ); - - switch ( flags ) - { - case CAMERA_ODOMETRY_TRANSFORM | POINT_3D: - switch ( camera->modelType( ) ) - { - case Camera::KANNALA_BRANDT: - if ( optimize_cam_odo_z ) - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< EquidistantCamera >, 2, 4, 3, 3 >( - new ReprojectionError3< EquidistantCamera >( - intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z ) ); - } - else - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< EquidistantCamera >, 2, 4, 2, 3 >( - new ReprojectionError3< EquidistantCamera >( - intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z ) ); - } - break; - case Camera::PINHOLE: - if ( optimize_cam_odo_z ) - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeCamera >, 2, 4, 3, 3 >( - new ReprojectionError3< PinholeCamera >( - intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z ) ); - } - else - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeCamera >, 2, 4, 2, 3 >( - new ReprojectionError3< PinholeCamera >( - intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z ) ); - } - break; - case Camera::PINHOLE_FULL: - if ( optimize_cam_odo_z ) - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeFullCamera >, 2, 4, 3, 3 >( - new ReprojectionError3< PinholeFullCamera >( - intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z ) ); - } - else - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeFullCamera >, 2, 4, 2, 3 >( - new ReprojectionError3< PinholeFullCamera >( - intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z ) ); - } - break; - case Camera::MEI: - if ( optimize_cam_odo_z ) - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< CataCamera >, 2, 4, 3, 3 >( - new ReprojectionError3< CataCamera >( - intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z ) ); - } - else - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< CataCamera >, 2, 4, 2, 3 >( - new ReprojectionError3< CataCamera >( - intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z ) ); - } - break; - case Camera::SCARAMUZZA: - if ( optimize_cam_odo_z ) - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< OCAMCamera >, 2, 4, 3, 3 >( - new ReprojectionError3< OCAMCamera >( - intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z ) ); - } - else - { - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< OCAMCamera >, 2, 4, 2, 3 >( - new ReprojectionError3< OCAMCamera >( - intrinsic_params, odo_pos, odo_att, observed_p, optimize_cam_odo_z ) ); - } - break; - } - break; - } - - return costFunction; -} +ceres::CostFunction* CostFunctionFactory::generateCostFunction( + const CameraConstPtr& camera, const Eigen::Vector3d& odo_pos, + const Eigen::Vector3d& odo_att, const Eigen::Vector2d& observed_p, + int flags, bool optimize_cam_odo_z) const { + ceres::CostFunction* costFunction = 0; -ceres::CostFunction* -CostFunctionFactory::generateCostFunction( const CameraConstPtr& camera, - const Eigen::Quaterniond& cam_odo_q, - const Eigen::Vector3d& cam_odo_t, - const Eigen::Vector3d& odo_pos, - const Eigen::Vector3d& odo_att, - const Eigen::Vector2d& observed_p, - int flags ) const -{ - ceres::CostFunction* costFunction = 0; - - std::vector< double > intrinsic_params; - camera->writeParameters( intrinsic_params ); - - switch ( flags ) - { - case POINT_3D: - switch ( camera->modelType( ) ) - { - case Camera::KANNALA_BRANDT: - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< EquidistantCamera >, 2, 3 >( - new ReprojectionError3< EquidistantCamera >( - intrinsic_params, cam_odo_q, cam_odo_t, odo_pos, odo_att, observed_p ) ); - break; - case Camera::PINHOLE: - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeCamera >, 2, 3 >( - new ReprojectionError3< PinholeCamera >( - intrinsic_params, cam_odo_q, cam_odo_t, odo_pos, odo_att, observed_p ) ); - break; - case Camera::PINHOLE_FULL: - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< PinholeFullCamera >, 2, 3 >( - new ReprojectionError3< PinholeFullCamera >( - intrinsic_params, cam_odo_q, cam_odo_t, odo_pos, odo_att, observed_p ) ); - break; - case Camera::MEI: - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< CataCamera >, 2, 3 >( - new ReprojectionError3< CataCamera >( - intrinsic_params, cam_odo_q, cam_odo_t, odo_pos, odo_att, observed_p ) ); - break; - case Camera::SCARAMUZZA: - costFunction - = new ceres::AutoDiffCostFunction< ReprojectionError3< OCAMCamera >, 2, 3 >( - new ReprojectionError3< OCAMCamera >( - intrinsic_params, cam_odo_q, cam_odo_t, odo_pos, odo_att, observed_p ) ); - break; - } - break; - } + std::vector intrinsic_params; + camera->writeParameters(intrinsic_params); - return costFunction; + switch (flags) { + case CAMERA_ODOMETRY_TRANSFORM | POINT_3D: + switch (camera->modelType()) { + case Camera::KANNALA_BRANDT: + if (optimize_cam_odo_z) { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 4, 3, 3>( + new ReprojectionError3( + intrinsic_params, odo_pos, odo_att, observed_p, + optimize_cam_odo_z)); + } else { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 4, 2, 3>( + new ReprojectionError3( + intrinsic_params, odo_pos, odo_att, observed_p, + optimize_cam_odo_z)); + } + break; + case Camera::PINHOLE: + if (optimize_cam_odo_z) { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 4, 3, 3>( + new ReprojectionError3(intrinsic_params, odo_pos, + odo_att, observed_p, + optimize_cam_odo_z)); + } else { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 4, 2, 3>( + new ReprojectionError3(intrinsic_params, odo_pos, + odo_att, observed_p, + optimize_cam_odo_z)); + } + break; + case Camera::PINHOLE_FULL: + if (optimize_cam_odo_z) { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 4, 3, 3>( + new ReprojectionError3( + intrinsic_params, odo_pos, odo_att, observed_p, + optimize_cam_odo_z)); + } else { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 4, 2, 3>( + new ReprojectionError3( + intrinsic_params, odo_pos, odo_att, observed_p, + optimize_cam_odo_z)); + } + break; + case Camera::MEI: + if (optimize_cam_odo_z) { + costFunction = + new ceres::AutoDiffCostFunction, + 2, 4, 3, 3>( + new ReprojectionError3( + intrinsic_params, odo_pos, odo_att, observed_p, + optimize_cam_odo_z)); + } else { + costFunction = + new ceres::AutoDiffCostFunction, + 2, 4, 2, 3>( + new ReprojectionError3( + intrinsic_params, odo_pos, odo_att, observed_p, + optimize_cam_odo_z)); + } + break; + case Camera::SCARAMUZZA: + if (optimize_cam_odo_z) { + costFunction = + new ceres::AutoDiffCostFunction, + 2, 4, 3, 3>( + new ReprojectionError3( + intrinsic_params, odo_pos, odo_att, observed_p, + optimize_cam_odo_z)); + } else { + costFunction = + new ceres::AutoDiffCostFunction, + 2, 4, 2, 3>( + new ReprojectionError3( + intrinsic_params, odo_pos, odo_att, observed_p, + optimize_cam_odo_z)); + } + break; + } + break; + } + + return costFunction; } -ceres::CostFunction* -CostFunctionFactory::generateCostFunction( const CameraConstPtr& cameraL, - const CameraConstPtr& cameraR, - const Eigen::Vector3d& observed_P, - const Eigen::Vector2d& observed_p_l, - const Eigen::Vector2d& observed_p_r ) const -{ - ceres::CostFunction* costFunction = 0; +ceres::CostFunction* CostFunctionFactory::generateCostFunction( + const CameraConstPtr& camera, const Eigen::Quaterniond& cam_odo_q, + const Eigen::Vector3d& cam_odo_t, const Eigen::Vector3d& odo_pos, + const Eigen::Vector3d& odo_att, const Eigen::Vector2d& observed_p, + int flags) const { + ceres::CostFunction* costFunction = 0; - if ( cameraL->modelType( ) != cameraR->modelType( ) ) - { - return costFunction; - } + std::vector intrinsic_params; + camera->writeParameters(intrinsic_params); - switch ( cameraL->modelType( ) ) - { + switch (flags) { + case POINT_3D: + switch (camera->modelType()) { case Camera::KANNALA_BRANDT: - costFunction - = new ceres::AutoDiffCostFunction< StereoReprojectionError< EquidistantCamera >, 4, 8, 8, 4, 3, 4, 3 >( - new StereoReprojectionError< EquidistantCamera >( observed_P, observed_p_l, observed_p_r ) ); - break; + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 3>( + new ReprojectionError3( + intrinsic_params, cam_odo_q, cam_odo_t, odo_pos, odo_att, + observed_p)); + break; case Camera::PINHOLE: - costFunction - = new ceres::AutoDiffCostFunction< StereoReprojectionError< PinholeCamera >, 4, 8, 8, 4, 3, 4, 3 >( - new StereoReprojectionError< PinholeCamera >( observed_P, observed_p_l, observed_p_r ) ); - break; + costFunction = + new ceres::AutoDiffCostFunction, + 2, 3>( + new ReprojectionError3( + intrinsic_params, cam_odo_q, cam_odo_t, odo_pos, odo_att, + observed_p)); + break; case Camera::PINHOLE_FULL: - costFunction - = new ceres::AutoDiffCostFunction< StereoReprojectionError< PinholeFullCamera >, 4, 12, 12, 4, 3, 4, 3 >( - new StereoReprojectionError< PinholeFullCamera >( observed_P, observed_p_l, observed_p_r ) ); - break; + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 3>( + new ReprojectionError3( + intrinsic_params, cam_odo_q, cam_odo_t, odo_pos, odo_att, + observed_p)); + break; case Camera::MEI: - costFunction - = new ceres::AutoDiffCostFunction< StereoReprojectionError< CataCamera >, 4, 9, 9, 4, 3, 4, 3 >( - new StereoReprojectionError< CataCamera >( observed_P, observed_p_l, observed_p_r ) ); - break; + costFunction = + new ceres::AutoDiffCostFunction, 2, + 3>( + new ReprojectionError3( + intrinsic_params, cam_odo_q, cam_odo_t, odo_pos, odo_att, + observed_p)); + break; case Camera::SCARAMUZZA: - costFunction - = new ceres::AutoDiffCostFunction< StereoReprojectionError< OCAMCamera >, 4, SCARAMUZZA_CAMERA_NUM_PARAMS, SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3, 4, 3 >( - new StereoReprojectionError< OCAMCamera >( observed_P, observed_p_l, observed_p_r ) ); - break; - } + costFunction = + new ceres::AutoDiffCostFunction, 2, + 3>( + new ReprojectionError3( + intrinsic_params, cam_odo_q, cam_odo_t, odo_pos, odo_att, + observed_p)); + break; + } + break; + } + + return costFunction; +} +ceres::CostFunction* CostFunctionFactory::generateCostFunction( + const CameraConstPtr& cameraL, const CameraConstPtr& cameraR, + const Eigen::Vector3d& observed_P, const Eigen::Vector2d& observed_p_l, + const Eigen::Vector2d& observed_p_r) const { + ceres::CostFunction* costFunction = 0; + + if (cameraL->modelType() != cameraR->modelType()) { return costFunction; + } + + switch (cameraL->modelType()) { + case Camera::KANNALA_BRANDT: + costFunction = new ceres::AutoDiffCostFunction< + StereoReprojectionError, 4, 8, 8, 4, 3, 4, 3>( + new StereoReprojectionError( + observed_P, observed_p_l, observed_p_r)); + break; + case Camera::PINHOLE: + costFunction = new ceres::AutoDiffCostFunction< + StereoReprojectionError, 4, 8, 8, 4, 3, 4, 3>( + new StereoReprojectionError(observed_P, observed_p_l, + observed_p_r)); + break; + case Camera::PINHOLE_FULL: + costFunction = new ceres::AutoDiffCostFunction< + StereoReprojectionError, 4, 12, 12, 4, 3, 4, 3>( + new StereoReprojectionError( + observed_P, observed_p_l, observed_p_r)); + break; + case Camera::MEI: + costFunction = + new ceres::AutoDiffCostFunction, + 4, 9, 9, 4, 3, 4, 3>( + new StereoReprojectionError(observed_P, observed_p_l, + observed_p_r)); + break; + case Camera::SCARAMUZZA: + costFunction = new ceres::AutoDiffCostFunction< + StereoReprojectionError, 4, SCARAMUZZA_CAMERA_NUM_PARAMS, + SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3, 4, 3>( + new StereoReprojectionError(observed_P, observed_p_l, + observed_p_r)); + break; + } + + return costFunction; } -} +} // namespace camodocal diff --git a/camera_models/src/camera_models/CylindricalCamera.cc b/camera_models/src/camera_models/CylindricalCamera.cc index 230ccbe0..dbea907d 100644 --- a/camera_models/src/camera_models/CylindricalCamera.cc +++ b/camera_models/src/camera_models/CylindricalCamera.cc @@ -4,248 +4,185 @@ #include #include #include +#include #include #include #include -#include + #include "camodocal/gpl/gpl.h" -namespace camodocal -{ +namespace camodocal { CylindricalCamera::Parameters::Parameters() - : Camera::Parameters(CYLINRICALCAMERA) - , m_fx(0.0) - , m_fy(0.0) - , m_cx(0.0) - , m_cy(0.0) -{ - -} - -CylindricalCamera::Parameters::Parameters(const std::string& cameraName, - int w, int h, - double fx, double fy, - double cx, double cy) - : Camera::Parameters(CYLINRICALCAMERA, cameraName, w, h) - , m_fx(fx) - , m_fy(fy) - , m_cx(cx) - , m_cy(cy) -{ - -} - -double& -CylindricalCamera::Parameters::fx(void) -{ - return m_fx; -} - -double& -CylindricalCamera::Parameters::fy(void) -{ - return m_fy; -} - -double& -CylindricalCamera::Parameters::cx(void) -{ - return m_cx; -} - -double& -CylindricalCamera::Parameters::cy(void) -{ - return m_cy; -} - -double -CylindricalCamera::Parameters::fx(void) const -{ - return m_fx; -} + : Camera::Parameters(CYLINRICALCAMERA), + m_fx(0.0), + m_fy(0.0), + m_cx(0.0), + m_cy(0.0) {} -double -CylindricalCamera::Parameters::fy(void) const -{ - return m_fy; -} +CylindricalCamera::Parameters::Parameters(const std::string& cameraName, int w, + int h, double fx, double fy, + double cx, double cy) + : Camera::Parameters(CYLINRICALCAMERA, cameraName, w, h), + m_fx(fx), + m_fy(fy), + m_cx(cx), + m_cy(cy) {} -double -CylindricalCamera::Parameters::cx(void) const -{ - return m_cx; -} +double& CylindricalCamera::Parameters::fx(void) { return m_fx; } -double -CylindricalCamera::Parameters::cy(void) const -{ - return m_cy; -} +double& CylindricalCamera::Parameters::fy(void) { return m_fy; } -bool -CylindricalCamera::Parameters::readFromYamlFile(const std::string& filename) -{ - cv::FileStorage fs(filename, cv::FileStorage::READ); +double& CylindricalCamera::Parameters::cx(void) { return m_cx; } - if (!fs.isOpened()) - { - return false; - } +double& CylindricalCamera::Parameters::cy(void) { return m_cy; } - if (!fs["model_type"].isNone()) - { - std::string sModelType; - fs["model_type"] >> sModelType; +double CylindricalCamera::Parameters::fx(void) const { return m_fx; } - if (sModelType.compare("PINHOLE") != 0) - { - return false; - } - } +double CylindricalCamera::Parameters::fy(void) const { return m_fy; } - m_modelType = CYLINRICALCAMERA; - fs["camera_name"] >> m_cameraName; - m_imageWidth = static_cast(fs["image_width"]); - m_imageHeight = static_cast(fs["image_height"]); - auto n = fs["projection_parameters"]; - m_fx = static_cast(n["fx"]); - m_fy = static_cast(n["fy"]); - m_cx = static_cast(n["cx"]); - m_cy = static_cast(n["cy"]); - - return true; -} +double CylindricalCamera::Parameters::cx(void) const { return m_cx; } -void -CylindricalCamera::Parameters::writeToYamlFile(const std::string& filename) const -{ - cv::FileStorage fs(filename, cv::FileStorage::WRITE); +double CylindricalCamera::Parameters::cy(void) const { return m_cy; } - fs << "model_type" << "PINHOLE"; - fs << "camera_name" << m_cameraName; - fs << "image_width" << m_imageWidth; - fs << "image_height" << m_imageHeight; +bool CylindricalCamera::Parameters::readFromYamlFile( + const std::string& filename) { + cv::FileStorage fs(filename, cv::FileStorage::READ); - // projection: fx, fy, cx, cy - fs << "projection_parameters"; - fs << "{" << "fx" << m_fx - << "fy" << m_fy - << "cx" << m_cx - << "cy" << m_cy << "}"; + if (!fs.isOpened()) { + return false; + } - fs.release(); -} + if (!fs["model_type"].isNone()) { + std::string sModelType; + fs["model_type"] >> sModelType; -CylindricalCamera::Parameters& -CylindricalCamera::Parameters::operator=(const CylindricalCamera::Parameters& other) -{ - if (this != &other) - { - m_modelType = other.m_modelType; - m_cameraName = other.m_cameraName; - m_imageWidth = other.m_imageWidth; - m_imageHeight = other.m_imageHeight; - m_fx = other.m_fx; - m_fy = other.m_fy; - m_cx = other.m_cx; - m_cy = other.m_cy; + if (sModelType.compare("PINHOLE") != 0) { + return false; } - - return *this; -} - -std::ostream& -operator<< (std::ostream& out, const CylindricalCamera::Parameters& params) -{ - out << "Camera Parameters:" << std::endl; - out << " model_type " << "PINHOLE" << std::endl; - out << " camera_name " << params.m_cameraName << std::endl; - out << " image_width " << params.m_imageWidth << std::endl; - out << " image_height " << params.m_imageHeight << std::endl; - - // projection: fx, fy, cx, cy - out << "Projection Parameters" << std::endl; - out << " fx " << params.m_fx << std::endl - << " fy " << params.m_fy << std::endl - << " cx " << params.m_cx << std::endl - << " cy " << params.m_cy << std::endl; - - return out; + } + + m_modelType = CYLINRICALCAMERA; + fs["camera_name"] >> m_cameraName; + m_imageWidth = static_cast(fs["image_width"]); + m_imageHeight = static_cast(fs["image_height"]); + auto n = fs["projection_parameters"]; + m_fx = static_cast(n["fx"]); + m_fy = static_cast(n["fy"]); + m_cx = static_cast(n["cx"]); + m_cy = static_cast(n["cy"]); + + return true; +} + +void CylindricalCamera::Parameters::writeToYamlFile( + const std::string& filename) const { + cv::FileStorage fs(filename, cv::FileStorage::WRITE); + + fs << "model_type" + << "PINHOLE"; + fs << "camera_name" << m_cameraName; + fs << "image_width" << m_imageWidth; + fs << "image_height" << m_imageHeight; + + // projection: fx, fy, cx, cy + fs << "projection_parameters"; + fs << "{" + << "fx" << m_fx << "fy" << m_fy << "cx" << m_cx << "cy" << m_cy << "}"; + + fs.release(); +} + +CylindricalCamera::Parameters& CylindricalCamera::Parameters::operator=( + const CylindricalCamera::Parameters& other) { + if (this != &other) { + m_modelType = other.m_modelType; + m_cameraName = other.m_cameraName; + m_imageWidth = other.m_imageWidth; + m_imageHeight = other.m_imageHeight; + m_fx = other.m_fx; + m_fy = other.m_fy; + m_cx = other.m_cx; + m_cy = other.m_cy; + } + + return *this; +} + +std::ostream& operator<<(std::ostream& out, + const CylindricalCamera::Parameters& params) { + out << "Camera Parameters:" << std::endl; + out << " model_type " + << "PINHOLE" << std::endl; + out << " camera_name " << params.m_cameraName << std::endl; + out << " image_width " << params.m_imageWidth << std::endl; + out << " image_height " << params.m_imageHeight << std::endl; + + // projection: fx, fy, cx, cy + out << "Projection Parameters" << std::endl; + out << " fx " << params.m_fx << std::endl + << " fy " << params.m_fy << std::endl + << " cx " << params.m_cx << std::endl + << " cy " << params.m_cy << std::endl; + + return out; } CylindricalCamera::CylindricalCamera() - : m_inv_K11(1.0) - , m_inv_K13(0.0) - , m_inv_K22(1.0) - , m_inv_K23(0.0) - , m_noDistortion(true) -{ - -} + : m_inv_K11(1.0), + m_inv_K13(0.0), + m_inv_K22(1.0), + m_inv_K23(0.0), + m_noDistortion(true) {} CylindricalCamera::CylindricalCamera(const std::string& cameraName, - int imageWidth, int imageHeight, - double fx, double fy, double cx, double cy) - : mParameters(cameraName, imageWidth, imageHeight, fx, fy, cx, cy) -{ - m_noDistortion = true; - // Inverse camera projection matrix parameters - m_inv_K11 = 1.0 / mParameters.fx(); - m_inv_K13 = -mParameters.cx() / mParameters.fx(); - m_inv_K22 = 1.0 / mParameters.fy(); - m_inv_K23 = -mParameters.cy() / mParameters.fy(); - - K << fx, 0, cx, - 0, fy, cy, - 0, 0, 1; + int imageWidth, int imageHeight, double fx, + double fy, double cx, double cy) + : mParameters(cameraName, imageWidth, imageHeight, fx, fy, cx, cy) { + m_noDistortion = true; + // Inverse camera projection matrix parameters + m_inv_K11 = 1.0 / mParameters.fx(); + m_inv_K13 = -mParameters.cx() / mParameters.fx(); + m_inv_K22 = 1.0 / mParameters.fy(); + m_inv_K23 = -mParameters.cy() / mParameters.fy(); + + K << fx, 0, cx, 0, fy, cy, 0, 0, 1; } -CylindricalCamera::CylindricalCamera(const CylindricalCamera::Parameters& params) - : mParameters(params) -{ - m_noDistortion = true; - // Inverse camera projection matrix parameters - m_inv_K11 = 1.0 / mParameters.fx(); - m_inv_K13 = -mParameters.cx() / mParameters.fx(); - m_inv_K22 = 1.0 / mParameters.fy(); - m_inv_K23 = -mParameters.cy() / mParameters.fy(); - K << mParameters.fx(), 0, mParameters.cx(), - 0, mParameters.fy(), mParameters.fy(), - 0, 0, 1; +CylindricalCamera::CylindricalCamera( + const CylindricalCamera::Parameters& params) + : mParameters(params) { + m_noDistortion = true; + // Inverse camera projection matrix parameters + m_inv_K11 = 1.0 / mParameters.fx(); + m_inv_K13 = -mParameters.cx() / mParameters.fx(); + m_inv_K22 = 1.0 / mParameters.fy(); + m_inv_K23 = -mParameters.cy() / mParameters.fy(); + K << mParameters.fx(), 0, mParameters.cx(), 0, mParameters.fy(), + mParameters.fy(), 0, 0, 1; } -Camera::ModelType -CylindricalCamera::modelType(void) const -{ - return mParameters.modelType(); +Camera::ModelType CylindricalCamera::modelType(void) const { + return mParameters.modelType(); } -const std::string& -CylindricalCamera::cameraName(void) const -{ - return mParameters.cameraName(); +const std::string& CylindricalCamera::cameraName(void) const { + return mParameters.cameraName(); } -int -CylindricalCamera::imageWidth(void) const -{ - return mParameters.imageWidth(); +int CylindricalCamera::imageWidth(void) const { + return mParameters.imageWidth(); } -int -CylindricalCamera::imageHeight(void) const -{ - return mParameters.imageHeight(); +int CylindricalCamera::imageHeight(void) const { + return mParameters.imageHeight(); } -void -CylindricalCamera::estimateIntrinsics(const cv::Size& boardSize, - const std::vector< std::vector >& objectPoints, - const std::vector< std::vector >& imagePoints) -{ - //TODO: implement this +void CylindricalCamera::estimateIntrinsics( + const cv::Size& boardSize, + const std::vector >& objectPoints, + const std::vector >& imagePoints) { + // TODO: implement this } /** @@ -254,12 +191,11 @@ CylindricalCamera::estimateIntrinsics(const cv::Size& boardSize, * \param p image coordinates * \param P coordinates of the point on the sphere */ -void -CylindricalCamera::liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const -{ - liftProjective(p, P); +void CylindricalCamera::liftSphere(const Eigen::Vector2d& p, + Eigen::Vector3d& P) const { + liftProjective(p, P); - P.normalize(); + P.normalize(); } /** @@ -268,40 +204,37 @@ CylindricalCamera::liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) cons * \param p image coordinates * \param P coordinates of the projective ray */ -void -CylindricalCamera::liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const -{ - // Lift points to normalised plane - double phi = m_inv_K11 * p(0) + m_inv_K13; - double y_by_rho = m_inv_K22 * p(1) + m_inv_K23; - //Assume z = 1 - double z = fabs(phi) > M_PI/2 ? -1.0 : 1.0; - //phi = atan2(X,Z); recover X - double x = z * tan(phi); - double rho = sqrt(x * x + z * z); - //\rho = \sqrt{x^2 + z^2} - double y = y_by_rho * rho; - P = Eigen::Vector3d(x, y, z); +void CylindricalCamera::liftProjective(const Eigen::Vector2d& p, + Eigen::Vector3d& P) const { + // Lift points to normalised plane + double phi = m_inv_K11 * p(0) + m_inv_K13; + double y_by_rho = m_inv_K22 * p(1) + m_inv_K23; + // Assume z = 1 + double z = fabs(phi) > M_PI / 2 ? -1.0 : 1.0; + // phi = atan2(X,Z); recover X + double x = z * tan(phi); + double rho = sqrt(x * x + z * z); + //\rho = \sqrt{x^2 + z^2} + double y = y_by_rho * rho; + P = Eigen::Vector3d(x, y, z); } - /** * \brief Project a 3D point (\a x,\a y,\a z) to the image plane in (\a u,\a v) * * \param P 3D point coordinates * \param p return value, contains the image point coordinates */ -void -CylindricalCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const -{ - auto rho = sqrt(P.x()*P.x() + P.z()*P.z()); - auto phi = atan2(P.x(), P.z()); - - Eigen::Vector3d p_c_cyn{rho*phi, P.y(), rho}; - Eigen::Vector3d _p = K*p_c_cyn; - _p/=_p.z(); - p.x() = _p.x(); - p.y() = _p.y(); +void CylindricalCamera::spaceToPlane(const Eigen::Vector3d& P, + Eigen::Vector2d& p) const { + auto rho = sqrt(P.x() * P.x() + P.z() * P.z()); + auto phi = atan2(P.x(), P.z()); + + Eigen::Vector3d p_c_cyn{rho * phi, P.y(), rho}; + Eigen::Vector3d _p = K * p_c_cyn; + _p /= _p.z(); + p.x() = _p.x(); + p.y() = _p.y(); } /** @@ -310,11 +243,10 @@ CylindricalCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) co * \param p_u 2D point coordinates * \return image point coordinates */ -void -CylindricalCamera::undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const -{ - Eigen::Vector3d p3d{p_u.x(), p_u.y(), 1.0}; - spaceToPlane(p3d, p); +void CylindricalCamera::undistToPlane(const Eigen::Vector2d& p_u, + Eigen::Vector2d& p) const { + Eigen::Vector3d p3d{p_u.x(), p_u.y(), 1.0}; + spaceToPlane(p3d, p); } /** @@ -323,10 +255,9 @@ CylindricalCamera::undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) * \param p_u undistorted coordinates of point on the normalised plane * \return to obtain the distorted point: p_d = p_u + d_u */ -void -CylindricalCamera::distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const -{ - d_u = p_u; +void CylindricalCamera::distortion(const Eigen::Vector2d& p_u, + Eigen::Vector2d& d_u) const { + d_u = p_u; } /** @@ -336,94 +267,78 @@ CylindricalCamera::distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) * \param p_u undistorted coordinates of point on the normalised plane * \return to obtain the distorted point: p_d = p_u + d_u */ -void -CylindricalCamera::distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u, - Eigen::Matrix2d& J) const -{ - d_u = p_u; +void CylindricalCamera::distortion(const Eigen::Vector2d& p_u, + Eigen::Vector2d& d_u, + Eigen::Matrix2d& J) const { + d_u = p_u; } -void -CylindricalCamera::initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale) const -{ - //No distortion +void CylindricalCamera::initUndistortMap(cv::Mat& map1, cv::Mat& map2, + double fScale) const { + // No distortion } -cv::Mat -CylindricalCamera::initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, - float fx, float fy, - cv::Size imageSize, - float cx, float cy, - cv::Mat rmat) const -{ - //No distortion +cv::Mat CylindricalCamera::initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, + float fx, float fy, + cv::Size imageSize, float cx, + float cy, + cv::Mat rmat) const { + // No distortion } -int -CylindricalCamera::parameterCount(void) const -{ - return 4; -} +int CylindricalCamera::parameterCount(void) const { return 4; } -const CylindricalCamera::Parameters& -CylindricalCamera::getParameters(void) const -{ - return mParameters; +const CylindricalCamera::Parameters& CylindricalCamera::getParameters( + void) const { + return mParameters; } -void -CylindricalCamera::setParameters(const CylindricalCamera::Parameters& parameters) -{ - mParameters = parameters; - m_noDistortion = true; +void CylindricalCamera::setParameters( + const CylindricalCamera::Parameters& parameters) { + mParameters = parameters; + m_noDistortion = true; - m_inv_K11 = 1.0 / mParameters.fx(); - m_inv_K13 = -mParameters.cx() / mParameters.fx(); - m_inv_K22 = 1.0 / mParameters.fy(); - m_inv_K23 = -mParameters.cy() / mParameters.fy(); + m_inv_K11 = 1.0 / mParameters.fx(); + m_inv_K13 = -mParameters.cx() / mParameters.fx(); + m_inv_K22 = 1.0 / mParameters.fy(); + m_inv_K23 = -mParameters.cy() / mParameters.fy(); } -void -CylindricalCamera::readParameters(const std::vector& parameterVec) -{ - if ((int)parameterVec.size() != parameterCount()) - { - return; - } +void CylindricalCamera::readParameters( + const std::vector& parameterVec) { + if ((int)parameterVec.size() != parameterCount()) { + return; + } - Parameters params = getParameters(); + Parameters params = getParameters(); - params.fx() = parameterVec.at(0); - params.fy() = parameterVec.at(1); - params.cx() = parameterVec.at(2); - params.cy() = parameterVec.at(3); + params.fx() = parameterVec.at(0); + params.fy() = parameterVec.at(1); + params.cx() = parameterVec.at(2); + params.cy() = parameterVec.at(3); - setParameters(params); + setParameters(params); } -void -CylindricalCamera::writeParameters(std::vector& parameterVec) const -{ - parameterVec.resize(parameterCount()); - parameterVec.at(0) = mParameters.fx(); - parameterVec.at(1) = mParameters.fy(); - parameterVec.at(2) = mParameters.cx(); - parameterVec.at(3) = mParameters.cy(); +void CylindricalCamera::writeParameters( + std::vector& parameterVec) const { + parameterVec.resize(parameterCount()); + parameterVec.at(0) = mParameters.fx(); + parameterVec.at(1) = mParameters.fy(); + parameterVec.at(2) = mParameters.cx(); + parameterVec.at(3) = mParameters.cy(); } -void -CylindricalCamera::writeParametersToYamlFile(const std::string& filename) const -{ - mParameters.writeToYamlFile(filename); +void CylindricalCamera::writeParametersToYamlFile( + const std::string& filename) const { + mParameters.writeToYamlFile(filename); } -std::string -CylindricalCamera::parametersToString(void) const -{ - std::ostringstream oss; - oss << mParameters; +std::string CylindricalCamera::parametersToString(void) const { + std::ostringstream oss; + oss << mParameters; - return oss.str(); + return oss.str(); } -} +} // namespace camodocal diff --git a/camera_models/src/camera_models/EquidistantCamera.cc b/camera_models/src/camera_models/EquidistantCamera.cc index 480f51ac..1cbabd21 100644 --- a/camera_models/src/camera_models/EquidistantCamera.cc +++ b/camera_models/src/camera_models/EquidistantCamera.cc @@ -11,399 +11,293 @@ #include "camodocal/gpl/gpl.h" -namespace camodocal -{ +namespace camodocal { EquidistantCamera::Parameters::Parameters() - : Camera::Parameters(KANNALA_BRANDT) - , m_k2(0.0) - , m_k3(0.0) - , m_k4(0.0) - , m_k5(0.0) - , m_mu(0.0) - , m_mv(0.0) - , m_u0(0.0) - , m_v0(0.0) -{ + : Camera::Parameters(KANNALA_BRANDT), + m_k2(0.0), + m_k3(0.0), + m_k4(0.0), + m_k5(0.0), + m_mu(0.0), + m_mv(0.0), + m_u0(0.0), + m_v0(0.0) {} -} - -EquidistantCamera::Parameters::Parameters(const std::string& cameraName, - int w, int h, - double k2, double k3, double k4, double k5, - double mu, double mv, - double u0, double v0) - : Camera::Parameters(KANNALA_BRANDT, cameraName, w, h) - , m_k2(k2) - , m_k3(k3) - , m_k4(k4) - , m_k5(k5) - , m_mu(mu) - , m_mv(mv) - , m_u0(u0) - , m_v0(v0) -{ - -} +EquidistantCamera::Parameters::Parameters(const std::string& cameraName, int w, + int h, double k2, double k3, + double k4, double k5, double mu, + double mv, double u0, double v0) + : Camera::Parameters(KANNALA_BRANDT, cameraName, w, h), + m_k2(k2), + m_k3(k3), + m_k4(k4), + m_k5(k5), + m_mu(mu), + m_mv(mv), + m_u0(u0), + m_v0(v0) {} -double& -EquidistantCamera::Parameters::k2(void) -{ - return m_k2; -} +double& EquidistantCamera::Parameters::k2(void) { return m_k2; } -double& -EquidistantCamera::Parameters::k3(void) -{ - return m_k3; -} +double& EquidistantCamera::Parameters::k3(void) { return m_k3; } -double& -EquidistantCamera::Parameters::k4(void) -{ - return m_k4; -} +double& EquidistantCamera::Parameters::k4(void) { return m_k4; } -double& -EquidistantCamera::Parameters::k5(void) -{ - return m_k5; -} +double& EquidistantCamera::Parameters::k5(void) { return m_k5; } -double& -EquidistantCamera::Parameters::mu(void) -{ - return m_mu; -} +double& EquidistantCamera::Parameters::mu(void) { return m_mu; } -double& -EquidistantCamera::Parameters::mv(void) -{ - return m_mv; -} +double& EquidistantCamera::Parameters::mv(void) { return m_mv; } -double& -EquidistantCamera::Parameters::u0(void) -{ - return m_u0; -} +double& EquidistantCamera::Parameters::u0(void) { return m_u0; } -double& -EquidistantCamera::Parameters::v0(void) -{ - return m_v0; -} +double& EquidistantCamera::Parameters::v0(void) { return m_v0; } -double -EquidistantCamera::Parameters::k2(void) const -{ - return m_k2; -} +double EquidistantCamera::Parameters::k2(void) const { return m_k2; } -double -EquidistantCamera::Parameters::k3(void) const -{ - return m_k3; -} +double EquidistantCamera::Parameters::k3(void) const { return m_k3; } -double -EquidistantCamera::Parameters::k4(void) const -{ - return m_k4; -} +double EquidistantCamera::Parameters::k4(void) const { return m_k4; } -double -EquidistantCamera::Parameters::k5(void) const -{ - return m_k5; -} +double EquidistantCamera::Parameters::k5(void) const { return m_k5; } -double -EquidistantCamera::Parameters::mu(void) const -{ - return m_mu; -} +double EquidistantCamera::Parameters::mu(void) const { return m_mu; } -double -EquidistantCamera::Parameters::mv(void) const -{ - return m_mv; -} +double EquidistantCamera::Parameters::mv(void) const { return m_mv; } -double -EquidistantCamera::Parameters::u0(void) const -{ - return m_u0; -} +double EquidistantCamera::Parameters::u0(void) const { return m_u0; } -double -EquidistantCamera::Parameters::v0(void) const -{ - return m_v0; -} +double EquidistantCamera::Parameters::v0(void) const { return m_v0; } -bool -EquidistantCamera::Parameters::readFromYamlFile(const std::string& filename) -{ - cv::FileStorage fs(filename, cv::FileStorage::READ); +bool EquidistantCamera::Parameters::readFromYamlFile( + const std::string& filename) { + cv::FileStorage fs(filename, cv::FileStorage::READ); - if (!fs.isOpened()) - { - return false; - } + if (!fs.isOpened()) { + return false; + } - if (!fs["model_type"].isNone()) - { - std::string sModelType; - fs["model_type"] >> sModelType; + if (!fs["model_type"].isNone()) { + std::string sModelType; + fs["model_type"] >> sModelType; - if (sModelType.compare("KANNALA_BRANDT") != 0) - { - return false; - } + if (sModelType.compare("KANNALA_BRANDT") != 0) { + return false; } - - m_modelType = KANNALA_BRANDT; - fs["camera_name"] >> m_cameraName; - m_imageWidth = static_cast(fs["image_width"]); - m_imageHeight = static_cast(fs["image_height"]); - - cv::FileNode n = fs["projection_parameters"]; - m_k2 = static_cast(n["k2"]); - m_k3 = static_cast(n["k3"]); - m_k4 = static_cast(n["k4"]); - m_k5 = static_cast(n["k5"]); - m_mu = static_cast(n["mu"]); - m_mv = static_cast(n["mv"]); - m_u0 = static_cast(n["u0"]); - m_v0 = static_cast(n["v0"]); - - return true; -} - -void -EquidistantCamera::Parameters::writeToYamlFile(const std::string& filename) const -{ - cv::FileStorage fs(filename, cv::FileStorage::WRITE); - - fs << "model_type" << "KANNALA_BRANDT"; - fs << "camera_name" << m_cameraName; - fs << "image_width" << m_imageWidth; - fs << "image_height" << m_imageHeight; - - // projection: k2, k3, k4, k5, mu, mv, u0, v0 - fs << "projection_parameters"; - fs << "{" << "k2" << m_k2 - << "k3" << m_k3 - << "k4" << m_k4 - << "k5" << m_k5 - << "mu" << m_mu - << "mv" << m_mv - << "u0" << m_u0 - << "v0" << m_v0 << "}"; - - fs.release(); -} - -EquidistantCamera::Parameters& -EquidistantCamera::Parameters::operator=(const EquidistantCamera::Parameters& other) -{ - if (this != &other) - { - m_modelType = other.m_modelType; - m_cameraName = other.m_cameraName; - m_imageWidth = other.m_imageWidth; - m_imageHeight = other.m_imageHeight; - m_k2 = other.m_k2; - m_k3 = other.m_k3; - m_k4 = other.m_k4; - m_k5 = other.m_k5; - m_mu = other.m_mu; - m_mv = other.m_mv; - m_u0 = other.m_u0; - m_v0 = other.m_v0; - } - - return *this; -} - -std::ostream& -operator<< (std::ostream& out, const EquidistantCamera::Parameters& params) -{ - out << "Camera Parameters:" << std::endl; - out << " model_type " << "KANNALA_BRANDT" << std::endl; - out << " camera_name " << params.m_cameraName << std::endl; - out << " image_width " << params.m_imageWidth << std::endl; - out << " image_height " << params.m_imageHeight << std::endl; - - // projection: k2, k3, k4, k5, mu, mv, u0, v0 - out << "Projection Parameters" << std::endl; - out << " k2 " << params.m_k2 << std::endl - << " k3 " << params.m_k3 << std::endl - << " k4 " << params.m_k4 << std::endl - << " k5 " << params.m_k5 << std::endl - << " mu " << params.m_mu << std::endl - << " mv " << params.m_mv << std::endl - << " u0 " << params.m_u0 << std::endl - << " v0 " << params.m_v0 << std::endl; - - return out; + } + + m_modelType = KANNALA_BRANDT; + fs["camera_name"] >> m_cameraName; + m_imageWidth = static_cast(fs["image_width"]); + m_imageHeight = static_cast(fs["image_height"]); + + cv::FileNode n = fs["projection_parameters"]; + m_k2 = static_cast(n["k2"]); + m_k3 = static_cast(n["k3"]); + m_k4 = static_cast(n["k4"]); + m_k5 = static_cast(n["k5"]); + m_mu = static_cast(n["mu"]); + m_mv = static_cast(n["mv"]); + m_u0 = static_cast(n["u0"]); + m_v0 = static_cast(n["v0"]); + + return true; +} + +void EquidistantCamera::Parameters::writeToYamlFile( + const std::string& filename) const { + cv::FileStorage fs(filename, cv::FileStorage::WRITE); + + fs << "model_type" + << "KANNALA_BRANDT"; + fs << "camera_name" << m_cameraName; + fs << "image_width" << m_imageWidth; + fs << "image_height" << m_imageHeight; + + // projection: k2, k3, k4, k5, mu, mv, u0, v0 + fs << "projection_parameters"; + fs << "{" + << "k2" << m_k2 << "k3" << m_k3 << "k4" << m_k4 << "k5" << m_k5 << "mu" + << m_mu << "mv" << m_mv << "u0" << m_u0 << "v0" << m_v0 << "}"; + + fs.release(); +} + +EquidistantCamera::Parameters& EquidistantCamera::Parameters::operator=( + const EquidistantCamera::Parameters& other) { + if (this != &other) { + m_modelType = other.m_modelType; + m_cameraName = other.m_cameraName; + m_imageWidth = other.m_imageWidth; + m_imageHeight = other.m_imageHeight; + m_k2 = other.m_k2; + m_k3 = other.m_k3; + m_k4 = other.m_k4; + m_k5 = other.m_k5; + m_mu = other.m_mu; + m_mv = other.m_mv; + m_u0 = other.m_u0; + m_v0 = other.m_v0; + } + + return *this; +} + +std::ostream& operator<<(std::ostream& out, + const EquidistantCamera::Parameters& params) { + out << "Camera Parameters:" << std::endl; + out << " model_type " + << "KANNALA_BRANDT" << std::endl; + out << " camera_name " << params.m_cameraName << std::endl; + out << " image_width " << params.m_imageWidth << std::endl; + out << " image_height " << params.m_imageHeight << std::endl; + + // projection: k2, k3, k4, k5, mu, mv, u0, v0 + out << "Projection Parameters" << std::endl; + out << " k2 " << params.m_k2 << std::endl + << " k3 " << params.m_k3 << std::endl + << " k4 " << params.m_k4 << std::endl + << " k5 " << params.m_k5 << std::endl + << " mu " << params.m_mu << std::endl + << " mv " << params.m_mv << std::endl + << " u0 " << params.m_u0 << std::endl + << " v0 " << params.m_v0 << std::endl; + + return out; } EquidistantCamera::EquidistantCamera() - : m_inv_K11(1.0) - , m_inv_K13(0.0) - , m_inv_K22(1.0) - , m_inv_K23(0.0) -{ - -} + : m_inv_K11(1.0), m_inv_K13(0.0), m_inv_K22(1.0), m_inv_K23(0.0) {} EquidistantCamera::EquidistantCamera(const std::string& cameraName, - int imageWidth, int imageHeight, - double k2, double k3, double k4, double k5, - double mu, double mv, - double u0, double v0) - : mParameters(cameraName, imageWidth, imageHeight, - k2, k3, k4, k5, mu, mv, u0, v0) -{ - // Inverse camera projection matrix parameters - m_inv_K11 = 1.0 / mParameters.mu(); - m_inv_K13 = -mParameters.u0() / mParameters.mu(); - m_inv_K22 = 1.0 / mParameters.mv(); - m_inv_K23 = -mParameters.v0() / mParameters.mv(); -} + int imageWidth, int imageHeight, double k2, + double k3, double k4, double k5, double mu, + double mv, double u0, double v0) + : mParameters(cameraName, imageWidth, imageHeight, k2, k3, k4, k5, mu, mv, + u0, v0) { + // Inverse camera projection matrix parameters + m_inv_K11 = 1.0 / mParameters.mu(); + m_inv_K13 = -mParameters.u0() / mParameters.mu(); + m_inv_K22 = 1.0 / mParameters.mv(); + m_inv_K23 = -mParameters.v0() / mParameters.mv(); +} + +EquidistantCamera::EquidistantCamera( + const EquidistantCamera::Parameters& params) + : mParameters(params) { + // Inverse camera projection matrix parameters + m_inv_K11 = 1.0 / mParameters.mu(); + m_inv_K13 = -mParameters.u0() / mParameters.mu(); + m_inv_K22 = 1.0 / mParameters.mv(); + m_inv_K23 = -mParameters.v0() / mParameters.mv(); +} + +Camera::ModelType EquidistantCamera::modelType(void) const { + return mParameters.modelType(); +} + +const std::string& EquidistantCamera::cameraName(void) const { + return mParameters.cameraName(); +} + +int EquidistantCamera::imageWidth(void) const { + return mParameters.imageWidth(); +} + +int EquidistantCamera::imageHeight(void) const { + return mParameters.imageHeight(); +} + +void EquidistantCamera::estimateIntrinsics( + const cv::Size& boardSize, + const std::vector >& objectPoints, + const std::vector >& imagePoints) { + Parameters params = getParameters(); + + double u0 = params.imageWidth() / 2.0; + double v0 = params.imageHeight() / 2.0; + + double minReprojErr = std::numeric_limits::max(); + + std::vector rvecs, tvecs; + rvecs.assign(objectPoints.size(), cv::Mat()); + tvecs.assign(objectPoints.size(), cv::Mat()); + + params.k2() = 0.0; + params.k3() = 0.0; + params.k4() = 0.0; + params.k5() = 0.0; + params.u0() = u0; + params.v0() = v0; + + // Initialize focal length + // C. Hughes, P. Denny, M. Glavin, and E. Jones, + // Equidistant Fish-Eye Calibration and Rectification by Vanishing Point + // Extraction, PAMI 2010 + // Find circles from rows of chessboard corners, and for each pair + // of circles, find vanishing points: v1 and v2. + // f = ||v1 - v2|| / PI; + double f0 = 0.0; + for (size_t i = 0; i < imagePoints.size(); ++i) { + std::vector center(boardSize.height); + double radius[boardSize.height]; + for (int r = 0; r < boardSize.height; ++r) { + std::vector circle; + for (int c = 0; c < boardSize.width; ++c) { + circle.push_back(imagePoints.at(i).at(r * boardSize.width + c)); + } + + fitCircle(circle, center[r](0), center[r](1), radius[r]); + } -EquidistantCamera::EquidistantCamera(const EquidistantCamera::Parameters& params) - : mParameters(params) -{ - // Inverse camera projection matrix parameters - m_inv_K11 = 1.0 / mParameters.mu(); - m_inv_K13 = -mParameters.u0() / mParameters.mu(); - m_inv_K22 = 1.0 / mParameters.mv(); - m_inv_K23 = -mParameters.v0() / mParameters.mv(); -} + for (int j = 0; j < boardSize.height; ++j) { + for (int k = j + 1; k < boardSize.height; ++k) { + // find distance between pair of vanishing points which + // correspond to intersection points of 2 circles + std::vector ipts; + ipts = intersectCircles(center[j](0), center[j](1), radius[j], + center[k](0), center[k](1), radius[k]); -Camera::ModelType -EquidistantCamera::modelType(void) const -{ - return mParameters.modelType(); -} + if (ipts.size() < 2) { + continue; + } -const std::string& -EquidistantCamera::cameraName(void) const -{ - return mParameters.cameraName(); -} + double f = cv::norm(ipts.at(0) - ipts.at(1)) / M_PI; -int -EquidistantCamera::imageWidth(void) const -{ - return mParameters.imageWidth(); -} + params.mu() = f; + params.mv() = f; -int -EquidistantCamera::imageHeight(void) const -{ - return mParameters.imageHeight(); -} + setParameters(params); -void -EquidistantCamera::estimateIntrinsics(const cv::Size& boardSize, - const std::vector< std::vector >& objectPoints, - const std::vector< std::vector >& imagePoints) -{ - Parameters params = getParameters(); - - double u0 = params.imageWidth() / 2.0; - double v0 = params.imageHeight() / 2.0; - - double minReprojErr = std::numeric_limits::max(); - - std::vector rvecs, tvecs; - rvecs.assign(objectPoints.size(), cv::Mat()); - tvecs.assign(objectPoints.size(), cv::Mat()); - - params.k2() = 0.0; - params.k3() = 0.0; - params.k4() = 0.0; - params.k5() = 0.0; - params.u0() = u0; - params.v0() = v0; - - // Initialize focal length - // C. Hughes, P. Denny, M. Glavin, and E. Jones, - // Equidistant Fish-Eye Calibration and Rectification by Vanishing Point - // Extraction, PAMI 2010 - // Find circles from rows of chessboard corners, and for each pair - // of circles, find vanishing points: v1 and v2. - // f = ||v1 - v2|| / PI; - double f0 = 0.0; - for (size_t i = 0; i < imagePoints.size(); ++i) - { - std::vector center(boardSize.height); - double radius[boardSize.height]; - for (int r = 0; r < boardSize.height; ++r) - { - std::vector circle; - for (int c = 0; c < boardSize.width; ++c) - { - circle.push_back(imagePoints.at(i).at(r * boardSize.width + c)); - } - - fitCircle(circle, center[r](0), center[r](1), radius[r]); + for (size_t l = 0; l < objectPoints.size(); ++l) { + estimateExtrinsics(objectPoints.at(l), imagePoints.at(l), rvecs.at(l), + tvecs.at(l)); } - for (int j = 0; j < boardSize.height; ++j) - { - for (int k = j + 1; k < boardSize.height; ++k) - { - // find distance between pair of vanishing points which - // correspond to intersection points of 2 circles - std::vector ipts; - ipts = intersectCircles(center[j](0), center[j](1), radius[j], - center[k](0), center[k](1), radius[k]); - - if (ipts.size() < 2) - { - continue; - } - - double f = cv::norm(ipts.at(0) - ipts.at(1)) / M_PI; - - params.mu() = f; - params.mv() = f; - - setParameters(params); - - for (size_t l = 0; l < objectPoints.size(); ++l) - { - estimateExtrinsics(objectPoints.at(l), imagePoints.at(l), rvecs.at(l), tvecs.at(l)); - } - - double reprojErr = reprojectionError(objectPoints, imagePoints, rvecs, tvecs, cv::noArray()); - - if (reprojErr < minReprojErr) - { - minReprojErr = reprojErr; - f0 = f; - } - } + double reprojErr = reprojectionError(objectPoints, imagePoints, rvecs, + tvecs, cv::noArray()); + + if (reprojErr < minReprojErr) { + minReprojErr = reprojErr; + f0 = f; } + } } + } - if (f0 <= 0.0 && minReprojErr >= std::numeric_limits::max()) - { - std::cout << "[" << params.cameraName() << "] " - << "# INFO: kannala-Brandt model fails with given data. " << std::endl; + if (f0 <= 0.0 && minReprojErr >= std::numeric_limits::max()) { + std::cout << "[" << params.cameraName() << "] " + << "# INFO: kannala-Brandt model fails with given data. " + << std::endl; - return; - } + return; + } - params.mu() = f0; - params.mv() = f0; + params.mu() = f0; + params.mv() = f0; - setParameters(params); + setParameters(params); } /** @@ -412,409 +306,349 @@ EquidistantCamera::estimateIntrinsics(const cv::Size& boardSize, * \param p image coordinates * \param P coordinates of the point on the sphere */ -void -EquidistantCamera::liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const -{ - liftProjective(p, P); +void EquidistantCamera::liftSphere(const Eigen::Vector2d& p, + Eigen::Vector3d& P) const { + liftProjective(p, P); } -/** +/** * \brief Lifts a point from the image plane to its projective ray * * \param p image coordinates * \param P coordinates of the projective ray */ -void -EquidistantCamera::liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const -{ - // Lift points to normalised plane - Eigen::Vector2d p_u; - p_u << m_inv_K11 * p(0) + m_inv_K13, - m_inv_K22 * p(1) + m_inv_K23; - - // Obtain a projective ray - double theta, phi; - backprojectSymmetric(p_u, theta, phi); - - P(0) = sin(theta) * cos(phi); - P(1) = sin(theta) * sin(phi); - P(2) = cos(theta); +void EquidistantCamera::liftProjective(const Eigen::Vector2d& p, + Eigen::Vector3d& P) const { + // Lift points to normalised plane + Eigen::Vector2d p_u; + p_u << m_inv_K11 * p(0) + m_inv_K13, m_inv_K22 * p(1) + m_inv_K23; + + // Obtain a projective ray + double theta, phi; + backprojectSymmetric(p_u, theta, phi); + + P(0) = sin(theta) * cos(phi); + P(1) = sin(theta) * sin(phi); + P(2) = cos(theta); } -/** +/** * \brief Project a 3D point (\a x,\a y,\a z) to the image plane in (\a u,\a v) * * \param P 3D point coordinates * \param p return value, contains the image point coordinates */ -void -EquidistantCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const -{ - double theta = acos(P(2) / P.norm()); - double phi = atan2(P(1), P(0)); +void EquidistantCamera::spaceToPlane(const Eigen::Vector3d& P, + Eigen::Vector2d& p) const { + double theta = acos(P(2) / P.norm()); + double phi = atan2(P(1), P(0)); - Eigen::Vector2d p_u = r(mParameters.k2(), mParameters.k3(), mParameters.k4(), mParameters.k5(), theta) * Eigen::Vector2d(cos(phi), sin(phi)); + Eigen::Vector2d p_u = r(mParameters.k2(), mParameters.k3(), mParameters.k4(), + mParameters.k5(), theta) * + Eigen::Vector2d(cos(phi), sin(phi)); - // Apply generalised projection matrix - p << mParameters.mu() * p_u(0) + mParameters.u0(), - mParameters.mv() * p_u(1) + mParameters.v0(); + // Apply generalised projection matrix + p << mParameters.mu() * p_u(0) + mParameters.u0(), + mParameters.mv() * p_u(1) + mParameters.v0(); } - -/** +/** * \brief Project a 3D point to the image plane and calculate Jacobian * * \param P 3D point coordinates * \param p return value, contains the image point coordinates */ -void -EquidistantCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, - Eigen::Matrix& J) const -{ - double theta = acos(P(2) / P.norm()); - double phi = atan2(P(1), P(0)); - - Eigen::Vector2d p_u = r(mParameters.k2(), mParameters.k3(), mParameters.k4(), mParameters.k5(), theta) * Eigen::Vector2d(cos(phi), sin(phi)); - - // Apply generalised projection matrix - p << mParameters.mu() * p_u(0) + mParameters.u0(), - mParameters.mv() * p_u(1) + mParameters.v0(); +void EquidistantCamera::spaceToPlane(const Eigen::Vector3d& P, + Eigen::Vector2d& p, + Eigen::Matrix& J) const { + double theta = acos(P(2) / P.norm()); + double phi = atan2(P(1), P(0)); + + Eigen::Vector2d p_u = r(mParameters.k2(), mParameters.k3(), mParameters.k4(), + mParameters.k5(), theta) * + Eigen::Vector2d(cos(phi), sin(phi)); + + // Apply generalised projection matrix + p << mParameters.mu() * p_u(0) + mParameters.u0(), + mParameters.mv() * p_u(1) + mParameters.v0(); } -/** +/** * \brief Projects an undistorted 2D point p_u to the image plane * * \param p_u 2D point coordinates * \return image point coordinates */ -void -EquidistantCamera::undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const -{ -// Eigen::Vector2d p_d; -// -// if (m_noDistortion) -// { -// p_d = p_u; -// } -// else -// { -// // Apply distortion -// Eigen::Vector2d d_u; -// distortion(p_u, d_u); -// p_d = p_u + d_u; -// } -// -// // Apply generalised projection matrix -// p << mParameters.gamma1() * p_d(0) + mParameters.u0(), -// mParameters.gamma2() * p_d(1) + mParameters.v0(); -} - -void -EquidistantCamera::initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale) const -{ - cv::Size imageSize(mParameters.imageWidth(), mParameters.imageHeight()); - - cv::Mat mapX = cv::Mat::zeros(imageSize, CV_32F); - cv::Mat mapY = cv::Mat::zeros(imageSize, CV_32F); - - for (int v = 0; v < imageSize.height; ++v) - { - for (int u = 0; u < imageSize.width; ++u) - { - double mx_u = m_inv_K11 / fScale * u + m_inv_K13 / fScale; - double my_u = m_inv_K22 / fScale * v + m_inv_K23 / fScale; - - double theta, phi; - backprojectSymmetric(Eigen::Vector2d(mx_u, my_u), theta, phi); - - Eigen::Vector3d P; - P << sin(theta) * cos(phi), sin(theta) * sin(phi), cos(theta); - - Eigen::Vector2d p; - spaceToPlane(P, p); - - mapX.at(v,u) = p(0); - mapY.at(v,u) = p(1); - } +void EquidistantCamera::undistToPlane(const Eigen::Vector2d& p_u, + Eigen::Vector2d& p) const { + // Eigen::Vector2d p_d; + // + // if (m_noDistortion) + // { + // p_d = p_u; + // } + // else + // { + // // Apply distortion + // Eigen::Vector2d d_u; + // distortion(p_u, d_u); + // p_d = p_u + d_u; + // } + // + // // Apply generalised projection matrix + // p << mParameters.gamma1() * p_d(0) + mParameters.u0(), + // mParameters.gamma2() * p_d(1) + mParameters.v0(); +} + +void EquidistantCamera::initUndistortMap(cv::Mat& map1, cv::Mat& map2, + double fScale) const { + cv::Size imageSize(mParameters.imageWidth(), mParameters.imageHeight()); + + cv::Mat mapX = cv::Mat::zeros(imageSize, CV_32F); + cv::Mat mapY = cv::Mat::zeros(imageSize, CV_32F); + + for (int v = 0; v < imageSize.height; ++v) { + for (int u = 0; u < imageSize.width; ++u) { + double mx_u = m_inv_K11 / fScale * u + m_inv_K13 / fScale; + double my_u = m_inv_K22 / fScale * v + m_inv_K23 / fScale; + + double theta, phi; + backprojectSymmetric(Eigen::Vector2d(mx_u, my_u), theta, phi); + + Eigen::Vector3d P; + P << sin(theta) * cos(phi), sin(theta) * sin(phi), cos(theta); + + Eigen::Vector2d p; + spaceToPlane(P, p); + + mapX.at(v, u) = p(0); + mapY.at(v, u) = p(1); } + } - cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false); + cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false); } -cv::Mat -EquidistantCamera::initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, - float fx, float fy, - cv::Size imageSize, - float cx, float cy, - cv::Mat rmat) const -{ - if (imageSize == cv::Size(0, 0)) - { - imageSize = cv::Size(mParameters.imageWidth(), mParameters.imageHeight()); - } +cv::Mat EquidistantCamera::initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, + float fx, float fy, + cv::Size imageSize, float cx, + float cy, + cv::Mat rmat) const { + if (imageSize == cv::Size(0, 0)) { + imageSize = cv::Size(mParameters.imageWidth(), mParameters.imageHeight()); + } - cv::Mat mapX = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F); - cv::Mat mapY = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F); + cv::Mat mapX = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F); + cv::Mat mapY = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F); - Eigen::Matrix3f K_rect; + Eigen::Matrix3f K_rect; - if (cx == -1.0f && cy == -1.0f) - { - K_rect << fx, 0, imageSize.width / 2, - 0, fy, imageSize.height / 2, - 0, 0, 1; - } - else - { - K_rect << fx, 0, cx, - 0, fy, cy, - 0, 0, 1; - } + if (cx == -1.0f && cy == -1.0f) { + K_rect << fx, 0, imageSize.width / 2, 0, fy, imageSize.height / 2, 0, 0, 1; + } else { + K_rect << fx, 0, cx, 0, fy, cy, 0, 0, 1; + } - if (fx == -1.0f || fy == -1.0f) - { - K_rect(0,0) = mParameters.mu(); - K_rect(1,1) = mParameters.mv(); - } + if (fx == -1.0f || fy == -1.0f) { + K_rect(0, 0) = mParameters.mu(); + K_rect(1, 1) = mParameters.mv(); + } - Eigen::Matrix3f K_rect_inv = K_rect.inverse(); + Eigen::Matrix3f K_rect_inv = K_rect.inverse(); - Eigen::Matrix3f R, R_inv; - cv::cv2eigen(rmat, R); - R_inv = R.inverse(); + Eigen::Matrix3f R, R_inv; + cv::cv2eigen(rmat, R); + R_inv = R.inverse(); - for (int v = 0; v < imageSize.height; ++v) - { - for (int u = 0; u < imageSize.width; ++u) - { - Eigen::Vector3f xo; - xo << u, v, 1; + for (int v = 0; v < imageSize.height; ++v) { + for (int u = 0; u < imageSize.width; ++u) { + Eigen::Vector3f xo; + xo << u, v, 1; - Eigen::Vector3f uo = R_inv * K_rect_inv * xo; + Eigen::Vector3f uo = R_inv * K_rect_inv * xo; - Eigen::Vector2d p; - spaceToPlane(uo.cast(), p); + Eigen::Vector2d p; + spaceToPlane(uo.cast(), p); - mapX.at(v,u) = p(0); - mapY.at(v,u) = p(1); - } + mapX.at(v, u) = p(0); + mapY.at(v, u) = p(1); } + } - cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false); + cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false); - cv::Mat K_rect_cv; - cv::eigen2cv(K_rect, K_rect_cv); - return K_rect_cv; + cv::Mat K_rect_cv; + cv::eigen2cv(K_rect, K_rect_cv); + return K_rect_cv; } -int -EquidistantCamera::parameterCount(void) const -{ - return 8; -} +int EquidistantCamera::parameterCount(void) const { return 8; } -const EquidistantCamera::Parameters& -EquidistantCamera::getParameters(void) const -{ - return mParameters; +const EquidistantCamera::Parameters& EquidistantCamera::getParameters( + void) const { + return mParameters; } -void -EquidistantCamera::setParameters(const EquidistantCamera::Parameters& parameters) -{ - mParameters = parameters; +void EquidistantCamera::setParameters( + const EquidistantCamera::Parameters& parameters) { + mParameters = parameters; - // Inverse camera projection matrix parameters - m_inv_K11 = 1.0 / mParameters.mu(); - m_inv_K13 = -mParameters.u0() / mParameters.mu(); - m_inv_K22 = 1.0 / mParameters.mv(); - m_inv_K23 = -mParameters.v0() / mParameters.mv(); + // Inverse camera projection matrix parameters + m_inv_K11 = 1.0 / mParameters.mu(); + m_inv_K13 = -mParameters.u0() / mParameters.mu(); + m_inv_K22 = 1.0 / mParameters.mv(); + m_inv_K23 = -mParameters.v0() / mParameters.mv(); } -void -EquidistantCamera::readParameters(const std::vector& parameterVec) -{ - if (parameterVec.size() != parameterCount()) - { - return; - } +void EquidistantCamera::readParameters( + const std::vector& parameterVec) { + if (parameterVec.size() != parameterCount()) { + return; + } - Parameters params = getParameters(); + Parameters params = getParameters(); - params.k2() = parameterVec.at(0); - params.k3() = parameterVec.at(1); - params.k4() = parameterVec.at(2); - params.k5() = parameterVec.at(3); - params.mu() = parameterVec.at(4); - params.mv() = parameterVec.at(5); - params.u0() = parameterVec.at(6); - params.v0() = parameterVec.at(7); + params.k2() = parameterVec.at(0); + params.k3() = parameterVec.at(1); + params.k4() = parameterVec.at(2); + params.k5() = parameterVec.at(3); + params.mu() = parameterVec.at(4); + params.mv() = parameterVec.at(5); + params.u0() = parameterVec.at(6); + params.v0() = parameterVec.at(7); - setParameters(params); + setParameters(params); } -void -EquidistantCamera::writeParameters(std::vector& parameterVec) const -{ - parameterVec.resize(parameterCount()); - parameterVec.at(0) = mParameters.k2(); - parameterVec.at(1) = mParameters.k3(); - parameterVec.at(2) = mParameters.k4(); - parameterVec.at(3) = mParameters.k5(); - parameterVec.at(4) = mParameters.mu(); - parameterVec.at(5) = mParameters.mv(); - parameterVec.at(6) = mParameters.u0(); - parameterVec.at(7) = mParameters.v0(); +void EquidistantCamera::writeParameters( + std::vector& parameterVec) const { + parameterVec.resize(parameterCount()); + parameterVec.at(0) = mParameters.k2(); + parameterVec.at(1) = mParameters.k3(); + parameterVec.at(2) = mParameters.k4(); + parameterVec.at(3) = mParameters.k5(); + parameterVec.at(4) = mParameters.mu(); + parameterVec.at(5) = mParameters.mv(); + parameterVec.at(6) = mParameters.u0(); + parameterVec.at(7) = mParameters.v0(); } -void -EquidistantCamera::writeParametersToYamlFile(const std::string& filename) const -{ - mParameters.writeToYamlFile(filename); +void EquidistantCamera::writeParametersToYamlFile( + const std::string& filename) const { + mParameters.writeToYamlFile(filename); } -std::string -EquidistantCamera::parametersToString(void) const -{ - std::ostringstream oss; - oss << mParameters; +std::string EquidistantCamera::parametersToString(void) const { + std::ostringstream oss; + oss << mParameters; - return oss.str(); + return oss.str(); } -void -EquidistantCamera::fitOddPoly(const std::vector& x, const std::vector& y, - int n, std::vector& coeffs) const -{ - std::vector pows; - for (int i = 1; i <= n; i += 2) - { - pows.push_back(i); - } +void EquidistantCamera::fitOddPoly(const std::vector& x, + const std::vector& y, int n, + std::vector& coeffs) const { + std::vector pows; + for (int i = 1; i <= n; i += 2) { + pows.push_back(i); + } - Eigen::MatrixXd X(x.size(), pows.size()); - Eigen::MatrixXd Y(y.size(), 1); - for (size_t i = 0; i < x.size(); ++i) - { - for (size_t j = 0; j < pows.size(); ++j) - { - X(i,j) = pow(x.at(i), pows.at(j)); - } - Y(i,0) = y.at(i); + Eigen::MatrixXd X(x.size(), pows.size()); + Eigen::MatrixXd Y(y.size(), 1); + for (size_t i = 0; i < x.size(); ++i) { + for (size_t j = 0; j < pows.size(); ++j) { + X(i, j) = pow(x.at(i), pows.at(j)); } - - Eigen::MatrixXd A = (X.transpose() * X).inverse() * X.transpose() * Y; - - coeffs.resize(A.rows()); - for (int i = 0; i < A.rows(); ++i) - { - coeffs.at(i) = A(i,0); - } -} - -void -EquidistantCamera::backprojectSymmetric(const Eigen::Vector2d& p_u, - double& theta, double& phi) const -{ - double tol = 1e-10; - double p_u_norm = p_u.norm(); - - if (p_u_norm < 1e-10) - { - phi = 0.0; - } - else - { - phi = atan2(p_u(1), p_u(0)); + Y(i, 0) = y.at(i); + } + + Eigen::MatrixXd A = (X.transpose() * X).inverse() * X.transpose() * Y; + + coeffs.resize(A.rows()); + for (int i = 0; i < A.rows(); ++i) { + coeffs.at(i) = A(i, 0); + } +} + +void EquidistantCamera::backprojectSymmetric(const Eigen::Vector2d& p_u, + double& theta, double& phi) const { + double tol = 1e-10; + double p_u_norm = p_u.norm(); + + if (p_u_norm < 1e-10) { + phi = 0.0; + } else { + phi = atan2(p_u(1), p_u(0)); + } + + int npow = 9; + if (mParameters.k5() == 0.0) { + npow -= 2; + } + if (mParameters.k4() == 0.0) { + npow -= 2; + } + if (mParameters.k3() == 0.0) { + npow -= 2; + } + if (mParameters.k2() == 0.0) { + npow -= 2; + } + + Eigen::MatrixXd coeffs(npow + 1, 1); + coeffs.setZero(); + coeffs(0) = -p_u_norm; + coeffs(1) = 1.0; + + if (npow >= 3) { + coeffs(3) = mParameters.k2(); + } + if (npow >= 5) { + coeffs(5) = mParameters.k3(); + } + if (npow >= 7) { + coeffs(7) = mParameters.k4(); + } + if (npow >= 9) { + coeffs(9) = mParameters.k5(); + } + + if (npow == 1) { + theta = p_u_norm; + } else { + // Get eigenvalues of companion matrix corresponding to polynomial. + // Eigenvalues correspond to roots of polynomial. + Eigen::MatrixXd A(npow, npow); + A.setZero(); + A.block(1, 0, npow - 1, npow - 1).setIdentity(); + A.col(npow - 1) = -coeffs.block(0, 0, npow, 1) / coeffs(npow); + + Eigen::EigenSolver es(A); + Eigen::MatrixXcd eigval = es.eigenvalues(); + + std::vector thetas; + for (int i = 0; i < eigval.rows(); ++i) { + if (fabs(eigval(i).imag()) > tol) { + continue; + } + + double t = eigval(i).real(); + + if (t < -tol) { + continue; + } else if (t < 0.0) { + t = 0.0; + } + + thetas.push_back(t); } - int npow = 9; - if (mParameters.k5() == 0.0) - { - npow -= 2; - } - if (mParameters.k4() == 0.0) - { - npow -= 2; - } - if (mParameters.k3() == 0.0) - { - npow -= 2; - } - if (mParameters.k2() == 0.0) - { - npow -= 2; - } - - Eigen::MatrixXd coeffs(npow + 1, 1); - coeffs.setZero(); - coeffs(0) = -p_u_norm; - coeffs(1) = 1.0; - - if (npow >= 3) - { - coeffs(3) = mParameters.k2(); - } - if (npow >= 5) - { - coeffs(5) = mParameters.k3(); - } - if (npow >= 7) - { - coeffs(7) = mParameters.k4(); - } - if (npow >= 9) - { - coeffs(9) = mParameters.k5(); - } - - if (npow == 1) - { - theta = p_u_norm; - } - else - { - // Get eigenvalues of companion matrix corresponding to polynomial. - // Eigenvalues correspond to roots of polynomial. - Eigen::MatrixXd A(npow, npow); - A.setZero(); - A.block(1, 0, npow - 1, npow - 1).setIdentity(); - A.col(npow - 1) = - coeffs.block(0, 0, npow, 1) / coeffs(npow); - - Eigen::EigenSolver es(A); - Eigen::MatrixXcd eigval = es.eigenvalues(); - - std::vector thetas; - for (int i = 0; i < eigval.rows(); ++i) - { - if (fabs(eigval(i).imag()) > tol) - { - continue; - } - - double t = eigval(i).real(); - - if (t < -tol) - { - continue; - } - else if (t < 0.0) - { - t = 0.0; - } - - thetas.push_back(t); - } - - if (thetas.empty()) - { - theta = p_u_norm; - } - else - { - theta = *std::min_element(thetas.begin(), thetas.end()); - } + if (thetas.empty()) { + theta = p_u_norm; + } else { + theta = *std::min_element(thetas.begin(), thetas.end()); } + } } -} +} // namespace camodocal diff --git a/camera_models/src/camera_models/PinholeCamera.cc b/camera_models/src/camera_models/PinholeCamera.cc index 98397241..77fdf459 100644 --- a/camera_models/src/camera_models/PinholeCamera.cc +++ b/camera_models/src/camera_models/PinholeCamera.cc @@ -10,420 +10,309 @@ #include "camodocal/gpl/gpl.h" -namespace camodocal -{ +namespace camodocal { PinholeCamera::Parameters::Parameters() - : Camera::Parameters(PINHOLE) - , m_k1(0.0) - , m_k2(0.0) - , m_p1(0.0) - , m_p2(0.0) - , m_fx(0.0) - , m_fy(0.0) - , m_cx(0.0) - , m_cy(0.0) -{ - -} - -PinholeCamera::Parameters::Parameters(const std::string& cameraName, - int w, int h, - double k1, double k2, - double p1, double p2, - double fx, double fy, + : Camera::Parameters(PINHOLE), + m_k1(0.0), + m_k2(0.0), + m_p1(0.0), + m_p2(0.0), + m_fx(0.0), + m_fy(0.0), + m_cx(0.0), + m_cy(0.0) {} + +PinholeCamera::Parameters::Parameters(const std::string& cameraName, int w, + int h, double k1, double k2, double p1, + double p2, double fx, double fy, double cx, double cy) - : Camera::Parameters(PINHOLE, cameraName, w, h) - , m_k1(k1) - , m_k2(k2) - , m_p1(p1) - , m_p2(p2) - , m_fx(fx) - , m_fy(fy) - , m_cx(cx) - , m_cy(cy) -{ -} + : Camera::Parameters(PINHOLE, cameraName, w, h), + m_k1(k1), + m_k2(k2), + m_p1(p1), + m_p2(p2), + m_fx(fx), + m_fy(fy), + m_cx(cx), + m_cy(cy) {} -double& -PinholeCamera::Parameters::k1(void) -{ - return m_k1; -} +double& PinholeCamera::Parameters::k1(void) { return m_k1; } -double& -PinholeCamera::Parameters::k2(void) -{ - return m_k2; -} +double& PinholeCamera::Parameters::k2(void) { return m_k2; } -double& -PinholeCamera::Parameters::p1(void) -{ - return m_p1; -} +double& PinholeCamera::Parameters::p1(void) { return m_p1; } -double& -PinholeCamera::Parameters::p2(void) -{ - return m_p2; -} +double& PinholeCamera::Parameters::p2(void) { return m_p2; } -double& -PinholeCamera::Parameters::fx(void) -{ - return m_fx; -} +double& PinholeCamera::Parameters::fx(void) { return m_fx; } -double& -PinholeCamera::Parameters::fy(void) -{ - return m_fy; -} +double& PinholeCamera::Parameters::fy(void) { return m_fy; } -double& -PinholeCamera::Parameters::cx(void) -{ - return m_cx; -} +double& PinholeCamera::Parameters::cx(void) { return m_cx; } -double& -PinholeCamera::Parameters::cy(void) -{ - return m_cy; -} +double& PinholeCamera::Parameters::cy(void) { return m_cy; } -double -PinholeCamera::Parameters::k1(void) const -{ - return m_k1; -} +double PinholeCamera::Parameters::k1(void) const { return m_k1; } -double -PinholeCamera::Parameters::k2(void) const -{ - return m_k2; -} +double PinholeCamera::Parameters::k2(void) const { return m_k2; } -double -PinholeCamera::Parameters::p1(void) const -{ - return m_p1; -} +double PinholeCamera::Parameters::p1(void) const { return m_p1; } -double -PinholeCamera::Parameters::p2(void) const -{ - return m_p2; -} +double PinholeCamera::Parameters::p2(void) const { return m_p2; } -double -PinholeCamera::Parameters::fx(void) const -{ - return m_fx; -} +double PinholeCamera::Parameters::fx(void) const { return m_fx; } -double -PinholeCamera::Parameters::fy(void) const -{ - return m_fy; -} +double PinholeCamera::Parameters::fy(void) const { return m_fy; } -double -PinholeCamera::Parameters::cx(void) const -{ - return m_cx; -} +double PinholeCamera::Parameters::cx(void) const { return m_cx; } -double -PinholeCamera::Parameters::cy(void) const -{ - return m_cy; -} +double PinholeCamera::Parameters::cy(void) const { return m_cy; } -bool -PinholeCamera::Parameters::readFromYamlFile(const std::string& filename) -{ - cv::FileStorage fs(filename, cv::FileStorage::READ); +bool PinholeCamera::Parameters::readFromYamlFile(const std::string& filename) { + cv::FileStorage fs(filename, cv::FileStorage::READ); - if (!fs.isOpened()) - { - return false; - } + if (!fs.isOpened()) { + return false; + } - if (!fs["model_type"].isNone()) - { - std::string sModelType; - fs["model_type"] >> sModelType; + if (!fs["model_type"].isNone()) { + std::string sModelType; + fs["model_type"] >> sModelType; - if (sModelType.compare("PINHOLE") != 0) - { - return false; - } + if (sModelType.compare("PINHOLE") != 0) { + return false; } - - m_modelType = PINHOLE; - fs["camera_name"] >> m_cameraName; - m_imageWidth = static_cast(fs["image_width"]); - m_imageHeight = static_cast(fs["image_height"]); - - cv::FileNode n = fs["distortion_parameters"]; - m_k1 = static_cast(n["k1"]); - m_k2 = static_cast(n["k2"]); - m_p1 = static_cast(n["p1"]); - m_p2 = static_cast(n["p2"]); - - n = fs["projection_parameters"]; - m_fx = static_cast(n["fx"]); - m_fy = static_cast(n["fy"]); - m_cx = static_cast(n["cx"]); - m_cy = static_cast(n["cy"]); - - return true; + } + + m_modelType = PINHOLE; + fs["camera_name"] >> m_cameraName; + m_imageWidth = static_cast(fs["image_width"]); + m_imageHeight = static_cast(fs["image_height"]); + + cv::FileNode n = fs["distortion_parameters"]; + m_k1 = static_cast(n["k1"]); + m_k2 = static_cast(n["k2"]); + m_p1 = static_cast(n["p1"]); + m_p2 = static_cast(n["p2"]); + + n = fs["projection_parameters"]; + m_fx = static_cast(n["fx"]); + m_fy = static_cast(n["fy"]); + m_cx = static_cast(n["cx"]); + m_cy = static_cast(n["cy"]); + + return true; +} + +void PinholeCamera::Parameters::writeToYamlFile( + const std::string& filename) const { + cv::FileStorage fs(filename, cv::FileStorage::WRITE); + + fs << "model_type" + << "PINHOLE"; + fs << "camera_name" << m_cameraName; + fs << "image_width" << m_imageWidth; + fs << "image_height" << m_imageHeight; + + // radial distortion: k1, k2 + // tangential distortion: p1, p2 + fs << "distortion_parameters"; + fs << "{" + << "k1" << m_k1 << "k2" << m_k2 << "p1" << m_p1 << "p2" << m_p2 << "}"; + + // projection: fx, fy, cx, cy + fs << "projection_parameters"; + fs << "{" + << "fx" << m_fx << "fy" << m_fy << "cx" << m_cx << "cy" << m_cy << "}"; + + fs.release(); +} + +PinholeCamera::Parameters& PinholeCamera::Parameters::operator=( + const PinholeCamera::Parameters& other) { + if (this != &other) { + m_modelType = other.m_modelType; + m_cameraName = other.m_cameraName; + m_imageWidth = other.m_imageWidth; + m_imageHeight = other.m_imageHeight; + m_k1 = other.m_k1; + m_k2 = other.m_k2; + m_p1 = other.m_p1; + m_p2 = other.m_p2; + m_fx = other.m_fx; + m_fy = other.m_fy; + m_cx = other.m_cx; + m_cy = other.m_cy; + } + + return *this; +} + +std::ostream& operator<<(std::ostream& out, + const PinholeCamera::Parameters& params) { + out << "Camera Parameters:" << std::endl; + out << " model_type " + << "PINHOLE" << std::endl; + out << " camera_name " << params.m_cameraName << std::endl; + out << " image_width " << params.m_imageWidth << std::endl; + out << " image_height " << params.m_imageHeight << std::endl; + + // radial distortion: k1, k2 + // tangential distortion: p1, p2 + out << "Distortion Parameters" << std::endl; + out << " k1 " << params.m_k1 << std::endl + << " k2 " << params.m_k2 << std::endl + << " p1 " << params.m_p1 << std::endl + << " p2 " << params.m_p2 << std::endl; + + // projection: fx, fy, cx, cy + out << "Projection Parameters" << std::endl; + out << " fx " << params.m_fx << std::endl + << " fy " << params.m_fy << std::endl + << " cx " << params.m_cx << std::endl + << " cy " << params.m_cy << std::endl; + + return out; } -void -PinholeCamera::Parameters::writeToYamlFile(const std::string& filename) const -{ - cv::FileStorage fs(filename, cv::FileStorage::WRITE); - - fs << "model_type" << "PINHOLE"; - fs << "camera_name" << m_cameraName; - fs << "image_width" << m_imageWidth; - fs << "image_height" << m_imageHeight; - - // radial distortion: k1, k2 - // tangential distortion: p1, p2 - fs << "distortion_parameters"; - fs << "{" << "k1" << m_k1 - << "k2" << m_k2 - << "p1" << m_p1 - << "p2" << m_p2 << "}"; - - // projection: fx, fy, cx, cy - fs << "projection_parameters"; - fs << "{" << "fx" << m_fx - << "fy" << m_fy - << "cx" << m_cx - << "cy" << m_cy << "}"; - - fs.release(); +PinholeCamera::PinholeCamera() + : m_inv_K11(1.0), + m_inv_K13(0.0), + m_inv_K22(1.0), + m_inv_K23(0.0), + m_noDistortion(true) {} + +PinholeCamera::PinholeCamera(const std::string& cameraName, int imageWidth, + int imageHeight, double k1, double k2, double p1, + double p2, double fx, double fy, double cx, + double cy) + : mParameters(cameraName, imageWidth, imageHeight, k1, k2, p1, p2, fx, fy, + cx, cy) { + if ((mParameters.k1() == 0.0) && (mParameters.k2() == 0.0) && + (mParameters.p1() == 0.0) && (mParameters.p2() == 0.0)) { + m_noDistortion = true; + } else { + m_noDistortion = false; + } + + // Inverse camera projection matrix parameters + m_inv_K11 = 1.0 / mParameters.fx(); + m_inv_K13 = -mParameters.cx() / mParameters.fx(); + m_inv_K22 = 1.0 / mParameters.fy(); + m_inv_K23 = -mParameters.cy() / mParameters.fy(); } -PinholeCamera::Parameters& -PinholeCamera::Parameters::operator=(const PinholeCamera::Parameters& other) -{ - if (this != &other) - { - m_modelType = other.m_modelType; - m_cameraName = other.m_cameraName; - m_imageWidth = other.m_imageWidth; - m_imageHeight = other.m_imageHeight; - m_k1 = other.m_k1; - m_k2 = other.m_k2; - m_p1 = other.m_p1; - m_p2 = other.m_p2; - m_fx = other.m_fx; - m_fy = other.m_fy; - m_cx = other.m_cx; - m_cy = other.m_cy; - } +PinholeCamera::PinholeCamera(const PinholeCamera::Parameters& params) + : mParameters(params) { + if ((mParameters.k1() == 0.0) && (mParameters.k2() == 0.0) && + (mParameters.p1() == 0.0) && (mParameters.p2() == 0.0)) { + m_noDistortion = true; + } else { + m_noDistortion = false; + } - return *this; + // Inverse camera projection matrix parameters + m_inv_K11 = 1.0 / mParameters.fx(); + m_inv_K13 = -mParameters.cx() / mParameters.fx(); + m_inv_K22 = 1.0 / mParameters.fy(); + m_inv_K23 = -mParameters.cy() / mParameters.fy(); } -std::ostream& -operator<< (std::ostream& out, const PinholeCamera::Parameters& params) -{ - out << "Camera Parameters:" << std::endl; - out << " model_type " << "PINHOLE" << std::endl; - out << " camera_name " << params.m_cameraName << std::endl; - out << " image_width " << params.m_imageWidth << std::endl; - out << " image_height " << params.m_imageHeight << std::endl; - - // radial distortion: k1, k2 - // tangential distortion: p1, p2 - out << "Distortion Parameters" << std::endl; - out << " k1 " << params.m_k1 << std::endl - << " k2 " << params.m_k2 << std::endl - << " p1 " << params.m_p1 << std::endl - << " p2 " << params.m_p2 << std::endl; - - // projection: fx, fy, cx, cy - out << "Projection Parameters" << std::endl; - out << " fx " << params.m_fx << std::endl - << " fy " << params.m_fy << std::endl - << " cx " << params.m_cx << std::endl - << " cy " << params.m_cy << std::endl; - - return out; +Camera::ModelType PinholeCamera::modelType(void) const { + return mParameters.modelType(); } -PinholeCamera::PinholeCamera() - : m_inv_K11(1.0) - , m_inv_K13(0.0) - , m_inv_K22(1.0) - , m_inv_K23(0.0) - , m_noDistortion(true) -{ - +const std::string& PinholeCamera::cameraName(void) const { + return mParameters.cameraName(); } -PinholeCamera::PinholeCamera(const std::string& cameraName, - int imageWidth, int imageHeight, - double k1, double k2, double p1, double p2, - double fx, double fy, double cx, double cy) - : mParameters(cameraName, imageWidth, imageHeight, - k1, k2, p1, p2, fx, fy, cx, cy) -{ - if ((mParameters.k1() == 0.0) && - (mParameters.k2() == 0.0) && - (mParameters.p1() == 0.0) && - (mParameters.p2() == 0.0)) - { - m_noDistortion = true; - } - else - { - m_noDistortion = false; - } +int PinholeCamera::imageWidth(void) const { return mParameters.imageWidth(); } - // Inverse camera projection matrix parameters - m_inv_K11 = 1.0 / mParameters.fx(); - m_inv_K13 = -mParameters.cx() / mParameters.fx(); - m_inv_K22 = 1.0 / mParameters.fy(); - m_inv_K23 = -mParameters.cy() / mParameters.fy(); -} +int PinholeCamera::imageHeight(void) const { return mParameters.imageHeight(); } -PinholeCamera::PinholeCamera(const PinholeCamera::Parameters& params) - : mParameters(params) -{ - if ((mParameters.k1() == 0.0) && - (mParameters.k2() == 0.0) && - (mParameters.p1() == 0.0) && - (mParameters.p2() == 0.0)) - { - m_noDistortion = true; - } - else - { - m_noDistortion = false; - } - - // Inverse camera projection matrix parameters - m_inv_K11 = 1.0 / mParameters.fx(); - m_inv_K13 = -mParameters.cx() / mParameters.fx(); - m_inv_K22 = 1.0 / mParameters.fy(); - m_inv_K23 = -mParameters.cy() / mParameters.fy(); -} +void PinholeCamera::estimateIntrinsics( + const cv::Size& boardSize, + const std::vector >& objectPoints, + const std::vector >& imagePoints) { + // Z. Zhang, A Flexible New Technique for Camera Calibration, PAMI 2000 -Camera::ModelType -PinholeCamera::modelType(void) const -{ - return mParameters.modelType(); -} - -const std::string& -PinholeCamera::cameraName(void) const -{ - return mParameters.cameraName(); -} + Parameters params = getParameters(); -int -PinholeCamera::imageWidth(void) const -{ - return mParameters.imageWidth(); -} + params.k1() = 0.0; + params.k2() = 0.0; + params.p1() = 0.0; + params.p2() = 0.0; -int -PinholeCamera::imageHeight(void) const -{ - return mParameters.imageHeight(); -} + double cx = params.imageWidth() / 2.0; + double cy = params.imageHeight() / 2.0; + params.cx() = cx; + params.cy() = cy; -void -PinholeCamera::estimateIntrinsics(const cv::Size& boardSize, - const std::vector< std::vector >& objectPoints, - const std::vector< std::vector >& imagePoints) -{ - // Z. Zhang, A Flexible New Technique for Camera Calibration, PAMI 2000 + size_t nImages = imagePoints.size(); - Parameters params = getParameters(); + cv::Mat A(nImages * 2, 2, CV_64F); + cv::Mat b(nImages * 2, 1, CV_64F); - params.k1() = 0.0; - params.k2() = 0.0; - params.p1() = 0.0; - params.p2() = 0.0; + for (size_t i = 0; i < nImages; ++i) { + const std::vector& oPoints = objectPoints.at(i); - double cx = params.imageWidth() / 2.0; - double cy = params.imageHeight() / 2.0; - params.cx() = cx; - params.cy() = cy; + std::vector M(oPoints.size()); + for (size_t j = 0; j < M.size(); ++j) { + M.at(j) = cv::Point2f(oPoints.at(j).x, oPoints.at(j).y); + } - size_t nImages = imagePoints.size(); + cv::Mat H = cv::findHomography(M, imagePoints.at(i)); + + H.at(0, 0) -= H.at(2, 0) * cx; + H.at(0, 1) -= H.at(2, 1) * cx; + H.at(0, 2) -= H.at(2, 2) * cx; + H.at(1, 0) -= H.at(2, 0) * cy; + H.at(1, 1) -= H.at(2, 1) * cy; + H.at(1, 2) -= H.at(2, 2) * cy; + + double h[3], v[3], d1[3], d2[3]; + double n[4] = {0, 0, 0, 0}; + + for (int j = 0; j < 3; ++j) { + double t0 = H.at(j, 0); + double t1 = H.at(j, 1); + h[j] = t0; + v[j] = t1; + d1[j] = (t0 + t1) * 0.5; + d2[j] = (t0 - t1) * 0.5; + n[0] += t0 * t0; + n[1] += t1 * t1; + n[2] += d1[j] * d1[j]; + n[3] += d2[j] * d2[j]; + } - cv::Mat A(nImages * 2, 2, CV_64F); - cv::Mat b(nImages * 2, 1, CV_64F); + for (int j = 0; j < 4; ++j) { + n[j] = 1.0 / sqrt(n[j]); + } - for (size_t i = 0; i < nImages; ++i) - { - const std::vector& oPoints = objectPoints.at(i); - - std::vector M(oPoints.size()); - for (size_t j = 0; j < M.size(); ++j) - { - M.at(j) = cv::Point2f(oPoints.at(j).x, oPoints.at(j).y); - } - - cv::Mat H = cv::findHomography(M, imagePoints.at(i)); - - H.at(0,0) -= H.at(2,0) * cx; - H.at(0,1) -= H.at(2,1) * cx; - H.at(0,2) -= H.at(2,2) * cx; - H.at(1,0) -= H.at(2,0) * cy; - H.at(1,1) -= H.at(2,1) * cy; - H.at(1,2) -= H.at(2,2) * cy; - - double h[3], v[3], d1[3], d2[3]; - double n[4] = {0,0,0,0}; - - for (int j = 0; j < 3; ++j) - { - double t0 = H.at(j,0); - double t1 = H.at(j,1); - h[j] = t0; v[j] = t1; - d1[j] = (t0 + t1) * 0.5; - d2[j] = (t0 - t1) * 0.5; - n[0] += t0 * t0; n[1] += t1 * t1; - n[2] += d1[j] * d1[j]; n[3] += d2[j] * d2[j]; - } - - for (int j = 0; j < 4; ++j) - { - n[j] = 1.0 / sqrt(n[j]); - } - - for (int j = 0; j < 3; ++j) - { - h[j] *= n[0]; v[j] *= n[1]; - d1[j] *= n[2]; d2[j] *= n[3]; - } - - A.at(i * 2, 0) = h[0] * v[0]; - A.at(i * 2, 1) = h[1] * v[1]; - A.at(i * 2 + 1, 0) = d1[0] * d2[0]; - A.at(i * 2 + 1, 1) = d1[1] * d2[1]; - b.at(i * 2, 0) = -h[2] * v[2]; - b.at(i * 2 + 1, 0) = -d1[2] * d2[2]; + for (int j = 0; j < 3; ++j) { + h[j] *= n[0]; + v[j] *= n[1]; + d1[j] *= n[2]; + d2[j] *= n[3]; } - cv::Mat f(2, 1, CV_64F); - cv::solve(A, b, f, cv::DECOMP_NORMAL | cv::DECOMP_LU); + A.at(i * 2, 0) = h[0] * v[0]; + A.at(i * 2, 1) = h[1] * v[1]; + A.at(i * 2 + 1, 0) = d1[0] * d2[0]; + A.at(i * 2 + 1, 1) = d1[1] * d2[1]; + b.at(i * 2, 0) = -h[2] * v[2]; + b.at(i * 2 + 1, 0) = -d1[2] * d2[2]; + } + + cv::Mat f(2, 1, CV_64F); + cv::solve(A, b, f, cv::DECOMP_NORMAL | cv::DECOMP_LU); - params.fx() = sqrt(fabs(1.0 / f.at(0))); - params.fy() = sqrt(fabs(1.0 / f.at(1))); + params.fx() = sqrt(fabs(1.0 / f.at(0))); + params.fy() = sqrt(fabs(1.0 / f.at(1))); - setParameters(params); + setParameters(params); } /** @@ -432,12 +321,11 @@ PinholeCamera::estimateIntrinsics(const cv::Size& boardSize, * \param p image coordinates * \param P coordinates of the point on the sphere */ -void -PinholeCamera::liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const -{ - liftProjective(p, P); +void PinholeCamera::liftSphere(const Eigen::Vector2d& p, + Eigen::Vector3d& P) const { + liftProjective(p, P); - P.normalize(); + P.normalize(); } /** @@ -446,99 +334,87 @@ PinholeCamera::liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const * \param p image coordinates * \param P coordinates of the projective ray */ -void -PinholeCamera::liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const -{ - double mx_d, my_d,mx2_d, mxy_d, my2_d, mx_u, my_u; - double rho2_d, rho4_d, radDist_d, Dx_d, Dy_d, inv_denom_d; - //double lambda; - - // Lift points to normalised plane - mx_d = m_inv_K11 * p(0) + m_inv_K13; - my_d = m_inv_K22 * p(1) + m_inv_K23; - - if (m_noDistortion) - { - mx_u = mx_d; - my_u = my_d; - } - else - { - if (0) - { - double k1 = mParameters.k1(); - double k2 = mParameters.k2(); - double p1 = mParameters.p1(); - double p2 = mParameters.p2(); - - // Apply inverse distortion model - // proposed by Heikkila - mx2_d = mx_d*mx_d; - my2_d = my_d*my_d; - mxy_d = mx_d*my_d; - rho2_d = mx2_d+my2_d; - rho4_d = rho2_d*rho2_d; - radDist_d = k1*rho2_d+k2*rho4_d; - Dx_d = mx_d*radDist_d + p2*(rho2_d+2*mx2_d) + 2*p1*mxy_d; - Dy_d = my_d*radDist_d + p1*(rho2_d+2*my2_d) + 2*p2*mxy_d; - inv_denom_d = 1/(1+4*k1*rho2_d+6*k2*rho4_d+8*p1*my_d+8*p2*mx_d); - - mx_u = mx_d - inv_denom_d*Dx_d; - my_u = my_d - inv_denom_d*Dy_d; - } - else - { - // Recursive distortion model - int n = 8; - Eigen::Vector2d d_u; - distortion(Eigen::Vector2d(mx_d, my_d), d_u); - // Approximate value - mx_u = mx_d - d_u(0); - my_u = my_d - d_u(1); - - for (int i = 1; i < n; ++i) - { - distortion(Eigen::Vector2d(mx_u, my_u), d_u); - mx_u = mx_d - d_u(0); - my_u = my_d - d_u(1); - } - } +void PinholeCamera::liftProjective(const Eigen::Vector2d& p, + Eigen::Vector3d& P) const { + double mx_d, my_d, mx2_d, mxy_d, my2_d, mx_u, my_u; + double rho2_d, rho4_d, radDist_d, Dx_d, Dy_d, inv_denom_d; + // double lambda; + + // Lift points to normalised plane + mx_d = m_inv_K11 * p(0) + m_inv_K13; + my_d = m_inv_K22 * p(1) + m_inv_K23; + + if (m_noDistortion) { + mx_u = mx_d; + my_u = my_d; + } else { + if (0) { + double k1 = mParameters.k1(); + double k2 = mParameters.k2(); + double p1 = mParameters.p1(); + double p2 = mParameters.p2(); + + // Apply inverse distortion model + // proposed by Heikkila + mx2_d = mx_d * mx_d; + my2_d = my_d * my_d; + mxy_d = mx_d * my_d; + rho2_d = mx2_d + my2_d; + rho4_d = rho2_d * rho2_d; + radDist_d = k1 * rho2_d + k2 * rho4_d; + Dx_d = mx_d * radDist_d + p2 * (rho2_d + 2 * mx2_d) + 2 * p1 * mxy_d; + Dy_d = my_d * radDist_d + p1 * (rho2_d + 2 * my2_d) + 2 * p2 * mxy_d; + inv_denom_d = 1 / (1 + 4 * k1 * rho2_d + 6 * k2 * rho4_d + 8 * p1 * my_d + + 8 * p2 * mx_d); + + mx_u = mx_d - inv_denom_d * Dx_d; + my_u = my_d - inv_denom_d * Dy_d; + } else { + // Recursive distortion model + int n = 8; + Eigen::Vector2d d_u; + distortion(Eigen::Vector2d(mx_d, my_d), d_u); + // Approximate value + mx_u = mx_d - d_u(0); + my_u = my_d - d_u(1); + + for (int i = 1; i < n; ++i) { + distortion(Eigen::Vector2d(mx_u, my_u), d_u); + mx_u = mx_d - d_u(0); + my_u = my_d - d_u(1); + } } + } - // Obtain a projective ray - P << mx_u, my_u, 1.0; + // Obtain a projective ray + P << mx_u, my_u, 1.0; } - /** * \brief Project a 3D point (\a x,\a y,\a z) to the image plane in (\a u,\a v) * * \param P 3D point coordinates * \param p return value, contains the image point coordinates */ -void -PinholeCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const -{ - Eigen::Vector2d p_u, p_d; +void PinholeCamera::spaceToPlane(const Eigen::Vector3d& P, + Eigen::Vector2d& p) const { + Eigen::Vector2d p_u, p_d; - // Project points to the normalised plane - p_u << P(0) / P(2), P(1) / P(2); + // Project points to the normalised plane + p_u << P(0) / P(2), P(1) / P(2); - if (m_noDistortion) - { - p_d = p_u; - } - else - { - // Apply distortion - Eigen::Vector2d d_u; - distortion(p_u, d_u); - p_d = p_u + d_u; - } + if (m_noDistortion) { + p_d = p_u; + } else { + // Apply distortion + Eigen::Vector2d d_u; + distortion(p_u, d_u); + p_d = p_u + d_u; + } - // Apply generalised projection matrix - p << mParameters.fx() * p_d(0) + mParameters.cx(), - mParameters.fy() * p_d(1) + mParameters.cy(); + // Apply generalised projection matrix + p << mParameters.fx() * p_d(0) + mParameters.cx(), + mParameters.fy() * p_d(1) + mParameters.cy(); } #if 0 @@ -614,26 +490,22 @@ PinholeCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, * \param p_u 2D point coordinates * \return image point coordinates */ -void -PinholeCamera::undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const -{ - Eigen::Vector2d p_d; +void PinholeCamera::undistToPlane(const Eigen::Vector2d& p_u, + Eigen::Vector2d& p) const { + Eigen::Vector2d p_d; - if (m_noDistortion) - { - p_d = p_u; - } - else - { - // Apply distortion - Eigen::Vector2d d_u; - distortion(p_u, d_u); - p_d = p_u + d_u; - } + if (m_noDistortion) { + p_d = p_u; + } else { + // Apply distortion + Eigen::Vector2d d_u; + distortion(p_u, d_u); + p_d = p_u + d_u; + } - // Apply generalised projection matrix - p << mParameters.fx() * p_d(0) + mParameters.cx(), - mParameters.fy() * p_d(1) + mParameters.cy(); + // Apply generalised projection matrix + p << mParameters.fx() * p_d(0) + mParameters.cx(), + mParameters.fy() * p_d(1) + mParameters.cy(); } /** @@ -642,23 +514,22 @@ PinholeCamera::undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) con * \param p_u undistorted coordinates of point on the normalised plane * \return to obtain the distorted point: p_d = p_u + d_u */ -void -PinholeCamera::distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const -{ - double k1 = mParameters.k1(); - double k2 = mParameters.k2(); - double p1 = mParameters.p1(); - double p2 = mParameters.p2(); - - double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u; - - mx2_u = p_u(0) * p_u(0); - my2_u = p_u(1) * p_u(1); - mxy_u = p_u(0) * p_u(1); - rho2_u = mx2_u + my2_u; - rad_dist_u = k1 * rho2_u + k2 * rho2_u * rho2_u; - d_u << p_u(0) * rad_dist_u + 2.0 * p1 * mxy_u + p2 * (rho2_u + 2.0 * mx2_u), - p_u(1) * rad_dist_u + 2.0 * p2 * mxy_u + p1 * (rho2_u + 2.0 * my2_u); +void PinholeCamera::distortion(const Eigen::Vector2d& p_u, + Eigen::Vector2d& d_u) const { + double k1 = mParameters.k1(); + double k2 = mParameters.k2(); + double p1 = mParameters.p1(); + double p2 = mParameters.p2(); + + double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u; + + mx2_u = p_u(0) * p_u(0); + my2_u = p_u(1) * p_u(1); + mxy_u = p_u(0) * p_u(1); + rho2_u = mx2_u + my2_u; + rad_dist_u = k1 * rho2_u + k2 * rho2_u * rho2_u; + d_u << p_u(0) * rad_dist_u + 2.0 * p1 * mxy_u + p2 * (rho2_u + 2.0 * mx2_u), + p_u(1) * rad_dist_u + 2.0 * p2 * mxy_u + p1 * (rho2_u + 2.0 * my2_u); } /** @@ -668,214 +539,179 @@ PinholeCamera::distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) cons * \param p_u undistorted coordinates of point on the normalised plane * \return to obtain the distorted point: p_d = p_u + d_u */ -void -PinholeCamera::distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u, - Eigen::Matrix2d& J) const -{ - double k1 = mParameters.k1(); - double k2 = mParameters.k2(); - double p1 = mParameters.p1(); - double p2 = mParameters.p2(); - - double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u; - - mx2_u = p_u(0) * p_u(0); - my2_u = p_u(1) * p_u(1); - mxy_u = p_u(0) * p_u(1); - rho2_u = mx2_u + my2_u; - rad_dist_u = k1 * rho2_u + k2 * rho2_u * rho2_u; - d_u << p_u(0) * rad_dist_u + 2.0 * p1 * mxy_u + p2 * (rho2_u + 2.0 * mx2_u), - p_u(1) * rad_dist_u + 2.0 * p2 * mxy_u + p1 * (rho2_u + 2.0 * my2_u); - - double dxdmx = 1.0 + rad_dist_u + k1 * 2.0 * mx2_u + k2 * rho2_u * 4.0 * mx2_u + 2.0 * p1 * p_u(1) + 6.0 * p2 * p_u(0); - double dydmx = k1 * 2.0 * p_u(0) * p_u(1) + k2 * 4.0 * rho2_u * p_u(0) * p_u(1) + p1 * 2.0 * p_u(0) + 2.0 * p2 * p_u(1); - double dxdmy = dydmx; - double dydmy = 1.0 + rad_dist_u + k1 * 2.0 * my2_u + k2 * rho2_u * 4.0 * my2_u + 6.0 * p1 * p_u(1) + 2.0 * p2 * p_u(0); - - J << dxdmx, dxdmy, - dydmx, dydmy; -} - -void -PinholeCamera::initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale) const -{ - cv::Size imageSize(mParameters.imageWidth(), mParameters.imageHeight()); - - cv::Mat mapX = cv::Mat::zeros(imageSize, CV_32F); - cv::Mat mapY = cv::Mat::zeros(imageSize, CV_32F); - - for (int v = 0; v < imageSize.height; ++v) - { - for (int u = 0; u < imageSize.width; ++u) - { - double mx_u = m_inv_K11 / fScale * u + m_inv_K13 / fScale; - double my_u = m_inv_K22 / fScale * v + m_inv_K23 / fScale; - - Eigen::Vector3d P; - P << mx_u, my_u, 1.0; - - Eigen::Vector2d p; - spaceToPlane(P, p); - - mapX.at(v,u) = p(0); - mapY.at(v,u) = p(1); - } +void PinholeCamera::distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u, + Eigen::Matrix2d& J) const { + double k1 = mParameters.k1(); + double k2 = mParameters.k2(); + double p1 = mParameters.p1(); + double p2 = mParameters.p2(); + + double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u; + + mx2_u = p_u(0) * p_u(0); + my2_u = p_u(1) * p_u(1); + mxy_u = p_u(0) * p_u(1); + rho2_u = mx2_u + my2_u; + rad_dist_u = k1 * rho2_u + k2 * rho2_u * rho2_u; + d_u << p_u(0) * rad_dist_u + 2.0 * p1 * mxy_u + p2 * (rho2_u + 2.0 * mx2_u), + p_u(1) * rad_dist_u + 2.0 * p2 * mxy_u + p1 * (rho2_u + 2.0 * my2_u); + + double dxdmx = 1.0 + rad_dist_u + k1 * 2.0 * mx2_u + + k2 * rho2_u * 4.0 * mx2_u + 2.0 * p1 * p_u(1) + + 6.0 * p2 * p_u(0); + double dydmx = k1 * 2.0 * p_u(0) * p_u(1) + + k2 * 4.0 * rho2_u * p_u(0) * p_u(1) + p1 * 2.0 * p_u(0) + + 2.0 * p2 * p_u(1); + double dxdmy = dydmx; + double dydmy = 1.0 + rad_dist_u + k1 * 2.0 * my2_u + + k2 * rho2_u * 4.0 * my2_u + 6.0 * p1 * p_u(1) + + 2.0 * p2 * p_u(0); + + J << dxdmx, dxdmy, dydmx, dydmy; +} + +void PinholeCamera::initUndistortMap(cv::Mat& map1, cv::Mat& map2, + double fScale) const { + cv::Size imageSize(mParameters.imageWidth(), mParameters.imageHeight()); + + cv::Mat mapX = cv::Mat::zeros(imageSize, CV_32F); + cv::Mat mapY = cv::Mat::zeros(imageSize, CV_32F); + + for (int v = 0; v < imageSize.height; ++v) { + for (int u = 0; u < imageSize.width; ++u) { + double mx_u = m_inv_K11 / fScale * u + m_inv_K13 / fScale; + double my_u = m_inv_K22 / fScale * v + m_inv_K23 / fScale; + + Eigen::Vector3d P; + P << mx_u, my_u, 1.0; + + Eigen::Vector2d p; + spaceToPlane(P, p); + + mapX.at(v, u) = p(0); + mapY.at(v, u) = p(1); } + } - cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false); + cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false); } -cv::Mat -PinholeCamera::initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, - float fx, float fy, - cv::Size imageSize, - float cx, float cy, - cv::Mat rmat) const -{ - if (imageSize == cv::Size(0, 0)) - { - imageSize = cv::Size(mParameters.imageWidth(), mParameters.imageHeight()); - } +cv::Mat PinholeCamera::initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, + float fx, float fy, + cv::Size imageSize, float cx, + float cy, cv::Mat rmat) const { + if (imageSize == cv::Size(0, 0)) { + imageSize = cv::Size(mParameters.imageWidth(), mParameters.imageHeight()); + } - cv::Mat mapX = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F); - cv::Mat mapY = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F); + cv::Mat mapX = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F); + cv::Mat mapY = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F); - Eigen::Matrix3f R, R_inv; - cv::cv2eigen(rmat, R); - R_inv = R.inverse(); + Eigen::Matrix3f R, R_inv; + cv::cv2eigen(rmat, R); + R_inv = R.inverse(); - // assume no skew - Eigen::Matrix3f K_rect; + // assume no skew + Eigen::Matrix3f K_rect; - if (cx == -1.0f || cy == -1.0f) - { - K_rect << fx, 0, imageSize.width / 2, - 0, fy, imageSize.height / 2, - 0, 0, 1; - } - else - { - K_rect << fx, 0, cx, - 0, fy, cy, - 0, 0, 1; - } + if (cx == -1.0f || cy == -1.0f) { + K_rect << fx, 0, imageSize.width / 2, 0, fy, imageSize.height / 2, 0, 0, 1; + } else { + K_rect << fx, 0, cx, 0, fy, cy, 0, 0, 1; + } - if (fx == -1.0f || fy == -1.0f) - { - K_rect(0,0) = mParameters.fx(); - K_rect(1,1) = mParameters.fy(); - } + if (fx == -1.0f || fy == -1.0f) { + K_rect(0, 0) = mParameters.fx(); + K_rect(1, 1) = mParameters.fy(); + } - Eigen::Matrix3f K_rect_inv = K_rect.inverse(); + Eigen::Matrix3f K_rect_inv = K_rect.inverse(); - for (int v = 0; v < imageSize.height; ++v) - { - for (int u = 0; u < imageSize.width; ++u) - { - Eigen::Vector3f xo; - xo << u, v, 1; + for (int v = 0; v < imageSize.height; ++v) { + for (int u = 0; u < imageSize.width; ++u) { + Eigen::Vector3f xo; + xo << u, v, 1; - Eigen::Vector3f uo = R_inv * K_rect_inv * xo; + Eigen::Vector3f uo = R_inv * K_rect_inv * xo; - Eigen::Vector2d p; - spaceToPlane(uo.cast(), p); + Eigen::Vector2d p; + spaceToPlane(uo.cast(), p); - mapX.at(v,u) = p(0); - mapY.at(v,u) = p(1); - } + mapX.at(v, u) = p(0); + mapY.at(v, u) = p(1); } + } - cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false); + cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false); - cv::Mat K_rect_cv; - cv::eigen2cv(K_rect, K_rect_cv); - return K_rect_cv; + cv::Mat K_rect_cv; + cv::eigen2cv(K_rect, K_rect_cv); + return K_rect_cv; } -int -PinholeCamera::parameterCount(void) const -{ - return 8; -} +int PinholeCamera::parameterCount(void) const { return 8; } -const PinholeCamera::Parameters& -PinholeCamera::getParameters(void) const -{ - return mParameters; +const PinholeCamera::Parameters& PinholeCamera::getParameters(void) const { + return mParameters; } -void -PinholeCamera::setParameters(const PinholeCamera::Parameters& parameters) -{ - mParameters = parameters; +void PinholeCamera::setParameters(const PinholeCamera::Parameters& parameters) { + mParameters = parameters; - if ((mParameters.k1() == 0.0) && - (mParameters.k2() == 0.0) && - (mParameters.p1() == 0.0) && - (mParameters.p2() == 0.0)) - { - m_noDistortion = true; - } - else - { - m_noDistortion = false; - } + if ((mParameters.k1() == 0.0) && (mParameters.k2() == 0.0) && + (mParameters.p1() == 0.0) && (mParameters.p2() == 0.0)) { + m_noDistortion = true; + } else { + m_noDistortion = false; + } - m_inv_K11 = 1.0 / mParameters.fx(); - m_inv_K13 = -mParameters.cx() / mParameters.fx(); - m_inv_K22 = 1.0 / mParameters.fy(); - m_inv_K23 = -mParameters.cy() / mParameters.fy(); + m_inv_K11 = 1.0 / mParameters.fx(); + m_inv_K13 = -mParameters.cx() / mParameters.fx(); + m_inv_K22 = 1.0 / mParameters.fy(); + m_inv_K23 = -mParameters.cy() / mParameters.fy(); } -void -PinholeCamera::readParameters(const std::vector& parameterVec) -{ - if ((int)parameterVec.size() != parameterCount()) - { - return; - } +void PinholeCamera::readParameters(const std::vector& parameterVec) { + if ((int)parameterVec.size() != parameterCount()) { + return; + } - Parameters params = getParameters(); + Parameters params = getParameters(); - params.k1() = parameterVec.at(0); - params.k2() = parameterVec.at(1); - params.p1() = parameterVec.at(2); - params.p2() = parameterVec.at(3); - params.fx() = parameterVec.at(4); - params.fy() = parameterVec.at(5); - params.cx() = parameterVec.at(6); - params.cy() = parameterVec.at(7); + params.k1() = parameterVec.at(0); + params.k2() = parameterVec.at(1); + params.p1() = parameterVec.at(2); + params.p2() = parameterVec.at(3); + params.fx() = parameterVec.at(4); + params.fy() = parameterVec.at(5); + params.cx() = parameterVec.at(6); + params.cy() = parameterVec.at(7); - setParameters(params); + setParameters(params); } -void -PinholeCamera::writeParameters(std::vector& parameterVec) const -{ - parameterVec.resize(parameterCount()); - parameterVec.at(0) = mParameters.k1(); - parameterVec.at(1) = mParameters.k2(); - parameterVec.at(2) = mParameters.p1(); - parameterVec.at(3) = mParameters.p2(); - parameterVec.at(4) = mParameters.fx(); - parameterVec.at(5) = mParameters.fy(); - parameterVec.at(6) = mParameters.cx(); - parameterVec.at(7) = mParameters.cy(); +void PinholeCamera::writeParameters(std::vector& parameterVec) const { + parameterVec.resize(parameterCount()); + parameterVec.at(0) = mParameters.k1(); + parameterVec.at(1) = mParameters.k2(); + parameterVec.at(2) = mParameters.p1(); + parameterVec.at(3) = mParameters.p2(); + parameterVec.at(4) = mParameters.fx(); + parameterVec.at(5) = mParameters.fy(); + parameterVec.at(6) = mParameters.cx(); + parameterVec.at(7) = mParameters.cy(); } -void -PinholeCamera::writeParametersToYamlFile(const std::string& filename) const -{ - mParameters.writeToYamlFile(filename); +void PinholeCamera::writeParametersToYamlFile( + const std::string& filename) const { + mParameters.writeToYamlFile(filename); } -std::string -PinholeCamera::parametersToString(void) const -{ - std::ostringstream oss; - oss << mParameters; +std::string PinholeCamera::parametersToString(void) const { + std::ostringstream oss; + oss << mParameters; - return oss.str(); + return oss.str(); } -} +} // namespace camodocal diff --git a/camera_models/src/camera_models/PinholeFullCamera.cc b/camera_models/src/camera_models/PinholeFullCamera.cc index 5365c064..ac39af10 100644 --- a/camera_models/src/camera_models/PinholeFullCamera.cc +++ b/camera_models/src/camera_models/PinholeFullCamera.cc @@ -10,505 +10,355 @@ #include "camodocal/gpl/gpl.h" -namespace camodocal -{ - -PinholeFullCamera::Parameters::Parameters( ) -: Camera::Parameters( PINHOLE_FULL ) -, m_k1( 0.0 ) -, m_k2( 0.0 ) -, m_k3( 0.0 ) -, m_k4( 0.0 ) -, m_k5( 0.0 ) -, m_k6( 0.0 ) -, m_p1( 0.0 ) -, m_p2( 0.0 ) -, m_fx( 0.0 ) -, m_fy( 0.0 ) -, m_cx( 0.0 ) -, m_cy( 0.0 ) -{ -} +namespace camodocal { -PinholeFullCamera::Parameters::Parameters( const std::string& cameraName, // - int w, - int h, - double k1, - double k2, - double k3, - double k4, - double k5, - double k6, - double p1, - double p2, - double fx, - double fy, - double cx, - double cy ) -: Camera::Parameters( PINHOLE_FULL, cameraName, w, h ) -, m_k1( k1 ) -, m_k2( k2 ) -, m_k3( k3 ) -, m_k4( k4 ) -, m_k5( k5 ) -, m_k6( k6 ) -, m_p1( p1 ) -, m_p2( p2 ) -, m_fx( fx ) -, m_fy( fy ) -, m_cx( cx ) -, m_cy( cy ) -{ -} +PinholeFullCamera::Parameters::Parameters() + : Camera::Parameters(PINHOLE_FULL), + m_k1(0.0), + m_k2(0.0), + m_k3(0.0), + m_k4(0.0), + m_k5(0.0), + m_k6(0.0), + m_p1(0.0), + m_p2(0.0), + m_fx(0.0), + m_fy(0.0), + m_cx(0.0), + m_cy(0.0) {} -double& -PinholeFullCamera::Parameters::k1( void ) -{ - return m_k1; -} +PinholeFullCamera::Parameters::Parameters(const std::string& cameraName, // + int w, int h, double k1, double k2, + double k3, double k4, double k5, + double k6, double p1, double p2, + double fx, double fy, double cx, + double cy) + : Camera::Parameters(PINHOLE_FULL, cameraName, w, h), + m_k1(k1), + m_k2(k2), + m_k3(k3), + m_k4(k4), + m_k5(k5), + m_k6(k6), + m_p1(p1), + m_p2(p2), + m_fx(fx), + m_fy(fy), + m_cx(cx), + m_cy(cy) {} -double& -PinholeFullCamera::Parameters::k2( void ) -{ - return m_k2; -} +double& PinholeFullCamera::Parameters::k1(void) { return m_k1; } -double& -PinholeFullCamera::Parameters::k3( ) -{ - return m_k3; -} +double& PinholeFullCamera::Parameters::k2(void) { return m_k2; } -double& -PinholeFullCamera::Parameters::k4( ) -{ - return m_k4; -} +double& PinholeFullCamera::Parameters::k3() { return m_k3; } -double& -PinholeFullCamera::Parameters::k5( ) -{ - return m_k5; -} +double& PinholeFullCamera::Parameters::k4() { return m_k4; } -double& -PinholeFullCamera::Parameters::k6( ) -{ - return m_k6; -} +double& PinholeFullCamera::Parameters::k5() { return m_k5; } -double& -PinholeFullCamera::Parameters::p1( void ) -{ - return m_p1; -} +double& PinholeFullCamera::Parameters::k6() { return m_k6; } -double& -PinholeFullCamera::Parameters::p2( void ) -{ - return m_p2; -} +double& PinholeFullCamera::Parameters::p1(void) { return m_p1; } -double& -PinholeFullCamera::Parameters::fx( void ) -{ - return m_fx; -} +double& PinholeFullCamera::Parameters::p2(void) { return m_p2; } -double& -PinholeFullCamera::Parameters::fy( void ) -{ - return m_fy; -} - -double& -PinholeFullCamera::Parameters::cx( void ) -{ - return m_cx; -} - -double& -PinholeFullCamera::Parameters::cy( void ) -{ - return m_cy; -} +double& PinholeFullCamera::Parameters::fx(void) { return m_fx; } -double -PinholeFullCamera::Parameters::k1( void ) const -{ - return m_k1; -} +double& PinholeFullCamera::Parameters::fy(void) { return m_fy; } -double -PinholeFullCamera::Parameters::k2( void ) const -{ - return m_k2; -} +double& PinholeFullCamera::Parameters::cx(void) { return m_cx; } -double -PinholeFullCamera::Parameters::p1( void ) const -{ - return m_p1; -} +double& PinholeFullCamera::Parameters::cy(void) { return m_cy; } -double -PinholeFullCamera::Parameters::p2( void ) const -{ - return m_p2; -} +double PinholeFullCamera::Parameters::k1(void) const { return m_k1; } -double -PinholeFullCamera::Parameters::fx( void ) const -{ - return m_fx; -} +double PinholeFullCamera::Parameters::k2(void) const { return m_k2; } -double -PinholeFullCamera::Parameters::fy( void ) const -{ - return m_fy; -} +double PinholeFullCamera::Parameters::p1(void) const { return m_p1; } -double -PinholeFullCamera::Parameters::cx( void ) const -{ - return m_cx; -} +double PinholeFullCamera::Parameters::p2(void) const { return m_p2; } -double -PinholeFullCamera::Parameters::cy( void ) const -{ - return m_cy; -} +double PinholeFullCamera::Parameters::fx(void) const { return m_fx; } -double -PinholeFullCamera::Parameters::k3( ) const -{ - return m_k3; -} +double PinholeFullCamera::Parameters::fy(void) const { return m_fy; } -double -PinholeFullCamera::Parameters::k4( ) const -{ - return m_k4; -} +double PinholeFullCamera::Parameters::cx(void) const { return m_cx; } -double -PinholeFullCamera::Parameters::k5( ) const -{ - return m_k5; -} +double PinholeFullCamera::Parameters::cy(void) const { return m_cy; } -double -PinholeFullCamera::Parameters::k6( ) const -{ - return m_k6; -} +double PinholeFullCamera::Parameters::k3() const { return m_k3; } -bool -PinholeFullCamera::Parameters::readFromYamlFile( const std::string& filename ) -{ - cv::FileStorage fs( filename, cv::FileStorage::READ ); +double PinholeFullCamera::Parameters::k4() const { return m_k4; } - if ( !fs.isOpened( ) ) - { - return false; - } +double PinholeFullCamera::Parameters::k5() const { return m_k5; } - if ( !fs["model_type"].isNone( ) ) - { - std::string sModelType; - fs["model_type"] >> sModelType; +double PinholeFullCamera::Parameters::k6() const { return m_k6; } - if ( sModelType.compare( "PINHOLE_FULL" ) != 0 ) - { - return false; - } - } +bool PinholeFullCamera::Parameters::readFromYamlFile( + const std::string& filename) { + cv::FileStorage fs(filename, cv::FileStorage::READ); - m_modelType = PINHOLE_FULL; - fs["camera_name"] >> m_cameraName; - m_imageWidth = static_cast< int >( fs["image_width"] ); - m_imageHeight = static_cast< int >( fs["image_height"] ); - - cv::FileNode n = fs["distortion_parameters"]; - m_k1 = static_cast< double >( n["k1"] ); - m_k2 = static_cast< double >( n["k2"] ); - m_k3 = static_cast< double >( n["k3"] ); - m_k4 = static_cast< double >( n["k4"] ); - m_k5 = static_cast< double >( n["k5"] ); - m_k6 = static_cast< double >( n["k6"] ); - m_p1 = static_cast< double >( n["p1"] ); - m_p2 = static_cast< double >( n["p2"] ); - - n = fs["projection_parameters"]; - m_fx = static_cast< double >( n["fx"] ); - m_fy = static_cast< double >( n["fy"] ); - m_cx = static_cast< double >( n["cx"] ); - m_cy = static_cast< double >( n["cy"] ); - - return true; -} + if (!fs.isOpened()) { + return false; + } -void -PinholeFullCamera::Parameters::writeToYamlFile( const std::string& filename ) const -{ - cv::FileStorage fs( filename, cv::FileStorage::WRITE ); - - fs << "model_type" - << "PINHOLE_FULL"; - fs << "camera_name" << m_cameraName; - fs << "image_width" << m_imageWidth; - fs << "image_height" << m_imageHeight; - - // radial distortion: k1, k2 - // tangential distortion: p1, p2 - fs << "distortion_parameters"; - fs << "{" - << "k1" << m_k1 << "k2" << m_k2 << "k3" << m_k3 << "k4" << m_k4 << "k5" << m_k5 - << "k6" << m_k6 << "p1" << m_p1 << "p2" << m_p2 << "}"; - - // projection: fx, fy, cx, cy - fs << "projection_parameters"; - fs << "{" - << "fx" << m_fx << "fy" << m_fy << "cx" << m_cx << "cy" << m_cy << "}"; - - fs.release( ); -} + if (!fs["model_type"].isNone()) { + std::string sModelType; + fs["model_type"] >> sModelType; -PinholeFullCamera::Parameters& -PinholeFullCamera::Parameters::operator=( const PinholeFullCamera::Parameters& other ) -{ - if ( this != &other ) - { - m_modelType = other.m_modelType; - m_cameraName = other.m_cameraName; - m_imageWidth = other.m_imageWidth; - m_imageHeight = other.m_imageHeight; - m_k1 = other.m_k1; - m_k2 = other.m_k2; - m_k3 = other.m_k3; - m_k4 = other.m_k4; - m_k5 = other.m_k5; - m_k6 = other.m_k6; - m_p1 = other.m_p1; - m_p2 = other.m_p2; - m_fx = other.m_fx; - m_fy = other.m_fy; - m_cx = other.m_cx; - m_cy = other.m_cy; + if (sModelType.compare("PINHOLE_FULL") != 0) { + return false; } + } + + m_modelType = PINHOLE_FULL; + fs["camera_name"] >> m_cameraName; + m_imageWidth = static_cast(fs["image_width"]); + m_imageHeight = static_cast(fs["image_height"]); + + cv::FileNode n = fs["distortion_parameters"]; + m_k1 = static_cast(n["k1"]); + m_k2 = static_cast(n["k2"]); + m_k3 = static_cast(n["k3"]); + m_k4 = static_cast(n["k4"]); + m_k5 = static_cast(n["k5"]); + m_k6 = static_cast(n["k6"]); + m_p1 = static_cast(n["p1"]); + m_p2 = static_cast(n["p2"]); + + n = fs["projection_parameters"]; + m_fx = static_cast(n["fx"]); + m_fy = static_cast(n["fy"]); + m_cx = static_cast(n["cx"]); + m_cy = static_cast(n["cy"]); + + return true; +} + +void PinholeFullCamera::Parameters::writeToYamlFile( + const std::string& filename) const { + cv::FileStorage fs(filename, cv::FileStorage::WRITE); + + fs << "model_type" + << "PINHOLE_FULL"; + fs << "camera_name" << m_cameraName; + fs << "image_width" << m_imageWidth; + fs << "image_height" << m_imageHeight; + + // radial distortion: k1, k2 + // tangential distortion: p1, p2 + fs << "distortion_parameters"; + fs << "{" + << "k1" << m_k1 << "k2" << m_k2 << "k3" << m_k3 << "k4" << m_k4 << "k5" + << m_k5 << "k6" << m_k6 << "p1" << m_p1 << "p2" << m_p2 << "}"; + + // projection: fx, fy, cx, cy + fs << "projection_parameters"; + fs << "{" + << "fx" << m_fx << "fy" << m_fy << "cx" << m_cx << "cy" << m_cy << "}"; + + fs.release(); +} + +PinholeFullCamera::Parameters& PinholeFullCamera::Parameters::operator=( + const PinholeFullCamera::Parameters& other) { + if (this != &other) { + m_modelType = other.m_modelType; + m_cameraName = other.m_cameraName; + m_imageWidth = other.m_imageWidth; + m_imageHeight = other.m_imageHeight; + m_k1 = other.m_k1; + m_k2 = other.m_k2; + m_k3 = other.m_k3; + m_k4 = other.m_k4; + m_k5 = other.m_k5; + m_k6 = other.m_k6; + m_p1 = other.m_p1; + m_p2 = other.m_p2; + m_fx = other.m_fx; + m_fy = other.m_fy; + m_cx = other.m_cx; + m_cy = other.m_cy; + } + + return *this; +} + +std::ostream& operator<<(std::ostream& out, + const PinholeFullCamera::Parameters& params) { + out << "Camera Parameters:" << std::endl; + out << " model_type " + << "PINHOLE_FULL" << std::endl; + out << " camera_name " << params.m_cameraName << std::endl; + out << " image_width " << params.m_imageWidth << std::endl; + out << " image_height " << params.m_imageHeight << std::endl; + + // radial distortion: k1, k2 + // tangential distortion: p1, p2 + out << "Distortion Parameters" << std::endl; + out << " k1 " << params.m_k1 << std::endl + << " k2 " << params.m_k2 << std::endl + << " k3 " << params.m_k3 << std::endl + << " k4 " << params.m_k4 << std::endl + << " k5 " << params.m_k5 << std::endl + << " k6 " << params.m_k6 << std::endl + << " p1 " << params.m_p1 << std::endl + << " p2 " << params.m_p2 << std::endl; + + // projection: fx, fy, cx, cy + out << "Projection Parameters" << std::endl; + out << " fx " << params.m_fx << std::endl + << " fy " << params.m_fy << std::endl + << " cx " << params.m_cx << std::endl + << " cy " << params.m_cy << std::endl; + + return out; +} + +PinholeFullCamera::PinholeFullCamera() + : m_inv_K11(1.0), + m_inv_K13(0.0), + m_inv_K22(1.0), + m_inv_K23(0.0), + m_noDistortion(true) {} + +PinholeFullCamera::PinholeFullCamera(const std::string& cameraName, + int imageWidth, int imageHeight, double k1, + double k2, double k3, double k4, double k5, + double k6, double p1, double p2, double fx, + double fy, double cx, double cy) + : mParameters(cameraName, imageWidth, imageHeight, k1, k2, k3, k4, k5, k6, + p1, p2, fx, fy, cx, cy) { + if ((mParameters.k1() == 0.0) && (mParameters.k2() == 0.0) && + (mParameters.p1() == 0.0) && (mParameters.p2() == 0.0)) { + m_noDistortion = true; + } else { + m_noDistortion = false; + } + + // Inverse camera projection matrix parameters + m_inv_K11 = 1.0 / mParameters.fx(); + m_inv_K13 = -mParameters.cx() / mParameters.fx(); + m_inv_K22 = 1.0 / mParameters.fy(); + m_inv_K23 = -mParameters.cy() / mParameters.fy(); +} + +PinholeFullCamera::PinholeFullCamera( + const PinholeFullCamera::Parameters& params) + : mParameters(params) { + if ((mParameters.k1() == 0.0) && (mParameters.k2() == 0.0) && + (mParameters.p1() == 0.0) && (mParameters.p2() == 0.0)) { + m_noDistortion = true; + } else { + m_noDistortion = false; + } + + // Inverse camera projection matrix parameters + m_inv_K11 = 1.0 / mParameters.fx(); + m_inv_K13 = -mParameters.cx() / mParameters.fx(); + m_inv_K22 = 1.0 / mParameters.fy(); + m_inv_K23 = -mParameters.cy() / mParameters.fy(); +} + +Camera::ModelType PinholeFullCamera::modelType(void) const { + return mParameters.modelType(); +} + +const std::string& PinholeFullCamera::cameraName(void) const { + return mParameters.cameraName(); +} + +int PinholeFullCamera::imageWidth(void) const { + return mParameters.imageWidth(); +} + +int PinholeFullCamera::imageHeight(void) const { + return mParameters.imageHeight(); +} + +void PinholeFullCamera::estimateIntrinsics( + const cv::Size& boardSize, + const std::vector >& objectPoints, + const std::vector >& imagePoints) { + // Z. Zhang, A Flexible New Technique for Camera Calibration, PAMI 2000 + + Parameters params = getParameters(); + + params.k1() = 0.0; + params.k2() = 0.0; + params.p1() = 0.0; + params.p2() = 0.0; + + double cx = params.imageWidth() / 2.0; + double cy = params.imageHeight() / 2.0; + params.cx() = cx; + params.cy() = cy; + + size_t nImages = imagePoints.size(); + + cv::Mat A(nImages * 2, 2, CV_64F); + cv::Mat b(nImages * 2, 1, CV_64F); - return *this; -} - -std::ostream& -operator<<( std::ostream& out, const PinholeFullCamera::Parameters& params ) -{ - out << "Camera Parameters:" << std::endl; - out << " model_type " - << "PINHOLE_FULL" << std::endl; - out << " camera_name " << params.m_cameraName << std::endl; - out << " image_width " << params.m_imageWidth << std::endl; - out << " image_height " << params.m_imageHeight << std::endl; - - // radial distortion: k1, k2 - // tangential distortion: p1, p2 - out << "Distortion Parameters" << std::endl; - out << " k1 " << params.m_k1 << std::endl - << " k2 " << params.m_k2 << std::endl - << " k3 " << params.m_k3 << std::endl - << " k4 " << params.m_k4 << std::endl - << " k5 " << params.m_k5 << std::endl - << " k6 " << params.m_k6 << std::endl - << " p1 " << params.m_p1 << std::endl - << " p2 " << params.m_p2 << std::endl; - - // projection: fx, fy, cx, cy - out << "Projection Parameters" << std::endl; - out << " fx " << params.m_fx << std::endl - << " fy " << params.m_fy << std::endl - << " cx " << params.m_cx << std::endl - << " cy " << params.m_cy << std::endl; - - return out; -} - -PinholeFullCamera::PinholeFullCamera( ) -: m_inv_K11( 1.0 ) -, m_inv_K13( 0.0 ) -, m_inv_K22( 1.0 ) -, m_inv_K23( 0.0 ) -, m_noDistortion( true ) -{ -} + for (size_t i = 0; i < nImages; ++i) { + const std::vector& oPoints = objectPoints.at(i); -PinholeFullCamera::PinholeFullCamera( const std::string& cameraName, - int imageWidth, - int imageHeight, - double k1, - double k2, - double k3, - double k4, - double k5, - double k6, - double p1, - double p2, - double fx, - double fy, - double cx, - double cy ) -: mParameters( cameraName, imageWidth, imageHeight, k1, k2, k3, k4, k5, k6, p1, p2, fx, fy, cx, cy ) -{ - if ( ( mParameters.k1( ) == 0.0 ) && ( mParameters.k2( ) == 0.0 ) - && ( mParameters.p1( ) == 0.0 ) && ( mParameters.p2( ) == 0.0 ) ) - { - m_noDistortion = true; + std::vector M(oPoints.size()); + for (size_t j = 0; j < M.size(); ++j) { + M.at(j) = cv::Point2f(oPoints.at(j).x, oPoints.at(j).y); } - else - { - m_noDistortion = false; - } - - // Inverse camera projection matrix parameters - m_inv_K11 = 1.0 / mParameters.fx( ); - m_inv_K13 = -mParameters.cx( ) / mParameters.fx( ); - m_inv_K22 = 1.0 / mParameters.fy( ); - m_inv_K23 = -mParameters.cy( ) / mParameters.fy( ); -} -PinholeFullCamera::PinholeFullCamera( const PinholeFullCamera::Parameters& params ) -: mParameters( params ) -{ - if ( ( mParameters.k1( ) == 0.0 ) && ( mParameters.k2( ) == 0.0 ) - && ( mParameters.p1( ) == 0.0 ) && ( mParameters.p2( ) == 0.0 ) ) - { - m_noDistortion = true; - } - else - { - m_noDistortion = false; + cv::Mat H = cv::findHomography(M, imagePoints.at(i)); + + H.at(0, 0) -= H.at(2, 0) * cx; + H.at(0, 1) -= H.at(2, 1) * cx; + H.at(0, 2) -= H.at(2, 2) * cx; + H.at(1, 0) -= H.at(2, 0) * cy; + H.at(1, 1) -= H.at(2, 1) * cy; + H.at(1, 2) -= H.at(2, 2) * cy; + + double h[3], v[3], d1[3], d2[3]; + double n[4] = {0, 0, 0, 0}; + + for (int j = 0; j < 3; ++j) { + double t0 = H.at(j, 0); + double t1 = H.at(j, 1); + h[j] = t0; + v[j] = t1; + d1[j] = (t0 + t1) * 0.5; + d2[j] = (t0 - t1) * 0.5; + n[0] += t0 * t0; + n[1] += t1 * t1; + n[2] += d1[j] * d1[j]; + n[3] += d2[j] * d2[j]; } - // Inverse camera projection matrix parameters - m_inv_K11 = 1.0 / mParameters.fx( ); - m_inv_K13 = -mParameters.cx( ) / mParameters.fx( ); - m_inv_K22 = 1.0 / mParameters.fy( ); - m_inv_K23 = -mParameters.cy( ) / mParameters.fy( ); -} - -Camera::ModelType -PinholeFullCamera::modelType( void ) const -{ - return mParameters.modelType( ); -} - -const std::string& -PinholeFullCamera::cameraName( void ) const -{ - return mParameters.cameraName( ); -} - -int -PinholeFullCamera::imageWidth( void ) const -{ - return mParameters.imageWidth( ); -} - -int -PinholeFullCamera::imageHeight( void ) const -{ - return mParameters.imageHeight( ); -} + for (int j = 0; j < 4; ++j) { + n[j] = 1.0 / sqrt(n[j]); + } -void -PinholeFullCamera::estimateIntrinsics( const cv::Size& boardSize, - const std::vector< std::vector< cv::Point3f > >& objectPoints, - const std::vector< std::vector< cv::Point2f > >& imagePoints ) -{ - // Z. Zhang, A Flexible New Technique for Camera Calibration, PAMI 2000 - - Parameters params = getParameters( ); - - params.k1( ) = 0.0; - params.k2( ) = 0.0; - params.p1( ) = 0.0; - params.p2( ) = 0.0; - - double cx = params.imageWidth( ) / 2.0; - double cy = params.imageHeight( ) / 2.0; - params.cx( ) = cx; - params.cy( ) = cy; - - size_t nImages = imagePoints.size( ); - - cv::Mat A( nImages * 2, 2, CV_64F ); - cv::Mat b( nImages * 2, 1, CV_64F ); - - for ( size_t i = 0; i < nImages; ++i ) - { - const std::vector< cv::Point3f >& oPoints = objectPoints.at( i ); - - std::vector< cv::Point2f > M( oPoints.size( ) ); - for ( size_t j = 0; j < M.size( ); ++j ) - { - M.at( j ) = cv::Point2f( oPoints.at( j ).x, oPoints.at( j ).y ); - } - - cv::Mat H = cv::findHomography( M, imagePoints.at( i ) ); - - H.at< double >( 0, 0 ) -= H.at< double >( 2, 0 ) * cx; - H.at< double >( 0, 1 ) -= H.at< double >( 2, 1 ) * cx; - H.at< double >( 0, 2 ) -= H.at< double >( 2, 2 ) * cx; - H.at< double >( 1, 0 ) -= H.at< double >( 2, 0 ) * cy; - H.at< double >( 1, 1 ) -= H.at< double >( 2, 1 ) * cy; - H.at< double >( 1, 2 ) -= H.at< double >( 2, 2 ) * cy; - - double h[3], v[3], d1[3], d2[3]; - double n[4] = { 0, 0, 0, 0 }; - - for ( int j = 0; j < 3; ++j ) - { - double t0 = H.at< double >( j, 0 ); - double t1 = H.at< double >( j, 1 ); - h[j] = t0; - v[j] = t1; - d1[j] = ( t0 + t1 ) * 0.5; - d2[j] = ( t0 - t1 ) * 0.5; - n[0] += t0 * t0; - n[1] += t1 * t1; - n[2] += d1[j] * d1[j]; - n[3] += d2[j] * d2[j]; - } - - for ( int j = 0; j < 4; ++j ) - { - n[j] = 1.0 / sqrt( n[j] ); - } - - for ( int j = 0; j < 3; ++j ) - { - h[j] *= n[0]; - v[j] *= n[1]; - d1[j] *= n[2]; - d2[j] *= n[3]; - } - - A.at< double >( i * 2, 0 ) = h[0] * v[0]; - A.at< double >( i * 2, 1 ) = h[1] * v[1]; - A.at< double >( i * 2 + 1, 0 ) = d1[0] * d2[0]; - A.at< double >( i * 2 + 1, 1 ) = d1[1] * d2[1]; - b.at< double >( i * 2, 0 ) = -h[2] * v[2]; - b.at< double >( i * 2 + 1, 0 ) = -d1[2] * d2[2]; + for (int j = 0; j < 3; ++j) { + h[j] *= n[0]; + v[j] *= n[1]; + d1[j] *= n[2]; + d2[j] *= n[3]; } - cv::Mat f( 2, 1, CV_64F ); - cv::solve( A, b, f, cv::DECOMP_NORMAL | cv::DECOMP_LU ); + A.at(i * 2, 0) = h[0] * v[0]; + A.at(i * 2, 1) = h[1] * v[1]; + A.at(i * 2 + 1, 0) = d1[0] * d2[0]; + A.at(i * 2 + 1, 1) = d1[1] * d2[1]; + b.at(i * 2, 0) = -h[2] * v[2]; + b.at(i * 2 + 1, 0) = -d1[2] * d2[2]; + } - params.fx( ) = sqrt( fabs( 1.0 / f.at< double >( 0 ) ) ); - params.fy( ) = sqrt( fabs( 1.0 / f.at< double >( 1 ) ) ); + cv::Mat f(2, 1, CV_64F); + cv::solve(A, b, f, cv::DECOMP_NORMAL | cv::DECOMP_LU); - setParameters( params ); + params.fx() = sqrt(fabs(1.0 / f.at(0))); + params.fy() = sqrt(fabs(1.0 / f.at(1))); + + setParameters(params); } /** @@ -517,12 +367,11 @@ PinholeFullCamera::estimateIntrinsics( const cv::Size& boardSize, * \param p image coordinates * \param P coordinates of the point on the sphere */ -void -PinholeFullCamera::liftSphere( const Eigen::Vector2d& p, Eigen::Vector3d& P ) const -{ - liftProjective( p, P ); +void PinholeFullCamera::liftSphere(const Eigen::Vector2d& p, + Eigen::Vector3d& P) const { + liftProjective(p, P); - P.normalize( ); + P.normalize(); } /** @@ -531,86 +380,82 @@ PinholeFullCamera::liftSphere( const Eigen::Vector2d& p, Eigen::Vector3d& P ) co * \param p image coordinates * \param P coordinates of the projective ray */ -void -PinholeFullCamera::liftProjective( const Eigen::Vector2d& p, Eigen::Vector3d& P ) const -{ - double k1 = mParameters.k1( ); - double k2 = mParameters.k2( ); - double k3 = mParameters.k3( ); - double k4 = mParameters.k4( ); - double k5 = mParameters.k5( ); - double k6 = mParameters.k6( ); - double p1 = mParameters.p1( ); - double p2 = mParameters.p2( ); - - double fx = mParameters.fx( ); - double fy = mParameters.fy( ); - double ifx = 1. / fx; - double ify = 1. / fy; - double cx = mParameters.cx( ); - double cy = mParameters.cy( ); - - // Lift points to normalised plane - double mx_d = ifx * p( 0 ) + m_inv_K13; - double my_d = ify * p( 1 ) + m_inv_K23; - double u = p( 0 ); - double v = p( 1 ); - double x = mx_d; - double y = my_d; - double x0 = x; - double y0 = y; - - double error = std::numeric_limits< double >::max( ); - - int max_cnt = 8; // 5 - int min_error = 0.01; - for ( int j = 0;; j++ ) - { - if ( j > max_cnt ) - break; - if ( error < min_error ) - break; - - double r2 = x * x + y * y; - double icdist = ( 1 + ( ( k6 * r2 + k5 ) * r2 + k4 ) * r2 ) - / ( 1 + ( ( k3 * r2 + k2 ) * r2 + k1 ) * r2 ); - double deltaX = 2 * p1 * x * y + p2 * ( r2 + 2 * x * x ); - double deltaY = p1 * ( r2 + 2 * y * y ) + 2 * p2 * x * y; - - x = ( x0 - deltaX ) * icdist; - y = ( y0 - deltaY ) * icdist; - - if ( 1 ) - { - double r4, r6, a1, a2, a3, cdist, icdist2; - double xd, yd, xd0, yd0; - - r2 = x * x + y * y; - r4 = r2 * r2; - r6 = r4 * r2; - a1 = 2 * x * y; - a2 = r2 + 2 * x * x; - a3 = r2 + 2 * y * y; - cdist = 1 + k1 * r2 + k2 * r4 + k3 * r6; - icdist2 = 1. / ( 1 + k4 * r2 + k5 * r4 + k6 * r6 ); - xd0 = x * cdist * icdist2 + p1 * a1 + p2 * a2; - yd0 = y * cdist * icdist2 + p1 * a3 + p2 * a1; - - double x_proj = xd * fx + cx; - double y_proj = yd * fy + cy; - - error = sqrt( pow( x_proj - u, 2 ) + pow( y_proj - v, 2 ) ); - } +void PinholeFullCamera::liftProjective(const Eigen::Vector2d& p, + Eigen::Vector3d& P) const { + double k1 = mParameters.k1(); + double k2 = mParameters.k2(); + double k3 = mParameters.k3(); + double k4 = mParameters.k4(); + double k5 = mParameters.k5(); + double k6 = mParameters.k6(); + double p1 = mParameters.p1(); + double p2 = mParameters.p2(); + + double fx = mParameters.fx(); + double fy = mParameters.fy(); + double ifx = 1. / fx; + double ify = 1. / fy; + double cx = mParameters.cx(); + double cy = mParameters.cy(); + + // Lift points to normalised plane + double mx_d = ifx * p(0) + m_inv_K13; + double my_d = ify * p(1) + m_inv_K23; + double u = p(0); + double v = p(1); + double x = mx_d; + double y = my_d; + double x0 = x; + double y0 = y; + + double error = std::numeric_limits::max(); + + int max_cnt = 8; // 5 + int min_error = 0.01; + for (int j = 0;; j++) { + if (j > max_cnt) break; + if (error < min_error) break; + + double r2 = x * x + y * y; + double icdist = (1 + ((k6 * r2 + k5) * r2 + k4) * r2) / + (1 + ((k3 * r2 + k2) * r2 + k1) * r2); + double deltaX = 2 * p1 * x * y + p2 * (r2 + 2 * x * x); + double deltaY = p1 * (r2 + 2 * y * y) + 2 * p2 * x * y; + + x = (x0 - deltaX) * icdist; + y = (y0 - deltaY) * icdist; + + if (1) { + double r4, r6, a1, a2, a3, cdist, icdist2; + double xd, yd, xd0, yd0; + + r2 = x * x + y * y; + r4 = r2 * r2; + r6 = r4 * r2; + a1 = 2 * x * y; + a2 = r2 + 2 * x * x; + a3 = r2 + 2 * y * y; + cdist = 1 + k1 * r2 + k2 * r4 + k3 * r6; + icdist2 = 1. / (1 + k4 * r2 + k5 * r4 + k6 * r6); + xd0 = x * cdist * icdist2 + p1 * a1 + p2 * a2; + yd0 = y * cdist * icdist2 + p1 * a3 + p2 * a1; + + double x_proj = xd * fx + cx; + double y_proj = yd * fy + cy; + + error = sqrt(pow(x_proj - u, 2) + pow(y_proj - v, 2)); } + } - P << x, y, 1.0; + P << x, y, 1.0; } -void -PinholeFullCamera::liftProjective( const Eigen::Vector2d& p, Eigen::Vector3d& P, float image_scale ) const -{ - Eigen::Vector2d p_tmp = p / image_scale; // p_tmp is without resize, p is with resize - liftProjective( p_tmp, P ); // p_tmp is without resize +void PinholeFullCamera::liftProjective(const Eigen::Vector2d& p, + Eigen::Vector3d& P, + float image_scale) const { + Eigen::Vector2d p_tmp = + p / image_scale; // p_tmp is without resize, p is with resize + liftProjective(p_tmp, P); // p_tmp is without resize } /** @@ -619,37 +464,33 @@ PinholeFullCamera::liftProjective( const Eigen::Vector2d& p, Eigen::Vector3d& P, * \param P 3D point coordinates * \param p return value, contains the image point coordinates */ -void -PinholeFullCamera::spaceToPlane( const Eigen::Vector3d& P, Eigen::Vector2d& p ) const -{ - Eigen::Vector2d p_u, p_d; +void PinholeFullCamera::spaceToPlane(const Eigen::Vector3d& P, + Eigen::Vector2d& p) const { + Eigen::Vector2d p_u, p_d; - // Project points to the normalised plane - p_u << P( 0 ) / P( 2 ), P( 1 ) / P( 2 ); + // Project points to the normalised plane + p_u << P(0) / P(2), P(1) / P(2); - if ( m_noDistortion ) - { - p_d = p_u; - } - else - { - // Apply distortion - Eigen::Vector2d d_u; - distortion( p_u, d_u ); - p_d = p_u + d_u; - } + if (m_noDistortion) { + p_d = p_u; + } else { + // Apply distortion + Eigen::Vector2d d_u; + distortion(p_u, d_u); + p_d = p_u + d_u; + } - // Apply generalised projection matrix - p << mParameters.fx( ) * p_d( 0 ) + mParameters.cx( ), - mParameters.fy( ) * p_d( 1 ) + mParameters.cy( ); + // Apply generalised projection matrix + p << mParameters.fx() * p_d(0) + mParameters.cx(), + mParameters.fy() * p_d(1) + mParameters.cy(); } -void -PinholeFullCamera::spaceToPlane( const Eigen::Vector3d& P, Eigen::Vector2d& p, float image_scalse ) const -{ - Eigen::Vector2d p_tmp; - spaceToPlane( P, p_tmp ); - p = p_tmp * image_scalse; +void PinholeFullCamera::spaceToPlane(const Eigen::Vector3d& P, + Eigen::Vector2d& p, + float image_scalse) const { + Eigen::Vector2d p_tmp; + spaceToPlane(P, p_tmp); + p = p_tmp * image_scalse; } /** @@ -658,62 +499,57 @@ PinholeFullCamera::spaceToPlane( const Eigen::Vector3d& P, Eigen::Vector2d& p, f * \param P 3D point coordinates * \param p return value, contains the image point coordinates */ -void -PinholeFullCamera::spaceToPlane( const Eigen::Vector3d& P, - Eigen::Vector2d& p, - Eigen::Matrix< double, 2, 3 >& J ) const -{ - Eigen::Vector2d p_u, p_d; - double norm, inv_denom; - double dxdmx, dydmx, dxdmy, dydmy; - - norm = P.norm( ); - // Project points to the normalised plane - inv_denom = 1.0 / P( 2 ); - p_u << inv_denom * P( 0 ), inv_denom * P( 1 ); - - // Calculate jacobian - double dudx = inv_denom; - double dvdx = 0.0; - double dudy = 0.0; - double dvdy = inv_denom; - inv_denom = -inv_denom * inv_denom; - double dudz = P( 0 ) * inv_denom; - double dvdz = P( 1 ) * inv_denom; - - if ( m_noDistortion ) - { - p_d = p_u; - } - else - { - // Apply distortion - Eigen::Vector2d d_u; - distortion( p_u, d_u ); - p_d = p_u + d_u; - } - - double fx = mParameters.fx( ); - double fy = mParameters.fy( ); - - // Make the product of the jacobians - // and add projection matrix jacobian - inv_denom = fx * ( dudx * dxdmx + dvdx * dxdmy ); // reuse - dvdx = fy * ( dudx * dydmx + dvdx * dydmy ); - dudx = inv_denom; - - inv_denom = fx * ( dudy * dxdmx + dvdy * dxdmy ); // reuse - dvdy = fy * ( dudy * dydmx + dvdy * dydmy ); - dudy = inv_denom; - - inv_denom = fx * ( dudz * dxdmx + dvdz * dxdmy ); // reuse - dvdz = fy * ( dudz * dydmx + dvdz * dydmy ); - dudz = inv_denom; - - // Apply generalised projection matrix - p << fx * p_d( 0 ) + mParameters.cx( ), fy * p_d( 1 ) + mParameters.cy( ); - - J << dudx, dudy, dudz, dvdx, dvdy, dvdz; +void PinholeFullCamera::spaceToPlane(const Eigen::Vector3d& P, + Eigen::Vector2d& p, + Eigen::Matrix& J) const { + Eigen::Vector2d p_u, p_d; + double norm, inv_denom; + double dxdmx, dydmx, dxdmy, dydmy; + + norm = P.norm(); + // Project points to the normalised plane + inv_denom = 1.0 / P(2); + p_u << inv_denom * P(0), inv_denom * P(1); + + // Calculate jacobian + double dudx = inv_denom; + double dvdx = 0.0; + double dudy = 0.0; + double dvdy = inv_denom; + inv_denom = -inv_denom * inv_denom; + double dudz = P(0) * inv_denom; + double dvdz = P(1) * inv_denom; + + if (m_noDistortion) { + p_d = p_u; + } else { + // Apply distortion + Eigen::Vector2d d_u; + distortion(p_u, d_u); + p_d = p_u + d_u; + } + + double fx = mParameters.fx(); + double fy = mParameters.fy(); + + // Make the product of the jacobians + // and add projection matrix jacobian + inv_denom = fx * (dudx * dxdmx + dvdx * dxdmy); // reuse + dvdx = fy * (dudx * dydmx + dvdx * dydmy); + dudx = inv_denom; + + inv_denom = fx * (dudy * dxdmx + dvdy * dxdmy); // reuse + dvdy = fy * (dudy * dydmx + dvdy * dydmy); + dudy = inv_denom; + + inv_denom = fx * (dudz * dxdmx + dvdz * dxdmy); // reuse + dvdz = fy * (dudz * dydmx + dvdz * dydmy); + dudz = inv_denom; + + // Apply generalised projection matrix + p << fx * p_d(0) + mParameters.cx(), fy * p_d(1) + mParameters.cy(); + + J << dudx, dudy, dudz, dvdx, dvdy, dvdz; } /** @@ -722,26 +558,22 @@ PinholeFullCamera::spaceToPlane( const Eigen::Vector3d& P, * \param p_u 2D point coordinates * \return image point coordinates */ -void -PinholeFullCamera::undistToPlane( const Eigen::Vector2d& p_u, Eigen::Vector2d& p ) const -{ - Eigen::Vector2d p_d; - - if ( m_noDistortion ) - { - p_d = p_u; - } - else - { - // Apply distortion - Eigen::Vector2d d_u; - distortion( p_u, d_u ); - p_d = p_u + d_u; - } +void PinholeFullCamera::undistToPlane(const Eigen::Vector2d& p_u, + Eigen::Vector2d& p) const { + Eigen::Vector2d p_d; + + if (m_noDistortion) { + p_d = p_u; + } else { + // Apply distortion + Eigen::Vector2d d_u; + distortion(p_u, d_u); + p_d = p_u + d_u; + } - // Apply generalised projection matrix - p << mParameters.fx( ) * p_d( 0 ) + mParameters.cx( ), - mParameters.fy( ) * p_d( 1 ) + mParameters.cy( ); + // Apply generalised projection matrix + p << mParameters.fx() * p_d(0) + mParameters.cx(), + mParameters.fy() * p_d(1) + mParameters.cy(); } /** @@ -750,34 +582,33 @@ PinholeFullCamera::undistToPlane( const Eigen::Vector2d& p_u, Eigen::Vector2d& p * \param p_u undistorted coordinates of point on the normalised plane * \return to obtain the distorted point: p_d = p_u + d_u */ -void -PinholeFullCamera::distortion( const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u ) const -{ - // project 3D object point to the image plane - double k1 = mParameters.k1( ); - double k2 = mParameters.k2( ); - double k3 = mParameters.k3( ); - double k4 = mParameters.k4( ); - double k5 = mParameters.k5( ); - double k6 = mParameters.k6( ); - double p1 = mParameters.p1( ); - double p2 = mParameters.p2( ); - - // Transform to model plane - double x = p_u( 0 ); - double y = p_u( 1 ); - - double r2 = x * x + y * y; - double r4 = r2 * r2; - double r6 = r4 * r2; - double a1 = 2 * x * y; - double a2 = r2 + 2 * x * x; - double a3 = r2 + 2 * y * y; - double cdist = 1 + k1 * r2 + k2 * r4 + k3 * r6; - double icdist2 = 1. / ( 1 + k4 * r2 + k5 * r4 + k6 * r6 ); - - d_u << x * cdist * icdist2 + p1 * a1 + p2 * a2 - x, // - y * cdist * icdist2 + p1 * a3 + p2 * a1 - y; +void PinholeFullCamera::distortion(const Eigen::Vector2d& p_u, + Eigen::Vector2d& d_u) const { + // project 3D object point to the image plane + double k1 = mParameters.k1(); + double k2 = mParameters.k2(); + double k3 = mParameters.k3(); + double k4 = mParameters.k4(); + double k5 = mParameters.k5(); + double k6 = mParameters.k6(); + double p1 = mParameters.p1(); + double p2 = mParameters.p2(); + + // Transform to model plane + double x = p_u(0); + double y = p_u(1); + + double r2 = x * x + y * y; + double r4 = r2 * r2; + double r6 = r4 * r2; + double a1 = 2 * x * y; + double a2 = r2 + 2 * x * x; + double a3 = r2 + 2 * y * y; + double cdist = 1 + k1 * r2 + k2 * r4 + k3 * r6; + double icdist2 = 1. / (1 + k4 * r2 + k5 * r4 + k6 * r6); + + d_u << x * cdist * icdist2 + p1 * a1 + p2 * a2 - x, // + y * cdist * icdist2 + p1 * a3 + p2 * a1 - y; } /** @@ -787,214 +618,190 @@ PinholeFullCamera::distortion( const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u * \param p_u undistorted coordinates of point on the normalised plane * \return to obtain the distorted point: p_d = p_u + d_u */ -void -PinholeFullCamera::distortion( const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u, Eigen::Matrix2d& J ) const -{ - // project 3D object point to the image plane - double k1 = mParameters.k1( ); - double k2 = mParameters.k2( ); - double k3 = mParameters.k3( ); - double k4 = mParameters.k4( ); - double k5 = mParameters.k5( ); - double k6 = mParameters.k6( ); - double p1 = mParameters.p1( ); - double p2 = mParameters.p2( ); - - // Transform to model plane - double x = p_u( 0 ); - double y = p_u( 1 ); - - double r2 = x * x + y * y; - double r4 = r2 * r2; - double r6 = r4 * r2; - double a1 = 2 * x * y; - double a2 = r2 + 2 * x * x; - double a3 = r2 + 2 * y * y; - double cdist = 1 + k1 * r2 + k2 * r4 + k3 * r6; - double icdist2 = 1. / ( 1 + k4 * r2 + k5 * r4 + k6 * r6 ); - - d_u << x * cdist * icdist2 + p1 * a1 + p2 * a2 - x, // - y * cdist * icdist2 + p1 * a3 + p2 * a1 - y; // -} - -void -PinholeFullCamera::initUndistortMap( cv::Mat& map1, cv::Mat& map2, double fScale ) const -{ - cv::Size imageSize( mParameters.imageWidth( ), mParameters.imageHeight( ) ); - - cv::Mat mapX = cv::Mat::zeros( imageSize, CV_32F ); - cv::Mat mapY = cv::Mat::zeros( imageSize, CV_32F ); - - for ( int v = 0; v < imageSize.height; ++v ) - { - for ( int u = 0; u < imageSize.width; ++u ) - { - double mx_u = m_inv_K11 / fScale * u + m_inv_K13 / fScale; - double my_u = m_inv_K22 / fScale * v + m_inv_K23 / fScale; - - Eigen::Vector3d P; - P << mx_u, my_u, 1.0; - - Eigen::Vector2d p; - spaceToPlane( P, p ); - - mapX.at< float >( v, u ) = p( 0 ); - mapY.at< float >( v, u ) = p( 1 ); - } +void PinholeFullCamera::distortion(const Eigen::Vector2d& p_u, + Eigen::Vector2d& d_u, + Eigen::Matrix2d& J) const { + // project 3D object point to the image plane + double k1 = mParameters.k1(); + double k2 = mParameters.k2(); + double k3 = mParameters.k3(); + double k4 = mParameters.k4(); + double k5 = mParameters.k5(); + double k6 = mParameters.k6(); + double p1 = mParameters.p1(); + double p2 = mParameters.p2(); + + // Transform to model plane + double x = p_u(0); + double y = p_u(1); + + double r2 = x * x + y * y; + double r4 = r2 * r2; + double r6 = r4 * r2; + double a1 = 2 * x * y; + double a2 = r2 + 2 * x * x; + double a3 = r2 + 2 * y * y; + double cdist = 1 + k1 * r2 + k2 * r4 + k3 * r6; + double icdist2 = 1. / (1 + k4 * r2 + k5 * r4 + k6 * r6); + + d_u << x * cdist * icdist2 + p1 * a1 + p2 * a2 - x, // + y * cdist * icdist2 + p1 * a3 + p2 * a1 - y; // +} + +void PinholeFullCamera::initUndistortMap(cv::Mat& map1, cv::Mat& map2, + double fScale) const { + cv::Size imageSize(mParameters.imageWidth(), mParameters.imageHeight()); + + cv::Mat mapX = cv::Mat::zeros(imageSize, CV_32F); + cv::Mat mapY = cv::Mat::zeros(imageSize, CV_32F); + + for (int v = 0; v < imageSize.height; ++v) { + for (int u = 0; u < imageSize.width; ++u) { + double mx_u = m_inv_K11 / fScale * u + m_inv_K13 / fScale; + double my_u = m_inv_K22 / fScale * v + m_inv_K23 / fScale; + + Eigen::Vector3d P; + P << mx_u, my_u, 1.0; + + Eigen::Vector2d p; + spaceToPlane(P, p); + + mapX.at(v, u) = p(0); + mapY.at(v, u) = p(1); } + } - cv::convertMaps( mapX, mapY, map1, map2, CV_32FC1, false ); + cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false); } -cv::Mat -PinholeFullCamera::initUndistortRectifyMap( -cv::Mat& map1, cv::Mat& map2, float fx, float fy, cv::Size imageSize, float cx, float cy, cv::Mat rmat ) const -{ - if ( imageSize == cv::Size( 0, 0 ) ) - { - imageSize = cv::Size( mParameters.imageWidth( ), mParameters.imageHeight( ) ); - } +cv::Mat PinholeFullCamera::initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, + float fx, float fy, + cv::Size imageSize, float cx, + float cy, + cv::Mat rmat) const { + if (imageSize == cv::Size(0, 0)) { + imageSize = cv::Size(mParameters.imageWidth(), mParameters.imageHeight()); + } - cv::Mat mapX = cv::Mat::zeros( imageSize.height, imageSize.width, CV_32F ); - cv::Mat mapY = cv::Mat::zeros( imageSize.height, imageSize.width, CV_32F ); + cv::Mat mapX = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F); + cv::Mat mapY = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F); - Eigen::Matrix3f R, R_inv; - cv::cv2eigen( rmat, R ); - R_inv = R.inverse( ); + Eigen::Matrix3f R, R_inv; + cv::cv2eigen(rmat, R); + R_inv = R.inverse(); - // assume no skew - Eigen::Matrix3f K_rect; + // assume no skew + Eigen::Matrix3f K_rect; - if ( cx == -1.0f || cy == -1.0f ) - { - K_rect << fx, 0, imageSize.width / 2, 0, fy, imageSize.height / 2, 0, 0, 1; - } - else - { - K_rect << fx, 0, cx, 0, fy, cy, 0, 0, 1; - } + if (cx == -1.0f || cy == -1.0f) { + K_rect << fx, 0, imageSize.width / 2, 0, fy, imageSize.height / 2, 0, 0, 1; + } else { + K_rect << fx, 0, cx, 0, fy, cy, 0, 0, 1; + } - if ( fx == -1.0f || fy == -1.0f ) - { - K_rect( 0, 0 ) = mParameters.fx( ); - K_rect( 1, 1 ) = mParameters.fy( ); - } + if (fx == -1.0f || fy == -1.0f) { + K_rect(0, 0) = mParameters.fx(); + K_rect(1, 1) = mParameters.fy(); + } - Eigen::Matrix3f K_rect_inv = K_rect.inverse( ); + Eigen::Matrix3f K_rect_inv = K_rect.inverse(); - for ( int v = 0; v < imageSize.height; ++v ) - { - for ( int u = 0; u < imageSize.width; ++u ) - { - Eigen::Vector3f xo; - xo << u, v, 1; + for (int v = 0; v < imageSize.height; ++v) { + for (int u = 0; u < imageSize.width; ++u) { + Eigen::Vector3f xo; + xo << u, v, 1; - Eigen::Vector3f uo = R_inv * K_rect_inv * xo; + Eigen::Vector3f uo = R_inv * K_rect_inv * xo; - Eigen::Vector2d p; - spaceToPlane( uo.cast< double >( ), p ); + Eigen::Vector2d p; + spaceToPlane(uo.cast(), p); - mapX.at< float >( v, u ) = p( 0 ); - mapY.at< float >( v, u ) = p( 1 ); - } + mapX.at(v, u) = p(0); + mapY.at(v, u) = p(1); } + } - cv::convertMaps( mapX, mapY, map1, map2, CV_32FC1, false ); + cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false); - cv::Mat K_rect_cv; - cv::eigen2cv( K_rect, K_rect_cv ); - return K_rect_cv; + cv::Mat K_rect_cv; + cv::eigen2cv(K_rect, K_rect_cv); + return K_rect_cv; } -int -PinholeFullCamera::parameterCount( void ) const -{ - return 12; -} +int PinholeFullCamera::parameterCount(void) const { return 12; } -const PinholeFullCamera::Parameters& -PinholeFullCamera::getParameters( void ) const -{ - return mParameters; +const PinholeFullCamera::Parameters& PinholeFullCamera::getParameters( + void) const { + return mParameters; } -void -PinholeFullCamera::setParameters( const PinholeFullCamera::Parameters& parameters ) -{ - mParameters = parameters; +void PinholeFullCamera::setParameters( + const PinholeFullCamera::Parameters& parameters) { + mParameters = parameters; - if ( ( mParameters.k1( ) == 0.0 ) && ( mParameters.k2( ) == 0.0 ) - && ( mParameters.p1( ) == 0.0 ) && ( mParameters.p2( ) == 0.0 ) ) - { - m_noDistortion = true; - } - else - { - m_noDistortion = false; - } + if ((mParameters.k1() == 0.0) && (mParameters.k2() == 0.0) && + (mParameters.p1() == 0.0) && (mParameters.p2() == 0.0)) { + m_noDistortion = true; + } else { + m_noDistortion = false; + } - m_inv_K11 = 1.0 / mParameters.fx( ); - m_inv_K13 = -mParameters.cx( ) / mParameters.fx( ); - m_inv_K22 = 1.0 / mParameters.fy( ); - m_inv_K23 = -mParameters.cy( ) / mParameters.fy( ); + m_inv_K11 = 1.0 / mParameters.fx(); + m_inv_K13 = -mParameters.cx() / mParameters.fx(); + m_inv_K22 = 1.0 / mParameters.fy(); + m_inv_K23 = -mParameters.cy() / mParameters.fy(); } -void -PinholeFullCamera::readParameters( const std::vector< double >& parameterVec ) -{ - if ( ( int )parameterVec.size( ) != parameterCount( ) ) - { - return; - } +void PinholeFullCamera::readParameters( + const std::vector& parameterVec) { + if ((int)parameterVec.size() != parameterCount()) { + return; + } + + Parameters params = getParameters(); - Parameters params = getParameters( ); - - params.k1( ) = parameterVec.at( 0 ); - params.k2( ) = parameterVec.at( 1 ); - params.k3( ) = parameterVec.at( 2 ); - params.k4( ) = parameterVec.at( 3 ); - params.k5( ) = parameterVec.at( 4 ); - params.k6( ) = parameterVec.at( 5 ); - params.p1( ) = parameterVec.at( 6 ); - params.p2( ) = parameterVec.at( 7 ); - params.fx( ) = parameterVec.at( 8 ); - params.fy( ) = parameterVec.at( 9 ); - params.cx( ) = parameterVec.at( 10 ); - params.cy( ) = parameterVec.at( 11 ); - - setParameters( params ); + params.k1() = parameterVec.at(0); + params.k2() = parameterVec.at(1); + params.k3() = parameterVec.at(2); + params.k4() = parameterVec.at(3); + params.k5() = parameterVec.at(4); + params.k6() = parameterVec.at(5); + params.p1() = parameterVec.at(6); + params.p2() = parameterVec.at(7); + params.fx() = parameterVec.at(8); + params.fy() = parameterVec.at(9); + params.cx() = parameterVec.at(10); + params.cy() = parameterVec.at(11); + + setParameters(params); } -void -PinholeFullCamera::writeParameters( std::vector< double >& parameterVec ) const -{ - parameterVec.resize( parameterCount( ) ); - parameterVec.at( 0 ) = mParameters.k1( ); - parameterVec.at( 1 ) = mParameters.k2( ); - parameterVec.at( 2 ) = mParameters.k3( ); - parameterVec.at( 3 ) = mParameters.k4( ); - parameterVec.at( 4 ) = mParameters.k5( ); - parameterVec.at( 5 ) = mParameters.k6( ); - parameterVec.at( 6 ) = mParameters.p1( ); - parameterVec.at( 7 ) = mParameters.p2( ); - parameterVec.at( 8 ) = mParameters.fx( ); - parameterVec.at( 9 ) = mParameters.fy( ); - parameterVec.at( 10 ) = mParameters.cx( ); - parameterVec.at( 11 ) = mParameters.cy( ); +void PinholeFullCamera::writeParameters( + std::vector& parameterVec) const { + parameterVec.resize(parameterCount()); + parameterVec.at(0) = mParameters.k1(); + parameterVec.at(1) = mParameters.k2(); + parameterVec.at(2) = mParameters.k3(); + parameterVec.at(3) = mParameters.k4(); + parameterVec.at(4) = mParameters.k5(); + parameterVec.at(5) = mParameters.k6(); + parameterVec.at(6) = mParameters.p1(); + parameterVec.at(7) = mParameters.p2(); + parameterVec.at(8) = mParameters.fx(); + parameterVec.at(9) = mParameters.fy(); + parameterVec.at(10) = mParameters.cx(); + parameterVec.at(11) = mParameters.cy(); } -void -PinholeFullCamera::writeParametersToYamlFile( const std::string& filename ) const -{ - mParameters.writeToYamlFile( filename ); +void PinholeFullCamera::writeParametersToYamlFile( + const std::string& filename) const { + mParameters.writeToYamlFile(filename); } -std::string -PinholeFullCamera::parametersToString( void ) const -{ - std::ostringstream oss; - oss << mParameters; +std::string PinholeFullCamera::parametersToString(void) const { + std::ostringstream oss; + oss << mParameters; - return oss.str( ); -} + return oss.str(); } +} // namespace camodocal diff --git a/camera_models/src/camera_models/PolyFisheyeCamera.cc b/camera_models/src/camera_models/PolyFisheyeCamera.cc index ccd5bacf..5b85fdbd 100644 --- a/camera_models/src/camera_models/PolyFisheyeCamera.cc +++ b/camera_models/src/camera_models/PolyFisheyeCamera.cc @@ -1,4 +1,6 @@ #include +#include +#include #include #include @@ -9,174 +11,138 @@ #include #include -#include -#include - -namespace camodocal -{ - -PolyFisheyeCamera::PolyFisheyeCamera( ) -: m_inv_K11( 1.0 ) -, m_inv_K12( 0.0 ) -, m_inv_K13( 0.0 ) -, m_inv_K22( 1.0 ) -, m_inv_K23( 0.0 ) -{ - poly = new math_utils::Polynomial( FISHEYE_POLY_ORDER ); - - poly->setPolyCoeff( 0, 0.0 ); - poly->setPolyCoeff( 1, 1.0 ); -} - -PolyFisheyeCamera::PolyFisheyeCamera( const std::string& cameraName, - int imageWidth, - int imageHeight, - double k2, - double k3, - double k4, - double k5, - double k6, - double k7, - double p1, - double p2, - double A11, - double A12, - double A22, - double u0, - double v0, - int isFast ) -: mParameters( cameraName, imageWidth, imageHeight, k2, k3, k4, k5, k6, k7, p1, p2, A11, A12, A22, u0, v0, isFast ) -{ - eigen_utils::Vector coeff( FISHEYE_POLY_ORDER + 1 ); - coeff << 0.0, 1.0, k2, k3, k4, k5, k6, k7; - - poly = new math_utils::Polynomial( FISHEYE_POLY_ORDER ); - poly->setPolyCoeff( coeff ); - - if ( mParameters.isFast( ) == 1 ) - { - eigen_utils::Vector coeff( FISHEYE_POLY_ORDER + 1 ); - coeff << 0.0, 1.0, mParameters.k2( ), mParameters.k3( ), mParameters.k4( ), - mParameters.k5( ), mParameters.k6( ), mParameters.k7( ); - - // fastCalc = new FastCalcPOLY(coeff, - // (double) mParameters.maxIncidentAngle()); - fastCalc - = new FastCalcTABLE( coeff, mParameters.numDiff( ), ( double )mParameters.maxIncidentAngle( ) ); - } -} - -PolyFisheyeCamera::PolyFisheyeCamera( const PolyFisheyeCamera::Parameters& params ) -: mParameters( params ) -{ - eigen_utils::Vector coeff( FISHEYE_POLY_ORDER + 1 ); - coeff << 0.0, 1.0, params.k2( ), params.k3( ), params.k4( ), params.k5( ), params.k6( ), - params.k7( ); - - poly = new math_utils::Polynomial( FISHEYE_POLY_ORDER ); - poly->setPolyCoeff( coeff ); - - calcKinvese( params.A11( ), params.A12( ), params.A22( ), params.u0( ), params.v0( ) ); - - if ( mParameters.isFast( ) == 1 ) - { - eigen_utils::Vector coeff( FISHEYE_POLY_ORDER + 1 ); - coeff << 0.0, 1.0, mParameters.k2( ), mParameters.k3( ), mParameters.k4( ), - mParameters.k5( ), mParameters.k6( ), mParameters.k7( ); - - // fastCalc = new FastCalcPOLY(coeff, - // (double) mParameters.maxIncidentAngle()); - fastCalc - = new FastCalcTABLE( coeff, mParameters.numDiff( ), ( double )mParameters.maxIncidentAngle( ) ); +namespace camodocal { + +PolyFisheyeCamera::PolyFisheyeCamera() + : m_inv_K11(1.0), + m_inv_K12(0.0), + m_inv_K13(0.0), + m_inv_K22(1.0), + m_inv_K23(0.0) { + poly = new math_utils::Polynomial(FISHEYE_POLY_ORDER); + + poly->setPolyCoeff(0, 0.0); + poly->setPolyCoeff(1, 1.0); +} + +PolyFisheyeCamera::PolyFisheyeCamera(const std::string& cameraName, + int imageWidth, int imageHeight, double k2, + double k3, double k4, double k5, double k6, + double k7, double p1, double p2, + double A11, double A12, double A22, + double u0, double v0, int isFast) + : mParameters(cameraName, imageWidth, imageHeight, k2, k3, k4, k5, k6, k7, + p1, p2, A11, A12, A22, u0, v0, isFast) { + eigen_utils::Vector coeff(FISHEYE_POLY_ORDER + 1); + coeff << 0.0, 1.0, k2, k3, k4, k5, k6, k7; + + poly = new math_utils::Polynomial(FISHEYE_POLY_ORDER); + poly->setPolyCoeff(coeff); + + if (mParameters.isFast() == 1) { + eigen_utils::Vector coeff(FISHEYE_POLY_ORDER + 1); + coeff << 0.0, 1.0, mParameters.k2(), mParameters.k3(), mParameters.k4(), + mParameters.k5(), mParameters.k6(), mParameters.k7(); + + // fastCalc = new FastCalcPOLY(coeff, + // (double) + // mParameters.maxIncidentAngle()); + fastCalc = new FastCalcTABLE(coeff, mParameters.numDiff(), + (double)mParameters.maxIncidentAngle()); + } +} + +PolyFisheyeCamera::PolyFisheyeCamera( + const PolyFisheyeCamera::Parameters& params) + : mParameters(params) { + eigen_utils::Vector coeff(FISHEYE_POLY_ORDER + 1); + coeff << 0.0, 1.0, params.k2(), params.k3(), params.k4(), params.k5(), + params.k6(), params.k7(); + + poly = new math_utils::Polynomial(FISHEYE_POLY_ORDER); + poly->setPolyCoeff(coeff); + + calcKinvese(params.A11(), params.A12(), params.A22(), params.u0(), + params.v0()); + + if (mParameters.isFast() == 1) { + eigen_utils::Vector coeff(FISHEYE_POLY_ORDER + 1); + coeff << 0.0, 1.0, mParameters.k2(), mParameters.k3(), mParameters.k4(), + mParameters.k5(), mParameters.k6(), mParameters.k7(); + + // fastCalc = new FastCalcPOLY(coeff, + // (double) + // mParameters.maxIncidentAngle()); + fastCalc = new FastCalcTABLE(coeff, mParameters.numDiff(), + (double)mParameters.maxIncidentAngle()); + } +} + +Camera::ModelType PolyFisheyeCamera::modelType() const { + return mParameters.modelType(); +} + +const std::string& PolyFisheyeCamera::cameraName() const { + return mParameters.cameraName(); +} + +int PolyFisheyeCamera::imageWidth() const { return mParameters.imageWidth(); } + +int PolyFisheyeCamera::imageHeight() const { return mParameters.imageHeight(); } + +void PolyFisheyeCamera::spaceToPlane(const Eigen::Vector3d& P, + Eigen::Vector2d& p) const { + double theta = acos(P(2) / P.norm()); + // double phi = atan2(P(1), P(0)); + double inverse_r_P2 = 1.0 / sqrt(P(1) * P(1) + P(0) * P(0)); + double sin_phi = P(1) * inverse_r_P2; + double cos_phi = P(0) * inverse_r_P2; + + // TODO TODO TODO + Eigen::Vector2d p_u; + if (mParameters.isDistortion()) { + if (mParameters.isFast() == 1) { + p_u = fastCalc->r(theta) * + // Eigen::Vector2d(cos(phi), sin(phi)); + Eigen::Vector2d(cos_phi, sin_phi); + // std::cout<< " p_u " << p_u.transpose()<r( theta) + // <r( theta ) * - // Eigen::Vector2d(cos(phi), sin(phi)); - Eigen::Vector2d( cos_phi, sin_phi ); - // std::cout<< " p_u " << p_u.transpose()<r( theta) <& J ) const -{ - double theta = acos( P( 2 ) / P.norm( ) ); - double inverse_r_P2 = 1.0 / sqrt( P( 1 ) * P( 1 ) + P( 0 ) * P( 0 ) ); - double sin_phi = P( 1 ) * inverse_r_P2; - double cos_phi = P( 0 ) * inverse_r_P2; - - // TODO - Eigen::Vector2d p_u; - if ( mParameters.isFast( ) == 1 ) - p_u = fastCalc->r( theta ) * Eigen::Vector2d( cos_phi, sin_phi ); - else - p_u = r( mParameters.k2( ), - mParameters.k3( ), - mParameters.k4( ), - mParameters.k5( ), - mParameters.k6( ), - mParameters.k7( ), - theta ) - * Eigen::Vector2d( cos_phi, sin_phi ); - - // Apply generalised projection matrix - p << mParameters.A11( ) * p_u( 0 ) + mParameters.A12( ) * p_u( 1 ) + mParameters.u0( ), - mParameters.A22( ) * p_u( 1 ) + mParameters.v0( ); -} - -void -PolyFisheyeCamera::estimateIntrinsics( const cv::Size& boardSize, - const std::vector< std::vector< cv::Point3f > >& objectPoints, - const std::vector< std::vector< cv::Point2f > >& imagePoints ) -{ - Parameters params = getParameters( ); - - double u0 = params.imageWidth( ) / 2.0; - double v0 = params.imageHeight( ) / 2.0; - - std::vector< cv::Mat > rvecs, tvecs; - rvecs.assign( objectPoints.size( ), cv::Mat( ) ); - tvecs.assign( objectPoints.size( ), cv::Mat( ) ); - - params.k2( ) = 0.0; - params.k3( ) = 0.0; - params.k4( ) = 0.0; - params.k5( ) = 0.0; - params.k6( ) = 0.0; - params.k7( ) = 0.0; - params.p1( ) = 0.0; - params.p2( ) = 0.0; - params.u0( ) = u0; - params.v0( ) = v0; - - // Initialize focal length - // C. Hughes, P. Denny, M. Glavin, and E. Jones, - // Equidistant Fish-Eye Calibration and Rectification by Vanishing Point - // Extraction, PAMI 2010 - // Find circles from rows of chessboard corners, and for each pair - // of circles, find vanishing points: v1 and v2. - // f = ||v1 - v2|| / PI; - - // double f0 = 0.0; - double sum_f = 0.0; - int f_count = 0; - for ( size_t i = 0; i < imagePoints.size( ); ++i ) - { - std::vector< Eigen::Vector2d > center( boardSize.height ); - double radius[boardSize.height]; - - for ( int r = 0; r < boardSize.height; ++r ) - { - std::vector< cv::Point2d > circle; - for ( int c = 0; c < boardSize.width; ++c ) - { - circle.push_back( imagePoints.at( i ).at( r * boardSize.width + c ) ); - } - - fitCircle( circle, center[r]( 0 ), center[r]( 1 ), radius[r] ); - } - - for ( int j = 0; j < boardSize.height; ++j ) - { - for ( int k = j + 1; k < boardSize.height; ++k ) - { - // find distance etween pair of vanishing points which - // correspond to intersection points of 2 circles - std::vector< cv::Point2d > ipts; - ipts = intersectCircles( - center[j]( 0 ), center[j]( 1 ), radius[j], center[k]( 0 ), center[k]( 1 ), radius[k] ); - - if ( ipts.size( ) < 2 ) - { - continue; - } - - double f = cv::norm( ipts.at( 0 ) - ipts.at( 1 ) ) / M_PI; - sum_f += f; - ++f_count; - } - } +void PolyFisheyeCamera::spaceToPlane(const Eigen::Vector3d& P, + Eigen::Vector2d& p, + Eigen::Matrix& J) const { + double theta = acos(P(2) / P.norm()); + double inverse_r_P2 = 1.0 / sqrt(P(1) * P(1) + P(0) * P(0)); + double sin_phi = P(1) * inverse_r_P2; + double cos_phi = P(0) * inverse_r_P2; + + // TODO + Eigen::Vector2d p_u; + if (mParameters.isFast() == 1) + p_u = fastCalc->r(theta) * Eigen::Vector2d(cos_phi, sin_phi); + else + p_u = r(mParameters.k2(), mParameters.k3(), mParameters.k4(), + mParameters.k5(), mParameters.k6(), mParameters.k7(), theta) * + Eigen::Vector2d(cos_phi, sin_phi); + + // Apply generalised projection matrix + p << mParameters.A11() * p_u(0) + mParameters.A12() * p_u(1) + + mParameters.u0(), + mParameters.A22() * p_u(1) + mParameters.v0(); +} + +void PolyFisheyeCamera::estimateIntrinsics( + const cv::Size& boardSize, + const std::vector >& objectPoints, + const std::vector >& imagePoints) { + Parameters params = getParameters(); + + double u0 = params.imageWidth() / 2.0; + double v0 = params.imageHeight() / 2.0; + + std::vector rvecs, tvecs; + rvecs.assign(objectPoints.size(), cv::Mat()); + tvecs.assign(objectPoints.size(), cv::Mat()); + + params.k2() = 0.0; + params.k3() = 0.0; + params.k4() = 0.0; + params.k5() = 0.0; + params.k6() = 0.0; + params.k7() = 0.0; + params.p1() = 0.0; + params.p2() = 0.0; + params.u0() = u0; + params.v0() = v0; + + // Initialize focal length + // C. Hughes, P. Denny, M. Glavin, and E. Jones, + // Equidistant Fish-Eye Calibration and Rectification by Vanishing Point + // Extraction, PAMI 2010 + // Find circles from rows of chessboard corners, and for each pair + // of circles, find vanishing points: v1 and v2. + // f = ||v1 - v2|| / PI; + + // double f0 = 0.0; + double sum_f = 0.0; + int f_count = 0; + for (size_t i = 0; i < imagePoints.size(); ++i) { + std::vector center(boardSize.height); + double radius[boardSize.height]; + + for (int r = 0; r < boardSize.height; ++r) { + std::vector circle; + for (int c = 0; c < boardSize.width; ++c) { + circle.push_back(imagePoints.at(i).at(r * boardSize.width + c)); + } + + fitCircle(circle, center[r](0), center[r](1), radius[r]); } - double f1 = sum_f / f_count; - std::cout << "# INFO: avg f " << f1 << std::endl; - params.A11( ) = f1; - params.A22( ) = f1; + for (int j = 0; j < boardSize.height; ++j) { + for (int k = j + 1; k < boardSize.height; ++k) { + // find distance etween pair of vanishing points which + // correspond to intersection points of 2 circles + std::vector ipts; + ipts = intersectCircles(center[j](0), center[j](1), radius[j], + center[k](0), center[k](1), radius[k]); - setParameters( params ); -} - -void -PolyFisheyeCamera::liftSphere( const Eigen::Vector2d& p, Eigen::Vector3d& P ) const -{ - liftProjective( p, P ); -} - -void -PolyFisheyeCamera::rayToPlane( const Ray& ray, Eigen::Vector2d& p ) const -{ - Eigen::Vector2d p_u = r( mParameters.k2( ), - mParameters.k3( ), - mParameters.k4( ), - mParameters.k5( ), - mParameters.k6( ), - mParameters.k7( ), - ray.theta( ) ) - * Eigen::Vector2d( cos( ray.phi( ) ), sin( ray.phi( ) ) ); - - p( 0 ) = mParameters.A11( ) * p_u( 0 ) + mParameters.A12( ) * p_u( 1 ) + mParameters.u0( ); - p( 1 ) = mParameters.A22( ) * p_u( 1 ) + mParameters.v0( ); -} - -void -PolyFisheyeCamera::liftProjectiveToRay( const Eigen::Vector2d& p, Ray& ray ) const -{ - // TODO - double cos_phi, sin_phi; - double cos_theta, sin_theta; - if ( mParameters.isFast( ) == 1 ) - { - fastCalc->backprojectSymmetric( Eigen::Vector2d( m_inv_K11 * p( 0 ) + m_inv_K12 * p( 1 ) + m_inv_K13, - m_inv_K22 * p( 1 ) + m_inv_K23 ), - cos_theta, - sin_theta, - cos_phi, - sin_phi ); - ray.phi( ) = acos( cos_phi ); - ray.theta( ) = acos( cos_theta ); - } - else - { - backprojectSymmetric( Eigen::Vector2d( m_inv_K11 * p( 0 ) + m_inv_K12 * p( 1 ) + m_inv_K13, - m_inv_K22 * p( 1 ) + m_inv_K23 ), - cos_theta, - sin_theta, - cos_phi, - sin_phi ); - ray.phi( ) = acos( cos_phi ); - ray.theta( ) = acos( cos_theta ); - } -} - -void -PolyFisheyeCamera::liftProjective( const Eigen::Vector2d& p, Eigen::Vector3d& P ) const -{ - // Lift points to normalised plane - double cos_theta, sin_theta; - // Obtain a projective ray - double cos_phi, sin_phi; - - if ( mParameters.isDistortion( ) ) - { - if ( mParameters.isFast( ) == 1 ) - { - fastCalc->backprojectSymmetric( Eigen::Vector2d( m_inv_K11 * p( 0 ) + m_inv_K12 * p( 1 ) + m_inv_K13, - m_inv_K22 * p( 1 ) + m_inv_K23 ), - cos_theta, - sin_theta, - cos_phi, - sin_phi ); - // std::cout << " cos_theta fastCalc " << cos_theta << std::endl; - } - else - { - backprojectSymmetric( Eigen::Vector2d( m_inv_K11 * p( 0 ) + m_inv_K12 * p( 1 ) + m_inv_K13, - m_inv_K22 * p( 1 ) + m_inv_K23 ), - cos_theta, - sin_theta, - cos_phi, - sin_phi ); - // std::cout << " cos_theta " << cos_theta << std::endl; + if (ipts.size() < 2) { + continue; } - } - else - { - Eigen::Vector2d p_u( m_inv_K11 * p( 0 ) + m_inv_K13, m_inv_K22 * p( 1 ) + m_inv_K23 ); - - double r = p_u.norm( ); - sin_phi = p_u( 1 ) / r; - cos_phi = p_u( 0 ) / r; - cos_theta = cos( r ); - sin_theta = sqrt( 1 - cos_theta * cos_theta ); // sin( r ); - } - P = Eigen::Vector3d( cos_phi * sin_theta, sin_phi * sin_theta, cos_theta ); -} - -void -PolyFisheyeCamera::liftProjective( const Eigen::Vector2d& p, Eigen::Vector3d& P, float image_scale ) const -{ - Eigen::Vector2d p_tmp = p / image_scale; // p_tmp is without resize, p is with resize - liftProjective( p_tmp, P ); // p_tmp is without resize -} -void -PolyFisheyeCamera::undistToPlane( const Eigen::Vector2d& p_u, Eigen::Vector2d& p ) const -{ -} - -cv::Mat -PolyFisheyeCamera::initUndistortRectifyMap( -cv::Mat& map1, cv::Mat& map2, float fx, float fy, cv::Size imageSize, float cx, float cy, cv::Mat rmat ) const -{ - if ( imageSize == cv::Size( 0, 0 ) ) - { - imageSize = cv::Size( mParameters.imageWidth( ), mParameters.imageHeight( ) ); + double f = cv::norm(ipts.at(0) - ipts.at(1)) / M_PI; + sum_f += f; + ++f_count; + } } - - cv::Mat mapX = cv::Mat::zeros( imageSize.height, imageSize.width, CV_32F ); - cv::Mat mapY = cv::Mat::zeros( imageSize.height, imageSize.width, CV_32F ); - - Eigen::Matrix3f K_rect; - - if ( cx == -1.0f && cy == -1.0f ) - { - K_rect << fx, 0, imageSize.width / 2, 0, fy, imageSize.height / 2, 0, 0, 1; + } + double f1 = sum_f / f_count; + std::cout << "# INFO: avg f " << f1 << std::endl; + + params.A11() = f1; + params.A22() = f1; + + setParameters(params); +} + +void PolyFisheyeCamera::liftSphere(const Eigen::Vector2d& p, + Eigen::Vector3d& P) const { + liftProjective(p, P); +} + +void PolyFisheyeCamera::rayToPlane(const Ray& ray, Eigen::Vector2d& p) const { + Eigen::Vector2d p_u = + r(mParameters.k2(), mParameters.k3(), mParameters.k4(), mParameters.k5(), + mParameters.k6(), mParameters.k7(), ray.theta()) * + Eigen::Vector2d(cos(ray.phi()), sin(ray.phi())); + + p(0) = mParameters.A11() * p_u(0) + mParameters.A12() * p_u(1) + + mParameters.u0(); + p(1) = mParameters.A22() * p_u(1) + mParameters.v0(); +} + +void PolyFisheyeCamera::liftProjectiveToRay(const Eigen::Vector2d& p, + Ray& ray) const { + // TODO + double cos_phi, sin_phi; + double cos_theta, sin_theta; + if (mParameters.isFast() == 1) { + fastCalc->backprojectSymmetric( + Eigen::Vector2d(m_inv_K11 * p(0) + m_inv_K12 * p(1) + m_inv_K13, + m_inv_K22 * p(1) + m_inv_K23), + cos_theta, sin_theta, cos_phi, sin_phi); + ray.phi() = acos(cos_phi); + ray.theta() = acos(cos_theta); + } else { + backprojectSymmetric( + Eigen::Vector2d(m_inv_K11 * p(0) + m_inv_K12 * p(1) + m_inv_K13, + m_inv_K22 * p(1) + m_inv_K23), + cos_theta, sin_theta, cos_phi, sin_phi); + ray.phi() = acos(cos_phi); + ray.theta() = acos(cos_theta); + } +} + +void PolyFisheyeCamera::liftProjective(const Eigen::Vector2d& p, + Eigen::Vector3d& P) const { + // Lift points to normalised plane + double cos_theta, sin_theta; + // Obtain a projective ray + double cos_phi, sin_phi; + + if (mParameters.isDistortion()) { + if (mParameters.isFast() == 1) { + fastCalc->backprojectSymmetric( + Eigen::Vector2d(m_inv_K11 * p(0) + m_inv_K12 * p(1) + m_inv_K13, + m_inv_K22 * p(1) + m_inv_K23), + cos_theta, sin_theta, cos_phi, sin_phi); + // std::cout << " cos_theta fastCalc " << cos_theta << + // std::endl; + } else { + backprojectSymmetric( + Eigen::Vector2d(m_inv_K11 * p(0) + m_inv_K12 * p(1) + m_inv_K13, + m_inv_K22 * p(1) + m_inv_K23), + cos_theta, sin_theta, cos_phi, sin_phi); + // std::cout << " cos_theta " << cos_theta << std::endl; } - else - { - K_rect << fx, 0, cx, 0, fy, cy, 0, 0, 1; + } else { + Eigen::Vector2d p_u(m_inv_K11 * p(0) + m_inv_K13, + m_inv_K22 * p(1) + m_inv_K23); + + double r = p_u.norm(); + sin_phi = p_u(1) / r; + cos_phi = p_u(0) / r; + cos_theta = cos(r); + sin_theta = sqrt(1 - cos_theta * cos_theta); // sin( r ); + } + P = Eigen::Vector3d(cos_phi * sin_theta, sin_phi * sin_theta, cos_theta); +} + +void PolyFisheyeCamera::liftProjective(const Eigen::Vector2d& p, + Eigen::Vector3d& P, + float image_scale) const { + Eigen::Vector2d p_tmp = + p / image_scale; // p_tmp is without resize, p is with resize + liftProjective(p_tmp, P); // p_tmp is without resize +} + +void PolyFisheyeCamera::undistToPlane(const Eigen::Vector2d& p_u, + Eigen::Vector2d& p) const {} + +cv::Mat PolyFisheyeCamera::initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, + float fx, float fy, + cv::Size imageSize, float cx, + float cy, + cv::Mat rmat) const { + if (imageSize == cv::Size(0, 0)) { + imageSize = cv::Size(mParameters.imageWidth(), mParameters.imageHeight()); + } + + cv::Mat mapX = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F); + cv::Mat mapY = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F); + + Eigen::Matrix3f K_rect; + + if (cx == -1.0f && cy == -1.0f) { + K_rect << fx, 0, imageSize.width / 2, 0, fy, imageSize.height / 2, 0, 0, 1; + } else { + K_rect << fx, 0, cx, 0, fy, cy, 0, 0, 1; + } + + if (fx == -1.0f || fy == -1.0f) { + K_rect(0, 0) = mParameters.A11(); + K_rect(0, 1) = mParameters.A12(); + K_rect(1, 1) = mParameters.A22(); + } + + Eigen::Matrix3f K_rect_inv = K_rect.inverse(); + + Eigen::Matrix3f R, R_inv; + cv::cv2eigen(rmat, R); + R_inv = R.inverse(); + + for (int v = 0; v < imageSize.height; ++v) { + for (int u = 0; u < imageSize.width; ++u) { + Eigen::Vector3f xo; + xo << u, v, 1; + + // TODO FIXME + Eigen::Vector3f uo = R_inv * K_rect_inv * xo; + + Eigen::Vector2d p; + spaceToPlane(uo.cast(), p); + + mapX.at(v, u) = p(0); + mapY.at(v, u) = p(1); } + } - if ( fx == -1.0f || fy == -1.0f ) - { - K_rect( 0, 0 ) = mParameters.A11( ); - K_rect( 0, 1 ) = mParameters.A12( ); - K_rect( 1, 1 ) = mParameters.A22( ); - } - - Eigen::Matrix3f K_rect_inv = K_rect.inverse( ); - - Eigen::Matrix3f R, R_inv; - cv::cv2eigen( rmat, R ); - R_inv = R.inverse( ); - - for ( int v = 0; v < imageSize.height; ++v ) - { - for ( int u = 0; u < imageSize.width; ++u ) - { - Eigen::Vector3f xo; - xo << u, v, 1; - - // TODO FIXME - Eigen::Vector3f uo = R_inv * K_rect_inv * xo; - - Eigen::Vector2d p; - spaceToPlane( uo.cast< double >( ), p ); - - mapX.at< float >( v, u ) = p( 0 ); - mapY.at< float >( v, u ) = p( 1 ); - } - } - - cv::convertMaps( mapX, mapY, map1, map2, CV_32FC1, false ); + cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false); - cv::Mat K_rect_cv; - cv::eigen2cv( K_rect, K_rect_cv ); - return K_rect_cv; + cv::Mat K_rect_cv; + cv::eigen2cv(K_rect, K_rect_cv); + return K_rect_cv; } -int -PolyFisheyeCamera::parameterCount( ) const -{ - return FISHEYE_PARAMS_NUM; -} +int PolyFisheyeCamera::parameterCount() const { return FISHEYE_PARAMS_NUM; } -const PolyFisheyeCamera::Parameters& -PolyFisheyeCamera::getParameters( ) const -{ - return mParameters; +const PolyFisheyeCamera::Parameters& PolyFisheyeCamera::getParameters() const { + return mParameters; } -void -PolyFisheyeCamera::setParameters( const PolyFisheyeCamera::Parameters& parameters ) -{ - mParameters = parameters; - - calcKinvese( - parameters.A11( ), parameters.A12( ), parameters.A22( ), parameters.u0( ), parameters.v0( ) ); - if ( mParameters.isDistortion( ) ) - { +void PolyFisheyeCamera::setParameters( + const PolyFisheyeCamera::Parameters& parameters) { + mParameters = parameters; - eigen_utils::Vector coeff( FISHEYE_POLY_ORDER + 1 ); - coeff << 0.0, 1.0, mParameters.k2( ), mParameters.k3( ), mParameters.k4( ), - mParameters.k5( ), mParameters.k6( ), mParameters.k7( ); + calcKinvese(parameters.A11(), parameters.A12(), parameters.A22(), + parameters.u0(), parameters.v0()); + if (mParameters.isDistortion()) { + eigen_utils::Vector coeff(FISHEYE_POLY_ORDER + 1); + coeff << 0.0, 1.0, mParameters.k2(), mParameters.k3(), mParameters.k4(), + mParameters.k5(), mParameters.k6(), mParameters.k7(); - delete poly; - poly = new math_utils::Polynomial( FISHEYE_POLY_ORDER ); - poly->setPolyCoeff( coeff ); + delete poly; + poly = new math_utils::Polynomial(FISHEYE_POLY_ORDER); + poly->setPolyCoeff(coeff); - if ( mParameters.isFast( ) == 1 ) - setFastCalc( ); + if (mParameters.isFast() == 1) setFastCalc(); // TODO #ifdef FAST_FIRST // if ( mParameters.isFast( ) != 1 && mParameters.k2( ) != 0.0 ) // setFastCalc( ); #endif - } + } } -void -PolyFisheyeCamera::readParameters( const std::vector< double >& parameterVec ) -{ - if ( int( parameterVec.size( ) ) != parameterCount( ) ) - { - return; - } +void PolyFisheyeCamera::readParameters( + const std::vector& parameterVec) { + if (int(parameterVec.size()) != parameterCount()) { + return; + } - Parameters params = getParameters( ); + Parameters params = getParameters(); - params.A11( ) = parameterVec.at( 0 ); - params.A12( ) = parameterVec.at( 1 ); - params.A22( ) = parameterVec.at( 2 ); - params.u0( ) = parameterVec.at( 3 ); - params.v0( ) = parameterVec.at( 4 ); + params.A11() = parameterVec.at(0); + params.A12() = parameterVec.at(1); + params.A22() = parameterVec.at(2); + params.u0() = parameterVec.at(3); + params.v0() = parameterVec.at(4); - params.k2( ) = parameterVec.at( 5 ); - params.k3( ) = parameterVec.at( 6 ); - params.k4( ) = parameterVec.at( 7 ); - params.k5( ) = parameterVec.at( 8 ); - params.k6( ) = parameterVec.at( 9 ); - params.k7( ) = parameterVec.at( 10 ); + params.k2() = parameterVec.at(5); + params.k3() = parameterVec.at(6); + params.k4() = parameterVec.at(7); + params.k5() = parameterVec.at(8); + params.k6() = parameterVec.at(9); + params.k7() = parameterVec.at(10); - params.p1( ) = parameterVec.at( 11 ); - params.p2( ) = parameterVec.at( 12 ); + params.p1() = parameterVec.at(11); + params.p2() = parameterVec.at(12); - setParameters( params ); + setParameters(params); } -void -PolyFisheyeCamera::writeParameters( std::vector< double >& parameterVec ) const -{ - parameterVec.resize( parameterCount( ) ); - - parameterVec.at( 0 ) = mParameters.A11( ); - parameterVec.at( 1 ) = mParameters.A12( ); - parameterVec.at( 2 ) = mParameters.A22( ); - parameterVec.at( 3 ) = mParameters.u0( ); - parameterVec.at( 4 ) = mParameters.v0( ); - - parameterVec.at( 5 ) = mParameters.k2( ); - parameterVec.at( 6 ) = mParameters.k3( ); - parameterVec.at( 7 ) = mParameters.k4( ); - parameterVec.at( 8 ) = mParameters.k5( ); - parameterVec.at( 9 ) = mParameters.k6( ); - parameterVec.at( 10 ) = mParameters.k7( ); - - parameterVec.at( 11 ) = mParameters.p1( ); - parameterVec.at( 12 ) = mParameters.p2( ); +void PolyFisheyeCamera::writeParameters( + std::vector& parameterVec) const { + parameterVec.resize(parameterCount()); + + parameterVec.at(0) = mParameters.A11(); + parameterVec.at(1) = mParameters.A12(); + parameterVec.at(2) = mParameters.A22(); + parameterVec.at(3) = mParameters.u0(); + parameterVec.at(4) = mParameters.v0(); + + parameterVec.at(5) = mParameters.k2(); + parameterVec.at(6) = mParameters.k3(); + parameterVec.at(7) = mParameters.k4(); + parameterVec.at(8) = mParameters.k5(); + parameterVec.at(9) = mParameters.k6(); + parameterVec.at(10) = mParameters.k7(); + + parameterVec.at(11) = mParameters.p1(); + parameterVec.at(12) = mParameters.p2(); } -void -PolyFisheyeCamera::writeParametersToYamlFile( const std::string& filename ) const -{ - mParameters.writeToYamlFile( filename ); +void PolyFisheyeCamera::writeParametersToYamlFile( + const std::string& filename) const { + mParameters.writeToYamlFile(filename); } -std::string -PolyFisheyeCamera::parametersToString( ) const -{ - std::ostringstream oss; - oss << mParameters; +std::string PolyFisheyeCamera::parametersToString() const { + std::ostringstream oss; + oss << mParameters; - return oss.str( ); + return oss.str(); } -void -PolyFisheyeCamera::backprojectSymmetric( const Eigen::Vector2d& p_u, - double& cos_theta, - double& sin_theta, - double& cos_phi, - double& sin_phi ) const -{ - double r = p_u.norm( ); - double theta; - - if ( r < 1e-10 ) - { - sin_phi = 0.0; - cos_phi = 1.0; - theta = 0.0; - } - else - { - sin_phi = p_u( 1 ) / r; - cos_phi = p_u( 0 ) / r; - - theta = poly->getOneRealRoot( r, 0.0, MAX_INCIDENT_ANGLE_DEGREE / RAD2DEG ); - if ( theta < 1e-10 ) - theta = 3.14; - } +void PolyFisheyeCamera::backprojectSymmetric(const Eigen::Vector2d& p_u, + double& cos_theta, + double& sin_theta, double& cos_phi, + double& sin_phi) const { + double r = p_u.norm(); + double theta; - sin_theta = sin( theta ); - cos_theta = cos( theta ); - // vec_root = poly->getRealRoot(r, 0.0, MAX_INCIDENT_ANGLE_DEGREE/RAD2DEG); + if (r < 1e-10) { + sin_phi = 0.0; + cos_phi = 1.0; + theta = 0.0; + } else { + sin_phi = p_u(1) / r; + cos_phi = p_u(0) / r; - // if(theta == 0) - // std::cout << " r " << r << " theta " << theta << std::endl; - // if(vec_root.size() == 1) - // theta = vec_root(0); - // else - // theta = 0; -} + theta = poly->getOneRealRoot(r, 0.0, MAX_INCIDENT_ANGLE_DEGREE / RAD2DEG); + if (theta < 1e-10) theta = 3.14; + } -bool -PolyFisheyeCamera::calcKinvese( double a11, double a12, double a22, double u0, double v0 ) -{ - eigen_utils::Matrix K( 3, 3 ); - K << a11, a12, u0, 0, a22, v0, 0, 0, 1; + sin_theta = sin(theta); + cos_theta = cos(theta); + // vec_root = poly->getRealRoot(r, 0.0, MAX_INCIDENT_ANGLE_DEGREE/RAD2DEG); + + // if(theta == 0) + // std::cout << " r " << r << " theta " << theta << std::endl; + // if(vec_root.size() == 1) + // theta = vec_root(0); + // else + // theta = 0; +} + +bool PolyFisheyeCamera::calcKinvese(double a11, double a12, double a22, + double u0, double v0) { + eigen_utils::Matrix K(3, 3); + K << a11, a12, u0, 0, a22, v0, 0, 0, 1; - eigen_utils::Matrix K_inv = K.inverse( ); - // std::cout << " K file: "<< K << std::endl; - // std::cout << " K_inv file: "<< K_inv << std::endl; + eigen_utils::Matrix K_inv = K.inverse(); + // std::cout << " K file: "<< K << std::endl; + // std::cout << " K_inv file: "<< K_inv << std::endl; - // Inverse camera projection matrix parameters - m_inv_K11 = K_inv( 0, 0 ); - m_inv_K12 = K_inv( 0, 1 ); - m_inv_K13 = K_inv( 0, 2 ); - m_inv_K22 = K_inv( 1, 1 ); - m_inv_K23 = K_inv( 1, 2 ); + // Inverse camera projection matrix parameters + m_inv_K11 = K_inv(0, 0); + m_inv_K12 = K_inv(0, 1); + m_inv_K13 = K_inv(0, 2); + m_inv_K22 = K_inv(1, 1); + m_inv_K23 = K_inv(1, 2); - return true; + return true; } -void -PolyFisheyeCamera::setPoly( math_utils::Polynomial* value ) -{ - poly = value; -} +void PolyFisheyeCamera::setPoly(math_utils::Polynomial* value) { poly = value; } -PolyFisheyeCamera::FastCalcTABLE* -PolyFisheyeCamera::getFastCalc( ) -{ - return fastCalc; +PolyFisheyeCamera::FastCalcTABLE* PolyFisheyeCamera::getFastCalc() { + return fastCalc; } -void -PolyFisheyeCamera::setFastCalc( ) -{ - eigen_utils::Vector coeff_fast( FISHEYE_POLY_ORDER + 1 ); - coeff_fast << 0.0, 1.0, mParameters.k2( ), mParameters.k3( ), mParameters.k4( ), - mParameters.k5( ), mParameters.k6( ), mParameters.k7( ); +void PolyFisheyeCamera::setFastCalc() { + eigen_utils::Vector coeff_fast(FISHEYE_POLY_ORDER + 1); + coeff_fast << 0.0, 1.0, mParameters.k2(), mParameters.k3(), mParameters.k4(), + mParameters.k5(), mParameters.k6(), mParameters.k7(); - std::cout << "[#Info][camera_model]" << std::endl; - // if(fastCalc != NULL) - // delete fastCalc; - // fastCalc = new FastCalcPOLY(coeff_fast, - // (double) mParameters.maxIncidentAngle()); + std::cout << "[#Info][camera_model]" << std::endl; + // if(fastCalc != NULL) + // delete fastCalc; + // fastCalc = new FastCalcPOLY(coeff_fast, + // (double) + // mParameters.maxIncidentAngle()); - fastCalc - = new FastCalcTABLE( coeff_fast, mParameters.numDiff( ), ( double )mParameters.maxIncidentAngle( ) ); + fastCalc = new FastCalcTABLE(coeff_fast, mParameters.numDiff(), + (double)mParameters.maxIncidentAngle()); - mParameters.setFast( ); + mParameters.setFast(); } -double -PolyFisheyeCamera::getInv_K23( ) const -{ - return m_inv_K23; -} +double PolyFisheyeCamera::getInv_K23() const { return m_inv_K23; } -double -PolyFisheyeCamera::getInv_K22( ) const -{ - return m_inv_K22; -} +double PolyFisheyeCamera::getInv_K22() const { return m_inv_K22; } -double -PolyFisheyeCamera::getInv_K13( ) const -{ - return m_inv_K13; -} +double PolyFisheyeCamera::getInv_K13() const { return m_inv_K13; } -double -PolyFisheyeCamera::getInv_K12( ) const -{ - return m_inv_K12; -} +double PolyFisheyeCamera::getInv_K12() const { return m_inv_K12; } -double -PolyFisheyeCamera::getInv_K11( ) const -{ - return m_inv_K11; -} +double PolyFisheyeCamera::getInv_K11() const { return m_inv_K11; } -math_utils::Polynomial* -PolyFisheyeCamera::getPoly( ) const -{ - return poly; -} +math_utils::Polynomial* PolyFisheyeCamera::getPoly() const { return poly; } -PolyFisheyeCamera::Parameters::Parameters( ) -: Camera::Parameters( POLYFISHEYE ) -, m_k2( 0.0 ) -, m_k3( 0.0 ) -, m_k4( 0.0 ) -, m_k5( 0.0 ) -, m_k6( 0.0 ) -, m_k7( 0.0 ) -, m_p1( 0.0 ) -, m_p2( 0.0 ) -, m_A11( 1.0 ) -, m_A12( 0.0 ) -, m_A22( 1.0 ) -, m_u0( 0.0 ) -, m_v0( 0.0 ) -, m_isFast( 0 ) -, m_numDiff( FAST_NUM_DEFAULT ) -, m_maxIncidentAngle( FAST_MAX_INCIDENT_ANGLE ) -{ -} +PolyFisheyeCamera::Parameters::Parameters() + : Camera::Parameters(POLYFISHEYE), + m_k2(0.0), + m_k3(0.0), + m_k4(0.0), + m_k5(0.0), + m_k6(0.0), + m_k7(0.0), + m_p1(0.0), + m_p2(0.0), + m_A11(1.0), + m_A12(0.0), + m_A22(1.0), + m_u0(0.0), + m_v0(0.0), + m_isFast(0), + m_numDiff(FAST_NUM_DEFAULT), + m_maxIncidentAngle(FAST_MAX_INCIDENT_ANGLE) {} -PolyFisheyeCamera::Parameters::Parameters( const std::string& cameraName, - int w, - int h, - double k2, - double k3, - double k4, - double k5, - double k6, - double k7, - double p1, - double p2, - double A11, - double A12, - double A22, - double u0, - double v0, - int isFast ) -: Camera::Parameters( POLYFISHEYE, cameraName, w, h ) -, m_k2( k2 ) -, m_k3( k3 ) -, m_k4( k4 ) -, m_k5( k5 ) -, m_k6( k6 ) -, m_k7( k7 ) -, m_p1( p1 ) -, m_p2( p2 ) -, m_A11( A11 ) -, m_A12( A12 ) -, m_A22( A22 ) -, m_u0( u0 ) -, m_v0( v0 ) -, m_isFast( isFast ) -, m_numDiff( FAST_NUM_DEFAULT ) -, m_maxIncidentAngle( FAST_MAX_INCIDENT_ANGLE ) -{ -} +PolyFisheyeCamera::Parameters::Parameters(const std::string& cameraName, int w, + int h, double k2, double k3, + double k4, double k5, double k6, + double k7, double p1, double p2, + double A11, double A12, double A22, + double u0, double v0, int isFast) + : Camera::Parameters(POLYFISHEYE, cameraName, w, h), + m_k2(k2), + m_k3(k3), + m_k4(k4), + m_k5(k5), + m_k6(k6), + m_k7(k7), + m_p1(p1), + m_p2(p2), + m_A11(A11), + m_A12(A12), + m_A22(A22), + m_u0(u0), + m_v0(v0), + m_isFast(isFast), + m_numDiff(FAST_NUM_DEFAULT), + m_maxIncidentAngle(FAST_MAX_INCIDENT_ANGLE) {} -double& -PolyFisheyeCamera::Parameters::k2( ) -{ - return m_k2; -} +double& PolyFisheyeCamera::Parameters::k2() { return m_k2; } -double& -PolyFisheyeCamera::Parameters::k3( ) -{ - return m_k3; -} +double& PolyFisheyeCamera::Parameters::k3() { return m_k3; } -double& -PolyFisheyeCamera::Parameters::k4( ) -{ - return m_k4; -} +double& PolyFisheyeCamera::Parameters::k4() { return m_k4; } -double& -PolyFisheyeCamera::Parameters::k5( ) -{ - return m_k5; -} +double& PolyFisheyeCamera::Parameters::k5() { return m_k5; } -double& -PolyFisheyeCamera::Parameters::k6( ) -{ - return m_k6; -} +double& PolyFisheyeCamera::Parameters::k6() { return m_k6; } -double& -PolyFisheyeCamera::Parameters::k7( ) -{ - return m_k7; -} +double& PolyFisheyeCamera::Parameters::k7() { return m_k7; } -double& -PolyFisheyeCamera::Parameters::p1( ) -{ - return m_p1; -} +double& PolyFisheyeCamera::Parameters::p1() { return m_p1; } -double& -PolyFisheyeCamera::Parameters::p2( ) -{ - return m_p2; -} +double& PolyFisheyeCamera::Parameters::p2() { return m_p2; } -double& -PolyFisheyeCamera::Parameters::A11( ) -{ - return m_A11; -} +double& PolyFisheyeCamera::Parameters::A11() { return m_A11; } -double& -PolyFisheyeCamera::Parameters::A12( ) -{ - return m_A12; -} +double& PolyFisheyeCamera::Parameters::A12() { return m_A12; } -double& -PolyFisheyeCamera::Parameters::A22( ) -{ - return m_A22; -} +double& PolyFisheyeCamera::Parameters::A22() { return m_A22; } -double& -PolyFisheyeCamera::Parameters::u0( ) -{ - return m_u0; -} +double& PolyFisheyeCamera::Parameters::u0() { return m_u0; } -double& -PolyFisheyeCamera::Parameters::v0( ) -{ - return m_v0; -} +double& PolyFisheyeCamera::Parameters::v0() { return m_v0; } -int& -PolyFisheyeCamera::Parameters::numDiff( ) -{ - return m_numDiff; -} +int& PolyFisheyeCamera::Parameters::numDiff() { return m_numDiff; } -int& -PolyFisheyeCamera::Parameters::maxIncidentAngle( ) -{ - return m_maxIncidentAngle; +int& PolyFisheyeCamera::Parameters::maxIncidentAngle() { + return m_maxIncidentAngle; } -int& -PolyFisheyeCamera::Parameters::isFast( ) -{ - return m_isFast; -} +int& PolyFisheyeCamera::Parameters::isFast() { return m_isFast; } -double -PolyFisheyeCamera::Parameters::k2( ) const -{ - return m_k2; -} +double PolyFisheyeCamera::Parameters::k2() const { return m_k2; } -double -PolyFisheyeCamera::Parameters::k3( ) const -{ - return m_k3; -} +double PolyFisheyeCamera::Parameters::k3() const { return m_k3; } -double -PolyFisheyeCamera::Parameters::k4( ) const -{ - return m_k4; -} +double PolyFisheyeCamera::Parameters::k4() const { return m_k4; } -double -PolyFisheyeCamera::Parameters::k5( ) const -{ - return m_k5; -} +double PolyFisheyeCamera::Parameters::k5() const { return m_k5; } -double -PolyFisheyeCamera::Parameters::k6( ) const -{ - return m_k6; -} +double PolyFisheyeCamera::Parameters::k6() const { return m_k6; } -double -PolyFisheyeCamera::Parameters::k7( ) const -{ - return m_k7; -} +double PolyFisheyeCamera::Parameters::k7() const { return m_k7; } -double -PolyFisheyeCamera::Parameters::p1( ) const -{ - return m_p1; -} +double PolyFisheyeCamera::Parameters::p1() const { return m_p1; } -double -PolyFisheyeCamera::Parameters::p2( ) const -{ - return m_p2; -} +double PolyFisheyeCamera::Parameters::p2() const { return m_p2; } -double -PolyFisheyeCamera::Parameters::A11( ) const -{ - return m_A11; -} +double PolyFisheyeCamera::Parameters::A11() const { return m_A11; } -double -PolyFisheyeCamera::Parameters::A12( ) const -{ - return m_A12; -} +double PolyFisheyeCamera::Parameters::A12() const { return m_A12; } -double -PolyFisheyeCamera::Parameters::A22( ) const -{ - return m_A22; -} +double PolyFisheyeCamera::Parameters::A22() const { return m_A22; } -double -PolyFisheyeCamera::Parameters::u0( ) const -{ - return m_u0; -} +double PolyFisheyeCamera::Parameters::u0() const { return m_u0; } -double -PolyFisheyeCamera::Parameters::v0( ) const -{ - return m_v0; -} +double PolyFisheyeCamera::Parameters::v0() const { return m_v0; } -int -PolyFisheyeCamera::Parameters::isFast( ) const -{ - return m_isFast; -} +int PolyFisheyeCamera::Parameters::isFast() const { return m_isFast; } -int -PolyFisheyeCamera::Parameters::numDiff( ) const -{ - return m_numDiff; -} +int PolyFisheyeCamera::Parameters::numDiff() const { return m_numDiff; } -int -PolyFisheyeCamera::Parameters::maxIncidentAngle( ) const -{ - return m_maxIncidentAngle; +int PolyFisheyeCamera::Parameters::maxIncidentAngle() const { + return m_maxIncidentAngle; } -void -PolyFisheyeCamera::Parameters::setFast( ) -{ - m_isFast = 1; -} +void PolyFisheyeCamera::Parameters::setFast() { m_isFast = 1; } -bool -PolyFisheyeCamera::Parameters::readFromYamlFile( const std::string& filename ) -{ - cv::FileStorage fs( filename, cv::FileStorage::READ ); +bool PolyFisheyeCamera::Parameters::readFromYamlFile( + const std::string& filename) { + cv::FileStorage fs(filename, cv::FileStorage::READ); - if ( !fs.isOpened( ) ) - { - return false; - } + if (!fs.isOpened()) { + return false; + } - if ( !fs["model_type"].isNone( ) ) - { - std::string sModelType; - fs["model_type"] >> sModelType; + if (!fs["model_type"].isNone()) { + std::string sModelType; + fs["model_type"] >> sModelType; - if ( sModelType.compare( "POLYFISHEYE" ) != 0 ) - { - return false; - } + if (sModelType.compare("POLYFISHEYE") != 0) { + return false; } + } + + m_modelType = POLYFISHEYE; + fs["camera_name"] >> m_cameraName; + m_imageWidth = static_cast(fs["image_width"]); + m_imageHeight = static_cast(fs["image_height"]); + + cv::FileNode n = fs["projection_parameters"]; + + m_k2 = static_cast(n["k2"]); + m_k3 = static_cast(n["k3"]); + m_k4 = static_cast(n["k4"]); + m_k5 = static_cast(n["k5"]); + m_k6 = static_cast(n["k6"]); + m_k7 = static_cast(n["k7"]); + m_p1 = static_cast(n["p1"]); + m_p2 = static_cast(n["p2"]); + m_A11 = static_cast(n["A11"]); + m_A12 = static_cast(n["A12"]); + m_A22 = static_cast(n["A22"]); + m_u0 = static_cast(n["u0"]); + m_v0 = static_cast(n["v0"]); + + m_isFast = static_cast(n["isFast"]); + m_numDiff = static_cast(n["numDiff"]); + m_maxIncidentAngle = static_cast(n["maxIncidentAngle"]); + + return true; +} + +bool PolyFisheyeCamera::Parameters::isDistortion() const { + return (m_k2 != 0.0); +} + +void PolyFisheyeCamera::Parameters::writeToYamlFile( + const std::string& filename) const { + cv::FileStorage fs(filename, cv::FileStorage::WRITE); + + fs << "model_type" + << "POLYFISHEYE"; + fs << "camera_name" << m_cameraName; + fs << "image_width" << m_imageWidth; + fs << "image_height" << m_imageHeight; + + // projection: k2, k3, k4, k5, k6, k7, A11, A22, u0, v0 + fs << "projection_parameters"; + fs << "{"; + + fs << "k2" << m_k2; + fs << "k3" << m_k3; + fs << "k4" << m_k4; + fs << "k5" << m_k5; + fs << "k6" << m_k6; + fs << "k7" << m_k7; + fs << "p1" << m_p1; + fs << "p2" << m_p2; + fs << "A11" << m_A11; + fs << "A12" << m_A12; + fs << "A22" << m_A22; + fs << "u0" << m_u0; + fs << "v0" << m_v0; + fs << "isFast" << m_isFast; + fs << "numDiff" << m_numDiff; + fs << "maxIncidentAngle" << m_maxIncidentAngle; + + fs << "}"; + + fs.release(); +} + +PolyFisheyeCamera::Parameters& PolyFisheyeCamera::Parameters::operator=( + const PolyFisheyeCamera::Parameters& other) { + if (this != &other) { + m_modelType = other.m_modelType; + m_cameraName = other.m_cameraName; + m_imageWidth = other.m_imageWidth; + m_imageHeight = other.m_imageHeight; - m_modelType = POLYFISHEYE; - fs["camera_name"] >> m_cameraName; - m_imageWidth = static_cast< int >( fs["image_width"] ); - m_imageHeight = static_cast< int >( fs["image_height"] ); - - cv::FileNode n = fs["projection_parameters"]; - - m_k2 = static_cast< double >( n["k2"] ); - m_k3 = static_cast< double >( n["k3"] ); - m_k4 = static_cast< double >( n["k4"] ); - m_k5 = static_cast< double >( n["k5"] ); - m_k6 = static_cast< double >( n["k6"] ); - m_k7 = static_cast< double >( n["k7"] ); - m_p1 = static_cast< double >( n["p1"] ); - m_p2 = static_cast< double >( n["p2"] ); - m_A11 = static_cast< double >( n["A11"] ); - m_A12 = static_cast< double >( n["A12"] ); - m_A22 = static_cast< double >( n["A22"] ); - m_u0 = static_cast< double >( n["u0"] ); - m_v0 = static_cast< double >( n["v0"] ); - - m_isFast = static_cast< int >( n["isFast"] ); - m_numDiff = static_cast< int >( n["numDiff"] ); - m_maxIncidentAngle = static_cast< int >( n["maxIncidentAngle"] ); - - return true; -} + m_k2 = other.m_k2; + m_k3 = other.m_k3; + m_k4 = other.m_k4; + m_k5 = other.m_k5; + m_k6 = other.m_k6; + m_k7 = other.m_k7; -bool -PolyFisheyeCamera::Parameters::isDistortion( ) const -{ - return ( m_k2 != 0.0 ); -} + m_p1 = other.m_p1; + m_p2 = other.m_p2; -void -PolyFisheyeCamera::Parameters::writeToYamlFile( const std::string& filename ) const -{ - cv::FileStorage fs( filename, cv::FileStorage::WRITE ); - - fs << "model_type" - << "POLYFISHEYE"; - fs << "camera_name" << m_cameraName; - fs << "image_width" << m_imageWidth; - fs << "image_height" << m_imageHeight; - - // projection: k2, k3, k4, k5, k6, k7, A11, A22, u0, v0 - fs << "projection_parameters"; - fs << "{"; - - fs << "k2" << m_k2; - fs << "k3" << m_k3; - fs << "k4" << m_k4; - fs << "k5" << m_k5; - fs << "k6" << m_k6; - fs << "k7" << m_k7; - fs << "p1" << m_p1; - fs << "p2" << m_p2; - fs << "A11" << m_A11; - fs << "A12" << m_A12; - fs << "A22" << m_A22; - fs << "u0" << m_u0; - fs << "v0" << m_v0; - fs << "isFast" << m_isFast; - fs << "numDiff" << m_numDiff; - fs << "maxIncidentAngle" << m_maxIncidentAngle; - - fs << "}"; - - fs.release( ); -} + m_A11 = other.m_A11; + m_A12 = other.m_A12; + m_A22 = other.m_A22; -PolyFisheyeCamera::Parameters& -PolyFisheyeCamera::Parameters::operator=( const PolyFisheyeCamera::Parameters& other ) -{ - if ( this != &other ) - { - m_modelType = other.m_modelType; - m_cameraName = other.m_cameraName; - m_imageWidth = other.m_imageWidth; - m_imageHeight = other.m_imageHeight; - - m_k2 = other.m_k2; - m_k3 = other.m_k3; - m_k4 = other.m_k4; - m_k5 = other.m_k5; - m_k6 = other.m_k6; - m_k7 = other.m_k7; - - m_p1 = other.m_p1; - m_p2 = other.m_p2; - - m_A11 = other.m_A11; - m_A12 = other.m_A12; - m_A22 = other.m_A22; - - m_u0 = other.m_u0; - m_v0 = other.m_v0; - - m_isFast = other.m_isFast; - m_numDiff = other.m_numDiff; - m_maxIncidentAngle = other.m_maxIncidentAngle; - } + m_u0 = other.m_u0; + m_v0 = other.m_v0; - return *this; -} + m_isFast = other.m_isFast; + m_numDiff = other.m_numDiff; + m_maxIncidentAngle = other.m_maxIncidentAngle; + } -std::ostream& -operator<<( std::ostream& out, const PolyFisheyeCamera::Parameters& params ) -{ - out << "Camera Parameters:" << std::endl; - out << "| model_type| " - << "POLYFISHEYE" << std::endl; - out << "| camera_name| " << params.m_cameraName << std::endl; - out << "| image_width| " << params.m_imageWidth << std::endl; - out << "| image_height| " << params.m_imageHeight << std::endl; - - // projection: k2, k3, k4, k5, k6, k7, A11, A22, u0, v0 - out << "Projection Parameters" << std::endl; - out << "polynomial model is:" << std::endl; - out << " r = " - << "x + "; - out << params.m_k2 << " x^2+ "; - out << params.m_k3 << " x^3+ "; - out << params.m_k4 << " x^4+ "; - out << params.m_k5 << " x^5+ "; - out << params.m_k6 << " x^6+ "; - out << params.m_k7 << " x^7" << std::endl; - - out << "| k2| " << params.m_k2 << std::endl; - out << "| k3| " << params.m_k3 << std::endl; - out << "| k4| " << params.m_k4 << std::endl; - out << "| k5| " << params.m_k5 << std::endl; - out << "| k6| " << params.m_k6 << std::endl; - out << "| k7| " << params.m_k7 << std::endl; - out << "| p1| " << params.m_p1 << std::endl; - out << "| p2| " << params.m_p2 << std::endl; - out << "| A11| " << params.m_A11 << std::endl; - out << "| A12| " << params.m_A12 << std::endl; - out << "| A22| " << params.m_A22 << std::endl; - out << "| u0| " << params.m_u0 << std::endl; - out << "| v0| " << params.m_v0 << std::endl; - out << "| isFast| " << params.m_isFast << std::endl; - out << "| max_theta| " << params.m_maxIncidentAngle << std::endl; - - return out; + return *this; } -void -PolyFisheyeCamera::FastCalcTABLE::setMaxIncidentAngle( double value ) -{ - maxIncidentAngle = value; - resetFastCalc( ); -} +std::ostream& operator<<(std::ostream& out, + const PolyFisheyeCamera::Parameters& params) { + out << "Camera Parameters:" << std::endl; + out << "| model_type| " + << "POLYFISHEYE" << std::endl; + out << "| camera_name| " << params.m_cameraName << std::endl; + out << "| image_width| " << params.m_imageWidth << std::endl; + out << "| image_height| " << params.m_imageHeight << std::endl; -void -PolyFisheyeCamera::FastCalcTABLE::setNumDiffAngle( int value ) -{ - numDiffAngle = value; - resetFastCalc( ); -} + // projection: k2, k3, k4, k5, k6, k7, A11, A22, u0, v0 + out << "Projection Parameters" << std::endl; + out << "polynomial model is:" << std::endl; + out << " r = " + << "x + "; + out << params.m_k2 << " x^2+ "; + out << params.m_k3 << " x^3+ "; + out << params.m_k4 << " x^4+ "; + out << params.m_k5 << " x^5+ "; + out << params.m_k6 << " x^6+ "; + out << params.m_k7 << " x^7" << std::endl; -void -PolyFisheyeCamera::FastCalcTABLE::setMaxImageR( double value ) -{ - maxImageR = value; - resetFastCalc( ); + out << "| k2| " << params.m_k2 << std::endl; + out << "| k3| " << params.m_k3 << std::endl; + out << "| k4| " << params.m_k4 << std::endl; + out << "| k5| " << params.m_k5 << std::endl; + out << "| k6| " << params.m_k6 << std::endl; + out << "| k7| " << params.m_k7 << std::endl; + out << "| p1| " << params.m_p1 << std::endl; + out << "| p2| " << params.m_p2 << std::endl; + out << "| A11| " << params.m_A11 << std::endl; + out << "| A12| " << params.m_A12 << std::endl; + out << "| A22| " << params.m_A22 << std::endl; + out << "| u0| " << params.m_u0 << std::endl; + out << "| v0| " << params.m_v0 << std::endl; + out << "| isFast| " << params.m_isFast << std::endl; + out << "| max_theta| " << params.m_maxIncidentAngle << std::endl; + + return out; } -void -PolyFisheyeCamera::FastCalcTABLE::setNumDiffR( int value ) -{ - numDiffR = value; - resetFastCalc( ); +void PolyFisheyeCamera::FastCalcTABLE::setMaxIncidentAngle(double value) { + maxIncidentAngle = value; + resetFastCalc(); } -eigen_utils::Matrix -PolyFisheyeCamera::FastCalcTABLE::getMatAngleToR( ) -{ - return angleToR; +void PolyFisheyeCamera::FastCalcTABLE::setNumDiffAngle(int value) { + numDiffAngle = value; + resetFastCalc(); } -eigen_utils::Matrix -PolyFisheyeCamera::FastCalcTABLE::getMatRToAngle( ) -{ - return rToAngle; +void PolyFisheyeCamera::FastCalcTABLE::setMaxImageR(double value) { + maxImageR = value; + resetFastCalc(); } -double -PolyFisheyeCamera::FastCalcTABLE::getMaxIncidentAngle( ) -{ - return maxIncidentAngle; +void PolyFisheyeCamera::FastCalcTABLE::setNumDiffR(int value) { + numDiffR = value; + resetFastCalc(); } -int -PolyFisheyeCamera::FastCalcTABLE::getNumDiff( ) -{ - return numDiffR; +eigen_utils::Matrix PolyFisheyeCamera::FastCalcTABLE::getMatAngleToR() { + return angleToR; } -double -PolyFisheyeCamera::FastCalcTABLE::getDiffAngle( ) -{ - return diffAngle; +eigen_utils::Matrix PolyFisheyeCamera::FastCalcTABLE::getMatRToAngle() { + return rToAngle; } -double -PolyFisheyeCamera::FastCalcTABLE::getDiffR( ) -{ - return diffR; +double PolyFisheyeCamera::FastCalcTABLE::getMaxIncidentAngle() { + return maxIncidentAngle; } -bool -PolyFisheyeCamera::FastCalcTABLE::calcAngleToR( eigen_utils::Matrix& _angleToR, - const int _numDiffAngle, - const double _diffAngle ) -{ - std::cout << "poly_angle " << fastPoly->getPolyCoeff( ).transpose( ) << std::endl; - - _angleToR.resize( _numDiffAngle + 1, 1 ); - - _angleToR( 0, 0 ) = 0; - // _angleToR(0, 1) = 0; - for ( int index = 1; index <= _numDiffAngle; ++index ) - { - // _angleToR(index, 0) = (double) index * _diffAngle; - _angleToR( index, 0 ) = fastPoly->getValue( ( double )index * _diffAngle ); - // std::cout<getPolyCoeff().transpose() + << std::endl; + + _angleToR.resize(_numDiffAngle + 1, 1); + + _angleToR(0, 0) = 0; + // _angleToR(0, 1) = 0; + for (int index = 1; index <= _numDiffAngle; ++index) { + // _angleToR(index, 0) = (double) index * _diffAngle; + _angleToR(index, 0) = fastPoly->getValue((double)index * _diffAngle); + // std::cout<getPolyCoeff( ).transpose( ) << std::endl; - - _rToAngle.resize( _numDiffR + 1, 1 ); - - _rToAngle( 0, 0 ) = 0; - // _rToAngle(0, 1) = 0; - for ( int index = 1; index <= _numDiffR; ++index ) - { - // _rToAngle(index, 0) = index * _diffR; - _rToAngle( index, 0 ) = +bool PolyFisheyeCamera::FastCalcTABLE::calcRToAngle( + eigen_utils::Matrix& _rToAngle, const int _numDiffR, const double _diffR, + const double _maxangle) { + std::cout << "poly_r " << fastPoly->getPolyCoeff().transpose() << std::endl; + + _rToAngle.resize(_numDiffR + 1, 1); + + _rToAngle(0, 0) = 0; + // _rToAngle(0, 1) = 0; + for (int index = 1; index <= _numDiffR; ++index) { + // _rToAngle(index, 0) = index * _diffR; + _rToAngle(index, 0) = // fastPoly->getOneRealRoot(index * _diffR, _rToAngle(index-1, 0), // MAX_INCIDENT_ANGLE_DEGREE / // RAD2DEG); - fastPoly->getOneRealRoot( index * _diffR, _rToAngle( index - 1, 0 ), _maxangle ); + fastPoly->getOneRealRoot(index * _diffR, _rToAngle(index - 1, 0), + _maxangle); - // std::cout<getPolyCoeff( ).transpose( ) << std::endl; + std::cout << "fast_poly " << fastPoly->getPolyCoeff().transpose() + << std::endl; - resetFastCalc( ); + resetFastCalc(); } -void -PolyFisheyeCamera::FastCalcTABLE::backprojectSymmetric( const Eigen::Vector2d& p_u, - double& cos_theta, - double& sin_theta, - double& cos_phi, - double& sin_phi ) const -{ - double r = p_u.norm( ); - double theta; - // std::cout << "#INFO: r is " << r << std::endl; - - if ( r < 1e-10 ) - { - sin_phi = 0.0; - cos_phi = 1.0; - theta = 0.0; - } - else - { - sin_phi = p_u( 1 ) / r; - cos_phi = p_u( 0 ) / r; - - // std::cout << "#INFO: phi is " << phi << std::endl; - - double num = r / diffR; - int num_down = std::floor( num ); - int num_up = std::ceil( num ); - // std::cout << " r " << r << " diffR " << diffR << " num " << num << " numDiffR " - // << numDiffR - // << std::endl; - - if ( num >= numDiffR || num_up >= numDiffR ) - { - theta = 3.14; - // return; - } - else - { - double theta_up = rToAngle( num_up, 0 ); - double theta_down = rToAngle( num_down, 0 ); +void PolyFisheyeCamera::FastCalcTABLE::backprojectSymmetric( + const Eigen::Vector2d& p_u, double& cos_theta, double& sin_theta, + double& cos_phi, double& sin_phi) const { + double r = p_u.norm(); + double theta; + // std::cout << "#INFO: r is " << r << std::endl; - // linearlize the line with more than 1000 segment - theta = theta_down - + ( num - ( double )num_down ) * ( theta_up - theta_down ) / ( num_up - num_down ); - - // std::cout << "theta " << theta_down << " " << theta_up << " " << - // theta << std::endl; - } - } - // std::cout << "theta " << theta << std::endl; + if (r < 1e-10) { + sin_phi = 0.0; + cos_phi = 1.0; + theta = 0.0; + } else { + sin_phi = p_u(1) / r; + cos_phi = p_u(0) / r; - sin_theta = sin( theta ); - cos_theta = cos( theta ); -} + // std::cout << "#INFO: phi is " << phi << std::endl; -double -PolyFisheyeCamera::FastCalcTABLE::r( const double theta ) const -{ - if ( theta > 1e-10 && theta < maxIncidentAngle ) - { - double num = theta / diffAngle; - int num_down = std::floor( num ); - int num_up = std::ceil( num ); - - if ( num >= numDiffAngle || num_up >= numDiffAngle ) - { - return angleToR( numDiffAngle, 0 ); - } - else - { - double r_up = angleToR( num_up, 0 ); - double r_down = angleToR( num_down, 0 ); - - // linearlize the line with more than 1000 segment - return ( r_down + ( num - ( double )num_down ) * ( r_up - r_down ) / ( num_up - num_down ) ); - // std::cout << "theta "<< theta_down<< " " <= numDiffR || num_up >= numDiffR) { + theta = 3.14; + // return; + } else { + double theta_up = rToAngle(num_up, 0); + double theta_down = rToAngle(num_down, 0); + + // linearlize the line with more than 1000 segment + theta = theta_down + (num - (double)num_down) * (theta_up - theta_down) / + (num_up - num_down); + + // std::cout << "theta " << theta_down << " " << theta_up << " + // " << theta << std::endl; } - else - return 0.0; + } + // std::cout << "theta " << theta << std::endl; + + sin_theta = sin(theta); + cos_theta = cos(theta); +} + +double PolyFisheyeCamera::FastCalcTABLE::r(const double theta) const { + if (theta > 1e-10 && theta < maxIncidentAngle) { + double num = theta / diffAngle; + int num_down = std::floor(num); + int num_up = std::ceil(num); + + if (num >= numDiffAngle || num_up >= numDiffAngle) { + return angleToR(numDiffAngle, 0); + } else { + double r_up = angleToR(num_up, 0); + double r_down = angleToR(num_down, 0); + + // linearlize the line with more than 1000 segment + return (r_down + + (num - (double)num_down) * (r_up - r_down) / (num_up - num_down)); + // std::cout << "theta "<< theta_down<< " " <getPolyCoeff( ).transpose( ) << std::endl; + std::cout << "fast_poly " << fastPoly->getPolyCoeff().transpose() + << std::endl; - resetFastCalc( ); - std::cout << "fastRootPoly " << fastRootPoly->getPolyCoeff( ).transpose( ) << std::endl; + resetFastCalc(); + std::cout << "fastRootPoly " << fastRootPoly->getPolyCoeff().transpose() + << std::endl; } -void -PolyFisheyeCamera::FastCalcPOLY::backprojectSymmetric( const Eigen::Vector2d& p_u, - double& cos_theta, - double& sin_theta, - double& cos_phi, - double& sin_phi ) const -{ - double r = p_u.norm( ); - // std::cout << "#INFO: r is " << r << std::endl; - - if ( r < 1e-10 ) - { - sin_phi = 0.0; - cos_phi = 1.0; - } - else - { - sin_phi = p_u( 1 ) / r; - cos_phi = p_u( 0 ) / r; - } - // std::cout << "#INFO: phi is " << phi << std::endl; +void PolyFisheyeCamera::FastCalcPOLY::backprojectSymmetric( + const Eigen::Vector2d& p_u, double& cos_theta, double& sin_theta, + double& cos_phi, double& sin_phi) const { + double r = p_u.norm(); + // std::cout << "#INFO: r is " << r << std::endl; + + if (r < 1e-10) { + sin_phi = 0.0; + cos_phi = 1.0; + } else { + sin_phi = p_u(1) / r; + cos_phi = p_u(0) / r; + } + // std::cout << "#INFO: phi is " << phi << std::endl; - double theta = this->fastRootPoly->getValue( r ); - cos_theta = cos( theta ); - sin_theta = sin( theta ); - // std::cout << "#INFO: theta is " << theta << std::endl; + double theta = this->fastRootPoly->getValue(r); + cos_theta = cos(theta); + sin_theta = sin(theta); + // std::cout << "#INFO: theta is " << theta << std::endl; } -double -PolyFisheyeCamera::FastCalcPOLY::r( const double theta ) const -{ - if ( theta > 1e-10 && theta < maxIncidentAngle ) - { - return fastPoly->getValue( theta ); - } - else - return 0.0; +double PolyFisheyeCamera::FastCalcPOLY::r(const double theta) const { + if (theta > 1e-10 && theta < maxIncidentAngle) { + return fastPoly->getValue(theta); + } else + return 0.0; } -void -PolyFisheyeCamera::FastCalcPOLY::setMaxIncidentAngle( double value ) -{ - maxIncidentAngle = value; - resetFastCalc( ); +void PolyFisheyeCamera::FastCalcPOLY::setMaxIncidentAngle(double value) { + maxIncidentAngle = value; + resetFastCalc(); } -void -PolyFisheyeCamera::FastCalcPOLY::setMaxImageR( double value ) -{ - maxImageR = value; - resetFastCalc( ); +void PolyFisheyeCamera::FastCalcPOLY::setMaxImageR(double value) { + maxImageR = value; + resetFastCalc(); } -void -PolyFisheyeCamera::FastCalcPOLY::resetFastCalc( ) -{ - int numDiffAngle = 1000; - double diffAngle = maxIncidentAngle / numDiffAngle; +void PolyFisheyeCamera::FastCalcPOLY::resetFastCalc() { + int numDiffAngle = 1000; + double diffAngle = maxIncidentAngle / numDiffAngle; - eigen_utils::Vector thetas( numDiffAngle + 1 ); - for ( int index = 0; index <= numDiffAngle; ++index ) - thetas( index ) = diffAngle * index; - std::cout << "thetas " << thetas.transpose( ) << std::endl; + eigen_utils::Vector thetas(numDiffAngle + 1); + for (int index = 0; index <= numDiffAngle; ++index) + thetas(index) = diffAngle * index; + std::cout << "thetas " << thetas.transpose() << std::endl; - eigen_utils::Vector rs( numDiffAngle + 1 ); - rs = fastPoly->getValue( thetas ); - std::cout << "rs " << rs.transpose( ) << std::endl; + eigen_utils::Vector rs(numDiffAngle + 1); + rs = fastPoly->getValue(thetas); + std::cout << "rs " << rs.transpose() << std::endl; - math_utils::PolynomialFit rootPolyFit( 7, rs, thetas ); - math_utils::Polynomial poly = rootPolyFit.getCoeff( ); + math_utils::PolynomialFit rootPolyFit(7, rs, thetas); + math_utils::Polynomial poly = rootPolyFit.getCoeff(); - std::cout << "fastRootPolyIN " << poly.getPolyCoeff( ).transpose( ) << std::endl; + std::cout << "fastRootPolyIN " << poly.getPolyCoeff().transpose() + << std::endl; - *fastRootPoly = rootPolyFit.getPolynomial( ); -} + *fastRootPoly = rootPolyFit.getPolynomial(); } +} // namespace camodocal diff --git a/camera_models/src/camera_models/ScaramuzzaCamera.cc b/camera_models/src/camera_models/ScaramuzzaCamera.cc index 3615594a..12cab999 100644 --- a/camera_models/src/camera_models/ScaramuzzaCamera.cc +++ b/camera_models/src/camera_models/ScaramuzzaCamera.cc @@ -1,5 +1,7 @@ #include "camodocal/camera_models/ScaramuzzaCamera.h" +#include +#include #include #include #include @@ -9,664 +11,644 @@ #include #include #include -#include -#include #include "camodocal/gpl/gpl.h" +Eigen::VectorXd polyfit(Eigen::VectorXd& xVec, Eigen::VectorXd& yVec, + int poly_order) { + assert(poly_order > 0); + assert(xVec.size() > poly_order); + assert(xVec.size() == yVec.size()); -Eigen::VectorXd polyfit(Eigen::VectorXd& xVec, Eigen::VectorXd& yVec, int poly_order) { - assert(poly_order > 0); - assert(xVec.size() > poly_order); - assert(xVec.size() == yVec.size()); + Eigen::MatrixXd A(xVec.size(), poly_order + 1); + Eigen::VectorXd B(xVec.size()); - Eigen::MatrixXd A(xVec.size(), poly_order+1); - Eigen::VectorXd B(xVec.size()); + for (int i = 0; i < xVec.size(); ++i) { + const double x = xVec(i); + const double y = yVec(i); - for(int i = 0; i < xVec.size(); ++i) { - const double x = xVec(i); - const double y = yVec(i); + double x_pow_k = 1.0; - double x_pow_k = 1.0; - - for(int k=0; k<=poly_order; ++k) { - A(i,k) = x_pow_k; - x_pow_k *= x; - } - - B(i) = y; + for (int k = 0; k <= poly_order; ++k) { + A(i, k) = x_pow_k; + x_pow_k *= x; } - Eigen::JacobiSVD svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV); - Eigen::VectorXd x = svd.solve(B); + B(i) = y; + } + + Eigen::JacobiSVD svd( + A, Eigen::ComputeThinU | Eigen::ComputeThinV); + Eigen::VectorXd x = svd.solve(B); - return x; + return x; } -namespace camodocal -{ +namespace camodocal { OCAMCamera::Parameters::Parameters() - : Camera::Parameters(SCARAMUZZA) - , m_C(0.0) - , m_D(0.0) - , m_E(0.0) - , m_center_x(0.0) - , m_center_y(0.0) -{ - memset(m_poly, 0, sizeof(double) * SCARAMUZZA_POLY_SIZE); - memset(m_inv_poly, 0, sizeof(double) * SCARAMUZZA_INV_POLY_SIZE); + : Camera::Parameters(SCARAMUZZA), + m_C(0.0), + m_D(0.0), + m_E(0.0), + m_center_x(0.0), + m_center_y(0.0) { + memset(m_poly, 0, sizeof(double) * SCARAMUZZA_POLY_SIZE); + memset(m_inv_poly, 0, sizeof(double) * SCARAMUZZA_INV_POLY_SIZE); } +bool OCAMCamera::Parameters::readFromYamlFile(const std::string& filename) { + cv::FileStorage fs(filename, cv::FileStorage::READ); + if (!fs.isOpened()) { + return false; + } -bool -OCAMCamera::Parameters::readFromYamlFile(const std::string& filename) -{ - cv::FileStorage fs(filename, cv::FileStorage::READ); - - if (!fs.isOpened()) - { - return false; - } - - if (!fs["model_type"].isNone()) - { - std::string sModelType; - fs["model_type"] >> sModelType; + if (!fs["model_type"].isNone()) { + std::string sModelType; + fs["model_type"] >> sModelType; - if (!boost::iequals(sModelType, "scaramuzza")) - { - return false; - } + if (!boost::iequals(sModelType, "scaramuzza")) { + return false; } + } - m_modelType = SCARAMUZZA; - fs["camera_name"] >> m_cameraName; - m_imageWidth = static_cast(fs["image_width"]); - m_imageHeight = static_cast(fs["image_height"]); + m_modelType = SCARAMUZZA; + fs["camera_name"] >> m_cameraName; + m_imageWidth = static_cast(fs["image_width"]); + m_imageHeight = static_cast(fs["image_height"]); - cv::FileNode n = fs["poly_parameters"]; - for(int i=0; i < SCARAMUZZA_POLY_SIZE; i++) - m_poly[i] = static_cast(n[std::string("p") + boost::lexical_cast(i)]); + cv::FileNode n = fs["poly_parameters"]; + for (int i = 0; i < SCARAMUZZA_POLY_SIZE; i++) + m_poly[i] = static_cast( + n[std::string("p") + boost::lexical_cast(i)]); - n = fs["inv_poly_parameters"]; - for(int i=0; i < SCARAMUZZA_INV_POLY_SIZE; i++) - m_inv_poly[i] = static_cast(n[std::string("p") + boost::lexical_cast(i)]); + n = fs["inv_poly_parameters"]; + for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++) + m_inv_poly[i] = static_cast( + n[std::string("p") + boost::lexical_cast(i)]); - n = fs["affine_parameters"]; - m_C = static_cast(n["ac"]); - m_D = static_cast(n["ad"]); - m_E = static_cast(n["ae"]); + n = fs["affine_parameters"]; + m_C = static_cast(n["ac"]); + m_D = static_cast(n["ad"]); + m_E = static_cast(n["ae"]); - m_center_x = static_cast(n["cx"]); - m_center_y = static_cast(n["cy"]); + m_center_x = static_cast(n["cx"]); + m_center_y = static_cast(n["cy"]); - return true; + return true; } -void -OCAMCamera::Parameters::writeToYamlFile(const std::string& filename) const -{ - cv::FileStorage fs(filename, cv::FileStorage::WRITE); - - fs << "model_type" << "scaramuzza"; - fs << "camera_name" << m_cameraName; - fs << "image_width" << m_imageWidth; - fs << "image_height" << m_imageHeight; - - fs << "poly_parameters"; - fs << "{"; - for(int i=0; i < SCARAMUZZA_POLY_SIZE; i++) - fs << std::string("p") + boost::lexical_cast(i) << m_poly[i]; - fs << "}"; - - fs << "inv_poly_parameters"; - fs << "{"; - for(int i=0; i < SCARAMUZZA_INV_POLY_SIZE; i++) - fs << std::string("p") + boost::lexical_cast(i) << m_inv_poly[i]; - fs << "}"; - - fs << "affine_parameters"; - fs << "{" << "ac" << m_C - << "ad" << m_D - << "ae" << m_E - << "cx" << m_center_x - << "cy" << m_center_y << "}"; - - fs.release(); +void OCAMCamera::Parameters::writeToYamlFile( + const std::string& filename) const { + cv::FileStorage fs(filename, cv::FileStorage::WRITE); + + fs << "model_type" + << "scaramuzza"; + fs << "camera_name" << m_cameraName; + fs << "image_width" << m_imageWidth; + fs << "image_height" << m_imageHeight; + + fs << "poly_parameters"; + fs << "{"; + for (int i = 0; i < SCARAMUZZA_POLY_SIZE; i++) + fs << std::string("p") + boost::lexical_cast(i) << m_poly[i]; + fs << "}"; + + fs << "inv_poly_parameters"; + fs << "{"; + for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++) + fs << std::string("p") + boost::lexical_cast(i) + << m_inv_poly[i]; + fs << "}"; + + fs << "affine_parameters"; + fs << "{" + << "ac" << m_C << "ad" << m_D << "ae" << m_E << "cx" << m_center_x << "cy" + << m_center_y << "}"; + + fs.release(); } -OCAMCamera::Parameters& -OCAMCamera::Parameters::operator=(const OCAMCamera::Parameters& other) -{ - if (this != &other) - { - m_modelType = other.m_modelType; - m_cameraName = other.m_cameraName; - m_imageWidth = other.m_imageWidth; - m_imageHeight = other.m_imageHeight; - m_C = other.m_C; - m_D = other.m_D; - m_E = other.m_E; - m_center_x = other.m_center_x; - m_center_y = other.m_center_y; - - memcpy(m_poly, other.m_poly, sizeof(double) * SCARAMUZZA_POLY_SIZE); - memcpy(m_inv_poly, other.m_inv_poly, sizeof(double) * SCARAMUZZA_INV_POLY_SIZE); - } - - return *this; +OCAMCamera::Parameters& OCAMCamera::Parameters::operator=( + const OCAMCamera::Parameters& other) { + if (this != &other) { + m_modelType = other.m_modelType; + m_cameraName = other.m_cameraName; + m_imageWidth = other.m_imageWidth; + m_imageHeight = other.m_imageHeight; + m_C = other.m_C; + m_D = other.m_D; + m_E = other.m_E; + m_center_x = other.m_center_x; + m_center_y = other.m_center_y; + + memcpy(m_poly, other.m_poly, sizeof(double) * SCARAMUZZA_POLY_SIZE); + memcpy(m_inv_poly, other.m_inv_poly, + sizeof(double) * SCARAMUZZA_INV_POLY_SIZE); + } + + return *this; } -std::ostream& -operator<< (std::ostream& out, const OCAMCamera::Parameters& params) -{ - out << "Camera Parameters:" << std::endl; - out << " model_type " << "scaramuzza" << std::endl; - out << " camera_name " << params.m_cameraName << std::endl; - out << " image_width " << params.m_imageWidth << std::endl; - out << " image_height " << params.m_imageHeight << std::endl; - - out << std::fixed << std::setprecision(10); - - out << "Poly Parameters" << std::endl; - for(int i=0; i < SCARAMUZZA_POLY_SIZE; i++) - out << std::string("p") + boost::lexical_cast(i) << ": " << params.m_poly[i] << std::endl; - - out << "Inverse Poly Parameters" << std::endl; - for(int i=0; i < SCARAMUZZA_INV_POLY_SIZE; i++) - out << std::string("p") + boost::lexical_cast(i) << ": " << params.m_inv_poly[i] << std::endl; - - out << "Affine Parameters" << std::endl; - out << " ac " << params.m_C << std::endl - << " ad " << params.m_D << std::endl - << " ae " << params.m_E << std::endl; - out << " cx " << params.m_center_x << std::endl - << " cy " << params.m_center_y << std::endl; - - return out; +std::ostream& operator<<(std::ostream& out, + const OCAMCamera::Parameters& params) { + out << "Camera Parameters:" << std::endl; + out << " model_type " + << "scaramuzza" << std::endl; + out << " camera_name " << params.m_cameraName << std::endl; + out << " image_width " << params.m_imageWidth << std::endl; + out << " image_height " << params.m_imageHeight << std::endl; + + out << std::fixed << std::setprecision(10); + + out << "Poly Parameters" << std::endl; + for (int i = 0; i < SCARAMUZZA_POLY_SIZE; i++) + out << std::string("p") + boost::lexical_cast(i) << ": " + << params.m_poly[i] << std::endl; + + out << "Inverse Poly Parameters" << std::endl; + for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++) + out << std::string("p") + boost::lexical_cast(i) << ": " + << params.m_inv_poly[i] << std::endl; + + out << "Affine Parameters" << std::endl; + out << " ac " << params.m_C << std::endl + << " ad " << params.m_D << std::endl + << " ae " << params.m_E << std::endl; + out << " cx " << params.m_center_x << std::endl + << " cy " << params.m_center_y << std::endl; + + return out; } -OCAMCamera::OCAMCamera() - : m_inv_scale(0.0) -{ - -} +OCAMCamera::OCAMCamera() : m_inv_scale(0.0) {} OCAMCamera::OCAMCamera(const OCAMCamera::Parameters& params) - : mParameters(params) -{ - m_inv_scale = 1.0 / (params.C() - params.D() * params.E()); + : mParameters(params) { + m_inv_scale = 1.0 / (params.C() - params.D() * params.E()); } -Camera::ModelType -OCAMCamera::modelType(void) const -{ - return mParameters.modelType(); +Camera::ModelType OCAMCamera::modelType(void) const { + return mParameters.modelType(); } -const std::string& -OCAMCamera::cameraName(void) const -{ - return mParameters.cameraName(); -} - -int -OCAMCamera::imageWidth(void) const -{ - return mParameters.imageWidth(); +const std::string& OCAMCamera::cameraName(void) const { + return mParameters.cameraName(); } -int -OCAMCamera::imageHeight(void) const -{ - return mParameters.imageHeight(); -} +int OCAMCamera::imageWidth(void) const { return mParameters.imageWidth(); } + +int OCAMCamera::imageHeight(void) const { return mParameters.imageHeight(); } + +void OCAMCamera::estimateIntrinsics( + const cv::Size& boardSize, + const std::vector >& objectPoints, + const std::vector >& imagePoints) { + // std::cout << "OCAMCamera::estimateIntrinsics - NOT IMPLEMENTED" << + // std::endl; throw std::string("OCAMCamera::estimateIntrinsics - NOT + // IMPLEMENTED"); + + // Reference: Page 30 of + // " Scaramuzza, D. Omnidirectional Vision: from Calibration to Robot Motion + // Estimation, ETH Zurich. Thesis no. 17635." + // http://e-collection.library.ethz.ch/eserv/eth:30301/eth-30301-02.pdf + // Matlab code: calibrate.m + + // First, estimate every image's extrinsics parameters + std::vector RList; + std::vector TList; + + RList.reserve(imagePoints.size()); + TList.reserve(imagePoints.size()); + + // i-th image + for (size_t image_index = 0; image_index < imagePoints.size(); + ++image_index) { + const std::vector& objPts = objectPoints.at(image_index); + const std::vector& imgPts = imagePoints.at(image_index); + + assert(objPts.size() == imgPts.size()); + assert(objPts.size() == + static_cast(boardSize.width * boardSize.height)); + + Eigen::MatrixXd M(objPts.size(), 6); + + for (size_t corner_index = 0; corner_index < objPts.size(); + ++corner_index) { + double X = objPts.at(corner_index).x; + double Y = objPts.at(corner_index).y; + assert(objPts.at(corner_index).z == 0.0); + + double u = imgPts.at(corner_index).x; + double v = imgPts.at(corner_index).y; + + M(corner_index, 0) = -v * X; + M(corner_index, 1) = -v * Y; + M(corner_index, 2) = u * X; + M(corner_index, 3) = u * Y; + M(corner_index, 4) = -v; + M(corner_index, 5) = u; + } -void -OCAMCamera::estimateIntrinsics(const cv::Size& boardSize, - const std::vector< std::vector >& objectPoints, - const std::vector< std::vector >& imagePoints) -{ - // std::cout << "OCAMCamera::estimateIntrinsics - NOT IMPLEMENTED" << std::endl; - // throw std::string("OCAMCamera::estimateIntrinsics - NOT IMPLEMENTED"); + Eigen::JacobiSVD svd( + M, Eigen::ComputeFullU | Eigen::ComputeFullV); + assert(svd.matrixV().cols() == 6); + Eigen::VectorXd h = -svd.matrixV().col(5); + + // scaled version of R and T + const double sr11 = h(0); + const double sr12 = h(1); + const double sr21 = h(2); + const double sr22 = h(3); + const double st1 = h(4); + const double st2 = h(5); + + const double AA = square(sr11 * sr12 + sr21 * sr22); + const double BB = square(sr11) + square(sr21); + const double CC = square(sr12) + square(sr22); + + const double sr32_squared_1 = + (-(CC - BB) + sqrt(square(CC - BB) + 4.0 * AA)) / 2.0; + const double sr32_squared_2 = + (-(CC - BB) - sqrt(square(CC - BB) + 4.0 * AA)) / 2.0; + + // printf("rst = %.12f\n", sr32_squared_1*sr32_squared_1 + + // (CC-BB)*sr32_squared_1 - AA); + + std::vector sr32_squared_values; + if (sr32_squared_1 > 0) sr32_squared_values.push_back(sr32_squared_1); + if (sr32_squared_2 > 0) sr32_squared_values.push_back(sr32_squared_2); + assert(!sr32_squared_values.empty()); + + std::vector sr32_values; + std::vector sr31_values; + for (auto sr32_squared : sr32_squared_values) { + for (int sign = -1; sign <= 1; sign += 2) { + const double sr32 = static_cast(sign) * std::sqrt(sr32_squared); + sr32_values.push_back(sr32); + if (sr32_squared == 0.0) { + // sr31 can be calculated through norm equality, + // but it has positive and negative posibilities + // positive one + sr31_values.push_back(std::sqrt(CC - BB)); + // negative one + sr32_values.push_back(sr32); + sr31_values.push_back(-std::sqrt(CC - BB)); + + break; // skip the same situation + } else { + // sr31 can be calculated throught dot product == 0 + sr31_values.push_back(-(sr11 * sr12 + sr21 * sr22) / sr32); + } + } + } - // Reference: Page 30 of - // " Scaramuzza, D. Omnidirectional Vision: from Calibration to Robot Motion Estimation, ETH Zurich. Thesis no. 17635." - // http://e-collection.library.ethz.ch/eserv/eth:30301/eth-30301-02.pdf - // Matlab code: calibrate.m + // std::cout << "h= " << std::setprecision(12) << h.transpose() << + // std::endl; std::cout << "length: " << sr32_values.size() << " & " << + // sr31_values.size() << std::endl; + + assert(!sr31_values.empty()); + assert(sr31_values.size() == sr32_values.size()); + + std::vector H_values; + for (size_t i = 0; i < sr31_values.size(); ++i) { + const double sr31 = sr31_values.at(i); + const double sr32 = sr32_values.at(i); + const double lambda = 1.0 / sqrt(sr11 * sr11 + sr21 * sr21 + sr31 * sr31); + Eigen::Matrix3d H; + H.setZero(); + H(0, 0) = sr11; + H(0, 1) = sr12; + H(0, 2) = st1; + H(1, 0) = sr21; + H(1, 1) = sr22; + H(1, 2) = st2; + H(2, 0) = sr31; + H(2, 1) = sr32; + H(2, 2) = 0; + + H_values.push_back(lambda * H); + H_values.push_back(-lambda * H); + } - // First, estimate every image's extrinsics parameters - std::vector< Eigen::Matrix3d > RList; - std::vector< Eigen::Vector3d > TList; + for (auto& H : H_values) { + // std::cout << "H=\n" << H << std::endl; + Eigen::Matrix3d R; + R.col(0) = H.col(0); + R.col(1) = H.col(1); + R.col(2) = H.col(0).cross(H.col(1)); + // std::cout << "R33 = " << R(2,2) << std::endl; + } - RList.reserve(imagePoints.size()); - TList.reserve(imagePoints.size()); + std::vector H_candidates; + + for (auto& H : H_values) { + Eigen::MatrixXd A_mat(2 * imagePoints.at(image_index).size(), 4); + Eigen::VectorXd B_vec(2 * imagePoints.at(image_index).size()); + A_mat.setZero(); + B_vec.setZero(); + + size_t line_index = 0; + + // iterate images + const double& r11 = H(0, 0); + const double& r12 = H(0, 1); + // const double& r13 = H(0,2); + const double& r21 = H(1, 0); + const double& r22 = H(1, 1); + // const double& r23 = H(1,2); + const double& r31 = H(2, 0); + const double& r32 = H(2, 1); + // const double& r33 = H(2,2); + const double& t1 = H(0); + const double& t2 = H(1); + + // iterate chessboard corners in the image + for (size_t j = 0; j < imagePoints.at(image_index).size(); ++j) { + assert(line_index == 2 * j); + + const double& X = objectPoints.at(image_index).at(j).x; + const double& Y = objectPoints.at(image_index).at(j).y; + const double& u = imagePoints.at(image_index).at(j).x; + const double& v = imagePoints.at(image_index).at(j).y; + + double A = r21 * X + r22 * Y + t2; + double B = v * (r31 * X + r32 * Y); + double C = r11 * X + r12 * Y + t1; + double D = u * (r31 * X + r32 * Y); + double rou = std::sqrt(u * u + v * v); + + A_mat(line_index + 0, 0) = A; + A_mat(line_index + 1, 0) = C; + A_mat(line_index + 0, 1) = A * rou; + A_mat(line_index + 1, 1) = C * rou; + A_mat(line_index + 0, 2) = A * rou * rou; + A_mat(line_index + 1, 2) = C * rou * rou; + + A_mat(line_index + 0, 3) = -v; + A_mat(line_index + 1, 3) = -u; + B_vec(line_index + 0) = B; + B_vec(line_index + 1) = D; + + line_index += 2; + } + + assert(line_index == static_cast(A_mat.rows())); + + // pseudo-inverse for polynomial parameters and all t3s + { + Eigen::JacobiSVD svd( + A_mat, Eigen::ComputeThinU | Eigen::ComputeThinV); - // i-th image - for (size_t image_index = 0; image_index < imagePoints.size(); ++image_index) - { - const std::vector& objPts = objectPoints.at(image_index); - const std::vector& imgPts = imagePoints.at(image_index); - - assert(objPts.size() == imgPts.size()); - assert(objPts.size() == static_cast(boardSize.width * boardSize.height)); - - Eigen::MatrixXd M(objPts.size(), 6); - - for(size_t corner_index = 0; corner_index < objPts.size(); ++corner_index) { - double X = objPts.at(corner_index).x; - double Y = objPts.at(corner_index).y; - assert(objPts.at(corner_index).z==0.0); - - double u = imgPts.at(corner_index).x; - double v = imgPts.at(corner_index).y; - - M(corner_index, 0) = -v * X; - M(corner_index, 1) = -v * Y; - M(corner_index, 2) = u * X; - M(corner_index, 3) = u * Y; - M(corner_index, 4) = -v; - M(corner_index, 5) = u; - } - - Eigen::JacobiSVD svd(M, Eigen::ComputeFullU | Eigen::ComputeFullV); - assert(svd.matrixV().cols() == 6); - Eigen::VectorXd h = -svd.matrixV().col(5); - - // scaled version of R and T - const double sr11 = h(0); - const double sr12 = h(1); - const double sr21 = h(2); - const double sr22 = h(3); - const double st1 = h(4); - const double st2 = h(5); - - const double AA = square(sr11*sr12 + sr21*sr22); - const double BB = square(sr11) + square(sr21); - const double CC = square(sr12) + square(sr22); - - const double sr32_squared_1 = (- (CC-BB) + sqrt(square(CC-BB) + 4.0 * AA)) / 2.0; - const double sr32_squared_2 = (- (CC-BB) - sqrt(square(CC-BB) + 4.0 * AA)) / 2.0; - -// printf("rst = %.12f\n", sr32_squared_1*sr32_squared_1 + (CC-BB)*sr32_squared_1 - AA); - - std::vector sr32_squared_values; - if (sr32_squared_1 > 0) sr32_squared_values.push_back(sr32_squared_1); - if (sr32_squared_2 > 0) sr32_squared_values.push_back(sr32_squared_2); - assert(!sr32_squared_values.empty()); - - std::vector sr32_values; - std::vector sr31_values; - for (auto sr32_squared : sr32_squared_values) { - for(int sign = -1; sign <= 1; sign += 2) { - const double sr32 = static_cast(sign) * std::sqrt(sr32_squared); - sr32_values.push_back( sr32 ); - if (sr32_squared == 0.0) { - // sr31 can be calculated through norm equality, - // but it has positive and negative posibilities - // positive one - sr31_values.push_back(std::sqrt(CC-BB)); - // negative one - sr32_values.push_back( sr32 ); - sr31_values.push_back(-std::sqrt(CC-BB)); - - break; // skip the same situation - } else { - // sr31 can be calculated throught dot product == 0 - sr31_values.push_back(- (sr11*sr12 + sr21*sr22) / sr32); - } - } - } + Eigen::VectorXd x = svd.solve(B_vec); - // std::cout << "h= " << std::setprecision(12) << h.transpose() << std::endl; - // std::cout << "length: " << sr32_values.size() << " & " << sr31_values.size() << std::endl; - - assert(!sr31_values.empty()); - assert(sr31_values.size() == sr32_values.size()); - - std::vector H_values; - for(size_t i=0;i(), p); + Eigen::Vector2d p; + spaceToPlane(uo.cast(), p); - mapX.at(v,u) = p(0); - mapY.at(v,u) = p(1); - } + mapX.at(v, u) = p(0); + mapY.at(v, u) = p(1); } + } - cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false); + cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false); - cv::Mat K_rect_cv; - cv::eigen2cv(K_rect, K_rect_cv); - return K_rect_cv; + cv::Mat K_rect_cv; + cv::eigen2cv(K_rect, K_rect_cv); + return K_rect_cv; } -int -OCAMCamera::parameterCount(void) const -{ - return SCARAMUZZA_CAMERA_NUM_PARAMS; +int OCAMCamera::parameterCount(void) const { + return SCARAMUZZA_CAMERA_NUM_PARAMS; } -const OCAMCamera::Parameters& -OCAMCamera::getParameters(void) const -{ - return mParameters; +const OCAMCamera::Parameters& OCAMCamera::getParameters(void) const { + return mParameters; } -void -OCAMCamera::setParameters(const OCAMCamera::Parameters& parameters) -{ - mParameters = parameters; +void OCAMCamera::setParameters(const OCAMCamera::Parameters& parameters) { + mParameters = parameters; - m_inv_scale = 1.0 / (parameters.C() - parameters.D() * parameters.E()); + m_inv_scale = 1.0 / (parameters.C() - parameters.D() * parameters.E()); } -void -OCAMCamera::readParameters(const std::vector& parameterVec) -{ - if ((int)parameterVec.size() != parameterCount()) - { - return; - } +void OCAMCamera::readParameters(const std::vector& parameterVec) { + if ((int)parameterVec.size() != parameterCount()) { + return; + } - Parameters params = getParameters(); + Parameters params = getParameters(); - params.C() = parameterVec.at(0); - params.D() = parameterVec.at(1); - params.E() = parameterVec.at(2); - params.center_x() = parameterVec.at(3); - params.center_y() = parameterVec.at(4); - for (int i=0; i < SCARAMUZZA_POLY_SIZE; i++) - params.poly(i) = parameterVec.at(5+i); - for (int i=0; i < SCARAMUZZA_INV_POLY_SIZE; i++) - params.inv_poly(i) = parameterVec.at(5 + SCARAMUZZA_POLY_SIZE + i); + params.C() = parameterVec.at(0); + params.D() = parameterVec.at(1); + params.E() = parameterVec.at(2); + params.center_x() = parameterVec.at(3); + params.center_y() = parameterVec.at(4); + for (int i = 0; i < SCARAMUZZA_POLY_SIZE; i++) + params.poly(i) = parameterVec.at(5 + i); + for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++) + params.inv_poly(i) = parameterVec.at(5 + SCARAMUZZA_POLY_SIZE + i); - setParameters(params); + setParameters(params); } -void -OCAMCamera::writeParameters(std::vector& parameterVec) const -{ - parameterVec.resize(parameterCount()); - parameterVec.at(0) = mParameters.C(); - parameterVec.at(1) = mParameters.D(); - parameterVec.at(2) = mParameters.E(); - parameterVec.at(3) = mParameters.center_x(); - parameterVec.at(4) = mParameters.center_y(); - for (int i=0; i < SCARAMUZZA_POLY_SIZE; i++) - parameterVec.at(5+i) = mParameters.poly(i); - for (int i=0; i < SCARAMUZZA_INV_POLY_SIZE; i++) - parameterVec.at(5 + SCARAMUZZA_POLY_SIZE + i) = mParameters.inv_poly(i); +void OCAMCamera::writeParameters(std::vector& parameterVec) const { + parameterVec.resize(parameterCount()); + parameterVec.at(0) = mParameters.C(); + parameterVec.at(1) = mParameters.D(); + parameterVec.at(2) = mParameters.E(); + parameterVec.at(3) = mParameters.center_x(); + parameterVec.at(4) = mParameters.center_y(); + for (int i = 0; i < SCARAMUZZA_POLY_SIZE; i++) + parameterVec.at(5 + i) = mParameters.poly(i); + for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++) + parameterVec.at(5 + SCARAMUZZA_POLY_SIZE + i) = mParameters.inv_poly(i); } -void -OCAMCamera::writeParametersToYamlFile(const std::string& filename) const -{ - mParameters.writeToYamlFile(filename); +void OCAMCamera::writeParametersToYamlFile(const std::string& filename) const { + mParameters.writeToYamlFile(filename); } -std::string -OCAMCamera::parametersToString(void) const -{ - std::ostringstream oss; - oss << mParameters; +std::string OCAMCamera::parametersToString(void) const { + std::ostringstream oss; + oss << mParameters; - return oss.str(); + return oss.str(); } -} +} // namespace camodocal diff --git a/camera_models/src/chessboard/Chessboard.cc b/camera_models/src/chessboard/Chessboard.cc index 4c0a761d..a9b040c3 100644 --- a/camera_models/src/chessboard/Chessboard.cc +++ b/camera_models/src/chessboard/Chessboard.cc @@ -6,321 +6,266 @@ #include "camodocal/chessboard/ChessboardQuad.h" #include "camodocal/chessboard/Spline.h" -#define MAX_CONTOUR_APPROX 7 +#define MAX_CONTOUR_APPROX 7 -namespace camodocal -{ +namespace camodocal { Chessboard::Chessboard(cv::Size boardSize, cv::Mat& image) - : mBoardSize(boardSize) - , mCornersFound(false) -{ - if (image.channels() == 1) - { - cv::cvtColor(image, mSketch, CV_GRAY2BGR); - image.copyTo(mImage); - } - else - { - image.copyTo(mSketch); - cv::cvtColor(image, mImage, CV_BGR2GRAY); - } + : mBoardSize(boardSize), mCornersFound(false) { + if (image.channels() == 1) { + cv::cvtColor(image, mSketch, CV_GRAY2BGR); + image.copyTo(mImage); + } else { + image.copyTo(mSketch); + cv::cvtColor(image, mImage, CV_BGR2GRAY); + } } -void -Chessboard::findCorners(bool useOpenCV) -{ - mCornersFound = findChessboardCorners(mImage, mBoardSize, mCorners, - CV_CALIB_CB_ADAPTIVE_THRESH + - CV_CALIB_CB_NORMALIZE_IMAGE + - CV_CALIB_CB_FILTER_QUADS + - CV_CALIB_CB_FAST_CHECK, - useOpenCV); - - if (mCornersFound) - { - // draw chessboard corners - cv::drawChessboardCorners(mSketch, mBoardSize, mCorners, mCornersFound); - } +void Chessboard::findCorners(bool useOpenCV) { + mCornersFound = findChessboardCorners( + mImage, mBoardSize, mCorners, + CV_CALIB_CB_ADAPTIVE_THRESH + CV_CALIB_CB_NORMALIZE_IMAGE + + CV_CALIB_CB_FILTER_QUADS + CV_CALIB_CB_FAST_CHECK, + useOpenCV); + + if (mCornersFound) { + // draw chessboard corners + cv::drawChessboardCorners(mSketch, mBoardSize, mCorners, mCornersFound); + } } -const std::vector& -Chessboard::getCorners(void) const -{ - return mCorners; +const std::vector& Chessboard::getCorners(void) const { + return mCorners; } -bool -Chessboard::cornersFound(void) const -{ - return mCornersFound; -} +bool Chessboard::cornersFound(void) const { return mCornersFound; } -const cv::Mat& -Chessboard::getImage(void) const -{ - return mImage; -} +const cv::Mat& Chessboard::getImage(void) const { return mImage; } -const cv::Mat& -Chessboard::getSketch(void) const -{ - return mSketch; -} +const cv::Mat& Chessboard::getSketch(void) const { return mSketch; } -bool -Chessboard::findChessboardCorners(const cv::Mat& image, - const cv::Size& patternSize, - std::vector& corners, - int flags, bool useOpenCV) -{ - if (useOpenCV) - { - return cv::findChessboardCorners(image, patternSize, corners, flags); - } - else - { - return findChessboardCornersImproved(image, patternSize, corners, flags); - } +bool Chessboard::findChessboardCorners(const cv::Mat& image, + const cv::Size& patternSize, + std::vector& corners, + int flags, bool useOpenCV) { + if (useOpenCV) { + return cv::findChessboardCorners(image, patternSize, corners, flags); + } else { + return findChessboardCornersImproved(image, patternSize, corners, flags); + } } -bool -Chessboard::findChessboardCornersImproved(const cv::Mat& image, - const cv::Size& patternSize, - std::vector& corners, - int flags) -{ - /************************************************************************************\ - This is improved variant of chessboard corner detection algorithm that - uses a graph of connected quads. It is based on the code contributed - by Vladimir Vezhnevets and Philip Gruebele. - Here is the copyright notice from the original Vladimir's code: - =============================================================== - - The algorithms developed and implemented by Vezhnevets Vldimir - aka Dead Moroz (vvp@graphics.cs.msu.ru) - See http://graphics.cs.msu.su/en/research/calibration/opencv.html - for detailed information. - - Reliability additions and modifications made by Philip Gruebele. - pgruebele@cox.net - - Some improvements were made by: - 1) Martin Rufli - increased chance of correct corner matching - Rufli, M., Scaramuzza, D., and Siegwart, R. (2008), - Automatic Detection of Checkerboards on Blurred and Distorted Images, - Proceedings of the IEEE/RSJ International Conference on - Intelligent Robots and Systems (IROS 2008), Nice, France, September 2008. - 2) Lionel Heng - post-detection checks and better thresholding - - \************************************************************************************/ - - //int bestDilation = -1; - const int minDilations = 0; - const int maxDilations = 7; - - std::vector outputQuadGroup; - - if (image.depth() != CV_8U || image.channels() == 2) - { - return false; +bool Chessboard::findChessboardCornersImproved( + const cv::Mat& image, const cv::Size& patternSize, + std::vector& corners, int flags) { + /************************************************************************************\ + This is improved variant of chessboard corner detection algorithm that + uses a graph of connected quads. It is based on the code contributed + by Vladimir Vezhnevets and Philip Gruebele. + Here is the copyright notice from the original Vladimir's code: + =============================================================== + + The algorithms developed and implemented by Vezhnevets Vldimir + aka Dead Moroz (vvp@graphics.cs.msu.ru) + See http://graphics.cs.msu.su/en/research/calibration/opencv.html + for detailed information. + + Reliability additions and modifications made by Philip Gruebele. + pgruebele@cox.net + + Some improvements were made by: + 1) Martin Rufli - increased chance of correct corner matching + Rufli, M., Scaramuzza, D., and Siegwart, R. (2008), + Automatic Detection of Checkerboards on Blurred and Distorted Images, + Proceedings of the IEEE/RSJ International Conference on + Intelligent Robots and Systems (IROS 2008), Nice, France, September + 2008. 2) Lionel Heng - post-detection checks and better thresholding + + \************************************************************************************/ + + // int bestDilation = -1; + const int minDilations = 0; + const int maxDilations = 7; + + std::vector outputQuadGroup; + + if (image.depth() != CV_8U || image.channels() == 2) { + return false; + } + + if (patternSize.width < 2 || patternSize.height < 2) { + return false; + } + + if (patternSize.width > 127 || patternSize.height > 127) { + return false; + } + + cv::Mat img = image; + + // Image histogram normalization and + // BGR to Grayscale image conversion (if applicable) + // MARTIN: Set to "false" + if (image.channels() != 1 || (flags & CV_CALIB_CB_NORMALIZE_IMAGE)) { + cv::Mat norm_img(image.rows, image.cols, CV_8UC1); + + if (image.channels() != 1) { + cv::cvtColor(image, norm_img, CV_BGR2GRAY); + img = norm_img; } - if (patternSize.width < 2 || patternSize.height < 2) - { - return false; + if (flags & CV_CALIB_CB_NORMALIZE_IMAGE) { + cv::equalizeHist(image, norm_img); + img = norm_img; } + } - if (patternSize.width > 127 || patternSize.height > 127) - { - return false; - } - - cv::Mat img = image; - - // Image histogram normalization and - // BGR to Grayscale image conversion (if applicable) - // MARTIN: Set to "false" - if (image.channels() != 1 || (flags & CV_CALIB_CB_NORMALIZE_IMAGE)) - { - cv::Mat norm_img(image.rows, image.cols, CV_8UC1); - - if (image.channels() != 1) - { - cv::cvtColor(image, norm_img, CV_BGR2GRAY); - img = norm_img; - } - - if (flags & CV_CALIB_CB_NORMALIZE_IMAGE) - { - cv::equalizeHist(image, norm_img); - img = norm_img; - } + if (flags & CV_CALIB_CB_FAST_CHECK) { + if (!checkChessboard(img, patternSize)) { + return false; } - - if (flags & CV_CALIB_CB_FAST_CHECK) - { - if (!checkChessboard(img, patternSize)) - { - return false; + } + + // PART 1: FIND LARGEST PATTERN + //----------------------------------------------------------------------- + // Checker patterns are tried to be found by dilating the background and + // then applying a canny edge finder on the closed contours (checkers). + // Try one dilation run, but if the pattern is not found, repeat until + // max_dilations is reached. + + int prevSqrSize = 0; + bool found = false; + std::vector outputCorners; + + for (int k = 0; k < 6; ++k) { + for (int dilations = minDilations; dilations <= maxDilations; ++dilations) { + if (found) { + break; + } + + cv::Mat thresh_img; + + // convert the input grayscale image to binary (black-n-white) + if (flags & CV_CALIB_CB_ADAPTIVE_THRESH) { + int blockSize = lround(prevSqrSize == 0 ? std::min(img.cols, img.rows) * + (k % 2 == 0 ? 0.2 : 0.1) + : prevSqrSize * 2) | + 1; + + // convert to binary + cv::adaptiveThreshold(img, thresh_img, 255, CV_ADAPTIVE_THRESH_MEAN_C, + CV_THRESH_BINARY, blockSize, (k / 2) * 5); + } else { + // empiric threshold level + double mean = (cv::mean(img))[0]; + int thresh_level = lround(mean - 10); + thresh_level = std::max(thresh_level, 10); + + cv::threshold(img, thresh_img, thresh_level, 255, CV_THRESH_BINARY); + } + + // MARTIN's Code + // Use both a rectangular and a cross kernel. In this way, a more + // homogeneous dilation is performed, which is crucial for small, + // distorted checkers. Use the CROSS kernel first, since its action + // on the image is more subtle + cv::Mat kernel1 = cv::getStructuringElement( + CV_SHAPE_CROSS, cv::Size(3, 3), cv::Point(1, 1)); + cv::Mat kernel2 = cv::getStructuringElement(CV_SHAPE_RECT, cv::Size(3, 3), + cv::Point(1, 1)); + + if (dilations >= 1) cv::dilate(thresh_img, thresh_img, kernel1); + if (dilations >= 2) cv::dilate(thresh_img, thresh_img, kernel2); + if (dilations >= 3) cv::dilate(thresh_img, thresh_img, kernel1); + if (dilations >= 4) cv::dilate(thresh_img, thresh_img, kernel2); + if (dilations >= 5) cv::dilate(thresh_img, thresh_img, kernel1); + if (dilations >= 6) cv::dilate(thresh_img, thresh_img, kernel2); + + // In order to find rectangles that go to the edge, we draw a white + // line around the image edge. Otherwise FindContours will miss those + // clipped rectangle contours. The border color will be the image mean, + // because otherwise we risk screwing up filters like cvSmooth() + cv::rectangle(thresh_img, cv::Point(0, 0), + cv::Point(thresh_img.cols - 1, thresh_img.rows - 1), + CV_RGB(255, 255, 255), 3, 8); + + // Generate quadrangles in the following function + std::vector quads; + + generateQuads(quads, thresh_img, flags, dilations, true); + if (quads.empty()) { + continue; + } + + // The following function finds and assigns neighbor quads to every + // quadrangle in the immediate vicinity fulfilling certain + // prerequisites + findQuadNeighbors(quads, dilations); + + // The connected quads will be organized in groups. The following loop + // increases a "group_idx" identifier. + // The function "findConnectedQuads assigns all connected quads + // a unique group ID. + // If more quadrangles were assigned to a given group (i.e. connected) + // than are expected by the input variable "patternSize", the + // function "cleanFoundConnectedQuads" erases the surplus + // quadrangles by minimizing the convex hull of the remaining pattern. + + for (int group_idx = 0;; ++group_idx) { + std::vector quadGroup; + + findConnectedQuads(quads, quadGroup, group_idx, dilations); + + if (quadGroup.empty()) { + break; } - } - - // PART 1: FIND LARGEST PATTERN - //----------------------------------------------------------------------- - // Checker patterns are tried to be found by dilating the background and - // then applying a canny edge finder on the closed contours (checkers). - // Try one dilation run, but if the pattern is not found, repeat until - // max_dilations is reached. - int prevSqrSize = 0; - bool found = false; - std::vector outputCorners; + cleanFoundConnectedQuads(quadGroup, patternSize); - for (int k = 0; k < 6; ++k) - { - for (int dilations = minDilations; dilations <= maxDilations; ++dilations) - { - if (found) - { - break; - } + // The following function labels all corners of every quad + // with a row and column entry. + // "count" specifies the number of found quads in "quad_group" + // with group identifier "group_idx" + // The last parameter is set to "true", because this is the + // first function call and some initializations need to be + // made. + labelQuadGroup(quadGroup, patternSize, true); - cv::Mat thresh_img; + found = checkQuadGroup(quadGroup, outputCorners, patternSize); - // convert the input grayscale image to binary (black-n-white) - if (flags & CV_CALIB_CB_ADAPTIVE_THRESH) - { - int blockSize = lround(prevSqrSize == 0 ? - std::min(img.cols,img.rows)*(k%2 == 0 ? 0.2 : 0.1): prevSqrSize*2)|1; - - // convert to binary - cv::adaptiveThreshold(img, thresh_img, 255, CV_ADAPTIVE_THRESH_MEAN_C, CV_THRESH_BINARY, blockSize, (k/2)*5); - } - else - { - // empiric threshold level - double mean = (cv::mean(img))[0]; - int thresh_level = lround(mean - 10); - thresh_level = std::max(thresh_level, 10); - - cv::threshold(img, thresh_img, thresh_level, 255, CV_THRESH_BINARY); - } - - // MARTIN's Code - // Use both a rectangular and a cross kernel. In this way, a more - // homogeneous dilation is performed, which is crucial for small, - // distorted checkers. Use the CROSS kernel first, since its action - // on the image is more subtle - cv::Mat kernel1 = cv::getStructuringElement(CV_SHAPE_CROSS, cv::Size(3,3), cv::Point(1,1)); - cv::Mat kernel2 = cv::getStructuringElement(CV_SHAPE_RECT, cv::Size(3,3), cv::Point(1,1)); - - if (dilations >= 1) - cv::dilate(thresh_img, thresh_img, kernel1); - if (dilations >= 2) - cv::dilate(thresh_img, thresh_img, kernel2); - if (dilations >= 3) - cv::dilate(thresh_img, thresh_img, kernel1); - if (dilations >= 4) - cv::dilate(thresh_img, thresh_img, kernel2); - if (dilations >= 5) - cv::dilate(thresh_img, thresh_img, kernel1); - if (dilations >= 6) - cv::dilate(thresh_img, thresh_img, kernel2); - - // In order to find rectangles that go to the edge, we draw a white - // line around the image edge. Otherwise FindContours will miss those - // clipped rectangle contours. The border color will be the image mean, - // because otherwise we risk screwing up filters like cvSmooth() - cv::rectangle(thresh_img, cv::Point(0,0), - cv::Point(thresh_img.cols - 1, thresh_img.rows - 1), - CV_RGB(255,255,255), 3, 8); - - // Generate quadrangles in the following function - std::vector quads; - - generateQuads(quads, thresh_img, flags, dilations, true); - if (quads.empty()) - { - continue; - } - - // The following function finds and assigns neighbor quads to every - // quadrangle in the immediate vicinity fulfilling certain - // prerequisites - findQuadNeighbors(quads, dilations); - - // The connected quads will be organized in groups. The following loop - // increases a "group_idx" identifier. - // The function "findConnectedQuads assigns all connected quads - // a unique group ID. - // If more quadrangles were assigned to a given group (i.e. connected) - // than are expected by the input variable "patternSize", the - // function "cleanFoundConnectedQuads" erases the surplus - // quadrangles by minimizing the convex hull of the remaining pattern. - - for (int group_idx = 0; ; ++group_idx) - { - std::vector quadGroup; - - findConnectedQuads(quads, quadGroup, group_idx, dilations); - - if (quadGroup.empty()) - { - break; - } + float sumDist = 0; + int total = 0; - cleanFoundConnectedQuads(quadGroup, patternSize); - - // The following function labels all corners of every quad - // with a row and column entry. - // "count" specifies the number of found quads in "quad_group" - // with group identifier "group_idx" - // The last parameter is set to "true", because this is the - // first function call and some initializations need to be - // made. - labelQuadGroup(quadGroup, patternSize, true); - - found = checkQuadGroup(quadGroup, outputCorners, patternSize); - - float sumDist = 0; - int total = 0; - - for (int i = 0; i < (int)outputCorners.size(); ++i) - { - int ni = 0; - float avgi = outputCorners.at(i)->meanDist(ni); - sumDist += avgi * ni; - total += ni; - } - prevSqrSize = lround(sumDist / std::max(total, 1)); + for (int i = 0; i < (int)outputCorners.size(); ++i) { + int ni = 0; + float avgi = outputCorners.at(i)->meanDist(ni); + sumDist += avgi * ni; + total += ni; + } + prevSqrSize = lround(sumDist / std::max(total, 1)); - if (found && !checkBoardMonotony(outputCorners, patternSize)) - { - found = false; - } - } + if (found && !checkBoardMonotony(outputCorners, patternSize)) { + found = false; } + } } + } - if (!found) - { - return false; + if (!found) { + return false; + } else { + corners.clear(); + corners.reserve(outputCorners.size()); + for (size_t i = 0; i < outputCorners.size(); ++i) { + corners.push_back(outputCorners.at(i)->pt); } - else - { - corners.clear(); - corners.reserve(outputCorners.size()); - for (size_t i = 0; i < outputCorners.size(); ++i) - { - corners.push_back(outputCorners.at(i)->pt); - } - cv::cornerSubPix(image, corners, cv::Size(11, 11), cv::Size(-1,-1), - cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1)); + cv::cornerSubPix( + image, corners, cv::Size(11, 11), cv::Size(-1, -1), + cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1)); - return true; - } + return true; + } } //=========================================================================== @@ -328,715 +273,625 @@ Chessboard::findChessboardCornersImproved(const cv::Mat& image, //=========================================================================== // If we found too many connected quads, remove those which probably do not // belong. -void -Chessboard::cleanFoundConnectedQuads(std::vector& quadGroup, - cv::Size patternSize) -{ - cv::Point2f center(0.0f, 0.0f); - - // Number of quads this pattern should contain - int count = ((patternSize.width + 1)*(patternSize.height + 1) + 1)/2; - - // If we have more quadrangles than we should, try to eliminate duplicates - // or ones which don't belong to the pattern rectangle. Else go to the end - // of the function - if ((int)quadGroup.size() <= count) - { - return; +void Chessboard::cleanFoundConnectedQuads( + std::vector& quadGroup, cv::Size patternSize) { + cv::Point2f center(0.0f, 0.0f); + + // Number of quads this pattern should contain + int count = ((patternSize.width + 1) * (patternSize.height + 1) + 1) / 2; + + // If we have more quadrangles than we should, try to eliminate duplicates + // or ones which don't belong to the pattern rectangle. Else go to the end + // of the function + if ((int)quadGroup.size() <= count) { + return; + } + + // Create an array of quadrangle centers + std::vector centers; + centers.resize(quadGroup.size()); + + for (size_t i = 0; i < quadGroup.size(); ++i) { + cv::Point2f ci(0.0f, 0.0f); + ChessboardQuadPtr& q = quadGroup[i]; + + for (int j = 0; j < 4; ++j) { + ci += q->corners[j]->pt; } - // Create an array of quadrangle centers - std::vector centers; - centers.resize(quadGroup.size()); + ci *= 0.25f; - for (size_t i = 0; i < quadGroup.size(); ++i) - { - cv::Point2f ci(0.0f, 0.0f); - ChessboardQuadPtr& q = quadGroup[i]; + // Centers(i), is the geometric center of quad(i) + // Center, is the center of all found quads + centers[i] = ci; - for (int j = 0; j < 4; ++j) - { - ci += q->corners[j]->pt; - } + center += ci; + } - ci *= 0.25f; + center *= 1.0f / quadGroup.size(); - // Centers(i), is the geometric center of quad(i) - // Center, is the center of all found quads - centers[i] = ci; + // If we have more quadrangles than we should, we try to eliminate bad + // ones based on minimizing the bounding box. We iteratively remove the + // point which reduces the size of the bounding box of the blobs the most + // (since we want the rectangle to be as small as possible) remove the + // quadrange that causes the biggest reduction in pattern size until we + // have the correct number + while ((int)quadGroup.size() > count) { + double minBoxArea = DBL_MAX; + int minBoxAreaIndex = -1; - center += ci; - } + // For each point, calculate box area without that point + for (size_t skip = 0; skip < quadGroup.size(); ++skip) { + // get bounding rectangle + cv::Point2f temp = centers[skip]; + centers[skip] = center; - center *= 1.0f / quadGroup.size(); - - // If we have more quadrangles than we should, we try to eliminate bad - // ones based on minimizing the bounding box. We iteratively remove the - // point which reduces the size of the bounding box of the blobs the most - // (since we want the rectangle to be as small as possible) remove the - // quadrange that causes the biggest reduction in pattern size until we - // have the correct number - while ((int)quadGroup.size() > count) - { - double minBoxArea = DBL_MAX; - int minBoxAreaIndex = -1; - - // For each point, calculate box area without that point - for (size_t skip = 0; skip < quadGroup.size(); ++skip) - { - // get bounding rectangle - cv::Point2f temp = centers[skip]; - centers[skip] = center; - - std::vector hull; - cv::convexHull(centers, hull, true, true); - centers[skip] = temp; - - double hull_area = fabs(cv::contourArea(hull)); - - // remember smallest box area - if (hull_area < minBoxArea) - { - minBoxArea = hull_area; - minBoxAreaIndex = skip; - } - } + std::vector hull; + cv::convexHull(centers, hull, true, true); + centers[skip] = temp; - ChessboardQuadPtr& q0 = quadGroup[minBoxAreaIndex]; - - // remove any references to this quad as a neighbor - for (size_t i = 0; i < quadGroup.size(); ++i) - { - ChessboardQuadPtr& q = quadGroup.at(i); - for (int j = 0; j < 4; ++j) - { - if (q->neighbors[j] == q0) - { - q->neighbors[j].reset(); - q->count--; - for (int k = 0; k < 4; ++k) - { - if (q0->neighbors[k] == q) - { - q0->neighbors[k].reset(); - q0->count--; - break; - } - } - break; - } + double hull_area = fabs(cv::contourArea(hull)); + + // remember smallest box area + if (hull_area < minBoxArea) { + minBoxArea = hull_area; + minBoxAreaIndex = skip; + } + } + + ChessboardQuadPtr& q0 = quadGroup[minBoxAreaIndex]; + + // remove any references to this quad as a neighbor + for (size_t i = 0; i < quadGroup.size(); ++i) { + ChessboardQuadPtr& q = quadGroup.at(i); + for (int j = 0; j < 4; ++j) { + if (q->neighbors[j] == q0) { + q->neighbors[j].reset(); + q->count--; + for (int k = 0; k < 4; ++k) { + if (q0->neighbors[k] == q) { + q0->neighbors[k].reset(); + q0->count--; + break; } + } + break; } - // remove the quad - quadGroup.at(minBoxAreaIndex) = quadGroup.back(); - centers.at(minBoxAreaIndex) = centers.back(); - quadGroup.pop_back(); - centers.pop_back(); + } } + // remove the quad + quadGroup.at(minBoxAreaIndex) = quadGroup.back(); + centers.at(minBoxAreaIndex) = centers.back(); + quadGroup.pop_back(); + centers.pop_back(); + } } //=========================================================================== // FIND COONECTED QUADS //=========================================================================== -void -Chessboard::findConnectedQuads(std::vector& quads, - std::vector& group, - int group_idx, int dilation) -{ - ChessboardQuadPtr q; - - // Scan the array for a first unlabeled quad - for (size_t i = 0; i < quads.size(); ++i) - { - ChessboardQuadPtr& quad = quads.at(i); - - if (quad->count > 0 && quad->group_idx < 0) - { - q = quad; - break; - } +void Chessboard::findConnectedQuads(std::vector& quads, + std::vector& group, + int group_idx, int dilation) { + ChessboardQuadPtr q; + + // Scan the array for a first unlabeled quad + for (size_t i = 0; i < quads.size(); ++i) { + ChessboardQuadPtr& quad = quads.at(i); + + if (quad->count > 0 && quad->group_idx < 0) { + q = quad; + break; } + } - if (q.get() == 0) - { - return; - } + if (q.get() == 0) { + return; + } - // Recursively find a group of connected quads starting from the seed quad + // Recursively find a group of connected quads starting from the seed quad - std::vector stack; - stack.push_back(q); + std::vector stack; + stack.push_back(q); - group.push_back(q); - q->group_idx = group_idx; + group.push_back(q); + q->group_idx = group_idx; - while (!stack.empty()) - { - q = stack.back(); - stack.pop_back(); + while (!stack.empty()) { + q = stack.back(); + stack.pop_back(); - for (int i = 0; i < 4; ++i) - { - ChessboardQuadPtr& neighbor = q->neighbors[i]; + for (int i = 0; i < 4; ++i) { + ChessboardQuadPtr& neighbor = q->neighbors[i]; - // If he neighbor exists and the neighbor has more than 0 - // neighbors and the neighbor has not been classified yet. - if (neighbor.get() && neighbor->count > 0 && neighbor->group_idx < 0) - { - stack.push_back(neighbor); - group.push_back(neighbor); - neighbor->group_idx = group_idx; - } - } + // If he neighbor exists and the neighbor has more than 0 + // neighbors and the neighbor has not been classified yet. + if (neighbor.get() && neighbor->count > 0 && neighbor->group_idx < 0) { + stack.push_back(neighbor); + group.push_back(neighbor); + neighbor->group_idx = group_idx; + } } + } } -void -Chessboard::labelQuadGroup(std::vector& quadGroup, - cv::Size patternSize, bool firstRun) -{ - // If this is the first function call, a seed quad needs to be selected - if (firstRun) - { - // Search for the (first) quad with the maximum number of neighbors - // (usually 4). This will be our starting point. - int mark = -1; - int maxNeighborCount = 0; - for (size_t i = 0; i < quadGroup.size(); ++i) - { - ChessboardQuadPtr& q = quadGroup.at(i); - if (q->count > maxNeighborCount) - { - mark = i; - maxNeighborCount = q->count; - - if (maxNeighborCount == 4) - { - break; - } - } +void Chessboard::labelQuadGroup(std::vector& quadGroup, + cv::Size patternSize, bool firstRun) { + // If this is the first function call, a seed quad needs to be selected + if (firstRun) { + // Search for the (first) quad with the maximum number of neighbors + // (usually 4). This will be our starting point. + int mark = -1; + int maxNeighborCount = 0; + for (size_t i = 0; i < quadGroup.size(); ++i) { + ChessboardQuadPtr& q = quadGroup.at(i); + if (q->count > maxNeighborCount) { + mark = i; + maxNeighborCount = q->count; + + if (maxNeighborCount == 4) { + break; } - - // Mark the starting quad's (per definition) upper left corner with - //(0,0) and then proceed clockwise - // The following labeling sequence enshures a "right coordinate system" - ChessboardQuadPtr& q = quadGroup.at(mark); - - q->labeled = true; - - q->corners[0]->row = 0; - q->corners[0]->column = 0; - q->corners[1]->row = 0; - q->corners[1]->column = 1; - q->corners[2]->row = 1; - q->corners[2]->column = 1; - q->corners[3]->row = 1; - q->corners[3]->column = 0; + } } - - // Mark all other corners with their respective row and column - bool flagChanged = true; - while (flagChanged) - { - // First reset the flag to "false" - flagChanged = false; - - // Go through all quads top down is faster, since unlabeled quads will - // be inserted at the end of the list - for (int i = quadGroup.size() - 1; i >= 0; --i) - { - ChessboardQuadPtr& quad = quadGroup.at(i); - - // Check whether quad "i" has been labeled already - if (!quad->labeled) - { - // Check its neighbors, whether some of them have been labeled - // already - for (int j = 0; j < 4; j++) - { - // Check whether the neighbor exists (i.e. is not the NULL - // pointer) - if (quad->neighbors[j]) - { - ChessboardQuadPtr& quadNeighbor = quad->neighbors[j]; - - // Only proceed, if neighbor "j" was labeled - if (quadNeighbor->labeled) - { - // For every quad it could happen to pass here - // multiple times. We therefore "break" later. - // Check whitch of the neighbors corners is - // connected to the current quad - int connectedNeighborCornerId = -1; - for (int k = 0; k < 4; ++k) - { - if (quadNeighbor->neighbors[k] == quad) - { - connectedNeighborCornerId = k; - - // there is only one, therefore - break; - } - } - - - // For the following calculations we need the row - // and column of the connected neighbor corner and - // all other corners of the connected quad "j", - // clockwise (CW) - ChessboardCornerPtr& conCorner = quadNeighbor->corners[connectedNeighborCornerId]; - ChessboardCornerPtr& conCornerCW1 = quadNeighbor->corners[(connectedNeighborCornerId+1)%4]; - ChessboardCornerPtr& conCornerCW2 = quadNeighbor->corners[(connectedNeighborCornerId+2)%4]; - ChessboardCornerPtr& conCornerCW3 = quadNeighbor->corners[(connectedNeighborCornerId+3)%4]; - - quad->corners[j]->row = conCorner->row; - quad->corners[j]->column = conCorner->column; - quad->corners[(j+1)%4]->row = conCorner->row - conCornerCW2->row + conCornerCW3->row; - quad->corners[(j+1)%4]->column = conCorner->column - conCornerCW2->column + conCornerCW3->column; - quad->corners[(j+2)%4]->row = conCorner->row + conCorner->row - conCornerCW2->row; - quad->corners[(j+2)%4]->column = conCorner->column + conCorner->column - conCornerCW2->column; - quad->corners[(j+3)%4]->row = conCorner->row - conCornerCW2->row + conCornerCW1->row; - quad->corners[(j+3)%4]->column = conCorner->column - conCornerCW2->column + conCornerCW1->column; - - // Mark this quad as labeled - quad->labeled = true; - - // Changes have taken place, set the flag - flagChanged = true; - - // once is enough! - break; - } - } + // Mark the starting quad's (per definition) upper left corner with + //(0,0) and then proceed clockwise + // The following labeling sequence enshures a "right coordinate system" + ChessboardQuadPtr& q = quadGroup.at(mark); + + q->labeled = true; + + q->corners[0]->row = 0; + q->corners[0]->column = 0; + q->corners[1]->row = 0; + q->corners[1]->column = 1; + q->corners[2]->row = 1; + q->corners[2]->column = 1; + q->corners[3]->row = 1; + q->corners[3]->column = 0; + } + + // Mark all other corners with their respective row and column + bool flagChanged = true; + while (flagChanged) { + // First reset the flag to "false" + flagChanged = false; + + // Go through all quads top down is faster, since unlabeled quads will + // be inserted at the end of the list + for (int i = quadGroup.size() - 1; i >= 0; --i) { + ChessboardQuadPtr& quad = quadGroup.at(i); + + // Check whether quad "i" has been labeled already + if (!quad->labeled) { + // Check its neighbors, whether some of them have been labeled + // already + for (int j = 0; j < 4; j++) { + // Check whether the neighbor exists (i.e. is not the NULL + // pointer) + if (quad->neighbors[j]) { + ChessboardQuadPtr& quadNeighbor = quad->neighbors[j]; + + // Only proceed, if neighbor "j" was labeled + if (quadNeighbor->labeled) { + // For every quad it could happen to pass here + // multiple times. We therefore "break" later. + // Check whitch of the neighbors corners is + // connected to the current quad + int connectedNeighborCornerId = -1; + for (int k = 0; k < 4; ++k) { + if (quadNeighbor->neighbors[k] == quad) { + connectedNeighborCornerId = k; + + // there is only one, therefore + break; } + } + + // For the following calculations we need the row + // and column of the connected neighbor corner and + // all other corners of the connected quad "j", + // clockwise (CW) + ChessboardCornerPtr& conCorner = + quadNeighbor->corners[connectedNeighborCornerId]; + ChessboardCornerPtr& conCornerCW1 = + quadNeighbor->corners[(connectedNeighborCornerId + 1) % 4]; + ChessboardCornerPtr& conCornerCW2 = + quadNeighbor->corners[(connectedNeighborCornerId + 2) % 4]; + ChessboardCornerPtr& conCornerCW3 = + quadNeighbor->corners[(connectedNeighborCornerId + 3) % 4]; + + quad->corners[j]->row = conCorner->row; + quad->corners[j]->column = conCorner->column; + quad->corners[(j + 1) % 4]->row = + conCorner->row - conCornerCW2->row + conCornerCW3->row; + quad->corners[(j + 1) % 4]->column = conCorner->column - + conCornerCW2->column + + conCornerCW3->column; + quad->corners[(j + 2) % 4]->row = + conCorner->row + conCorner->row - conCornerCW2->row; + quad->corners[(j + 2) % 4]->column = + conCorner->column + conCorner->column - conCornerCW2->column; + quad->corners[(j + 3) % 4]->row = + conCorner->row - conCornerCW2->row + conCornerCW1->row; + quad->corners[(j + 3) % 4]->column = conCorner->column - + conCornerCW2->column + + conCornerCW1->column; + + // Mark this quad as labeled + quad->labeled = true; + + // Changes have taken place, set the flag + flagChanged = true; + + // once is enough! + break; } + } } + } } - - - // All corners are marked with row and column - // Record the minimal and maximal row and column indices - // It is unlikely that more than 8bit checkers are used per dimension, if there are - // an error would have been thrown at the beginning of "cvFindChessboardCorners2" - int min_row = 127; - int max_row = -127; - int min_column = 127; - int max_column = -127; - - for (int i = 0; i < (int)quadGroup.size(); ++i) - { - ChessboardQuadPtr& q = quadGroup.at(i); - - for (int j = 0; j < 4; ++j) - { - ChessboardCornerPtr& c = q->corners[j]; - - if (c->row > max_row) - { - max_row = c->row; - } - if (c->row < min_row) - { - min_row = c->row; - } - if (c->column > max_column) - { - max_column = c->column; - } - if (c->column < min_column) - { - min_column = c->column; - } - } + } + + // All corners are marked with row and column + // Record the minimal and maximal row and column indices + // It is unlikely that more than 8bit checkers are used per dimension, if + // there are an error would have been thrown at the beginning of + // "cvFindChessboardCorners2" + int min_row = 127; + int max_row = -127; + int min_column = 127; + int max_column = -127; + + for (int i = 0; i < (int)quadGroup.size(); ++i) { + ChessboardQuadPtr& q = quadGroup.at(i); + + for (int j = 0; j < 4; ++j) { + ChessboardCornerPtr& c = q->corners[j]; + + if (c->row > max_row) { + max_row = c->row; + } + if (c->row < min_row) { + min_row = c->row; + } + if (c->column > max_column) { + max_column = c->column; + } + if (c->column < min_column) { + min_column = c->column; + } } - - // Label all internal corners with "needsNeighbor" = false - // Label all external corners with "needsNeighbor" = true, - // except if in a given dimension the pattern size is reached - for (int i = min_row; i <= max_row; ++i) - { - for (int j = min_column; j <= max_column; ++j) - { - // A flag that indicates, whether a row/column combination is - // executed multiple times - bool flag = false; - - // Remember corner and quad - int cornerID; - int quadID; - - for (int k = 0; k < (int)quadGroup.size(); ++k) - { - ChessboardQuadPtr& q = quadGroup.at(k); - - for (int l = 0; l < 4; ++l) - { - if ((q->corners[l]->row == i) && (q->corners[l]->column == j)) - { - if (flag) - { - // Passed at least twice through here - q->corners[l]->needsNeighbor = false; - quadGroup[quadID]->corners[cornerID]->needsNeighbor = false; - } - else - { - // Mark with needs a neighbor, but note the - // address - q->corners[l]->needsNeighbor = true; - cornerID = l; - quadID = k; - } - - // set the flag to true - flag = true; - } - } + } + + // Label all internal corners with "needsNeighbor" = false + // Label all external corners with "needsNeighbor" = true, + // except if in a given dimension the pattern size is reached + for (int i = min_row; i <= max_row; ++i) { + for (int j = min_column; j <= max_column; ++j) { + // A flag that indicates, whether a row/column combination is + // executed multiple times + bool flag = false; + + // Remember corner and quad + int cornerID; + int quadID; + + for (int k = 0; k < (int)quadGroup.size(); ++k) { + ChessboardQuadPtr& q = quadGroup.at(k); + + for (int l = 0; l < 4; ++l) { + if ((q->corners[l]->row == i) && (q->corners[l]->column == j)) { + if (flag) { + // Passed at least twice through here + q->corners[l]->needsNeighbor = false; + quadGroup[quadID]->corners[cornerID]->needsNeighbor = false; + } else { + // Mark with needs a neighbor, but note the + // address + q->corners[l]->needsNeighbor = true; + cornerID = l; + quadID = k; } + + // set the flag to true + flag = true; + } } + } } + } + + // Complete Linking: + // sometimes not all corners were properly linked in "findQuadNeighbors", + // but after labeling each corner with its respective row and column, it is + // possible to match them anyway. + for (int i = min_row; i <= max_row; ++i) { + for (int j = min_column; j <= max_column; ++j) { + // the following "number" indicates the number of corners which + // correspond to the given (i,j) value + // 1 is a border corner or a conrer which still needs a neighbor + // 2 is a fully connected internal corner + // >2 something went wrong during labeling, report a warning + int number = 1; + + // remember corner and quad + int cornerID; + int quadID; + + for (int k = 0; k < (int)quadGroup.size(); ++k) { + ChessboardQuadPtr& q = quadGroup.at(k); + + for (int l = 0; l < 4; ++l) { + if ((q->corners[l]->row == i) && (q->corners[l]->column == j)) { + if (number == 1) { + // First corner, note its ID + cornerID = l; + quadID = k; + } - // Complete Linking: - // sometimes not all corners were properly linked in "findQuadNeighbors", - // but after labeling each corner with its respective row and column, it is - // possible to match them anyway. - for (int i = min_row; i <= max_row; ++i) - { - for (int j = min_column; j <= max_column; ++j) - { - // the following "number" indicates the number of corners which - // correspond to the given (i,j) value - // 1 is a border corner or a conrer which still needs a neighbor - // 2 is a fully connected internal corner - // >2 something went wrong during labeling, report a warning - int number = 1; - - // remember corner and quad - int cornerID; - int quadID; - - for (int k = 0; k < (int)quadGroup.size(); ++k) - { - ChessboardQuadPtr& q = quadGroup.at(k); - - for (int l = 0; l < 4; ++l) - { - if ((q->corners[l]->row == i) && (q->corners[l]->column == j)) - { - - if (number == 1) - { - // First corner, note its ID - cornerID = l; - quadID = k; - } - - else if (number == 2) - { - // Second corner, check wheter this and the - // first one have equal coordinates, else - // interpolate - cv::Point2f delta = q->corners[l]->pt - quadGroup[quadID]->corners[cornerID]->pt; - - if (delta.x != 0.0f || delta.y != 0.0f) - { - // Interpolate - q->corners[l]->pt -= delta * 0.5f; - - quadGroup[quadID]->corners[cornerID]->pt += delta * 0.5f; - } - } - else if (number > 2) - { - // Something went wrong during row/column labeling - // Report a Warning - // ->Implemented in the function "mrWriteCorners" - } - - // increase the number by one - ++number; - } - } + else if (number == 2) { + // Second corner, check wheter this and the + // first one have equal coordinates, else + // interpolate + cv::Point2f delta = + q->corners[l]->pt - quadGroup[quadID]->corners[cornerID]->pt; + + if (delta.x != 0.0f || delta.y != 0.0f) { + // Interpolate + q->corners[l]->pt -= delta * 0.5f; + + quadGroup[quadID]->corners[cornerID]->pt += delta * 0.5f; + } + } else if (number > 2) { + // Something went wrong during row/column labeling + // Report a Warning + // ->Implemented in the function "mrWriteCorners" } + + // increase the number by one + ++number; + } } + } } - - - // Bordercorners don't need any neighbors, if the pattern size in the - // respective direction is reached - // The only time we can make shure that the target pattern size is reached in a given - // dimension, is when the larger side has reached the target size in the maximal - // direction, or if the larger side is larger than the smaller target size and the - // smaller side equals the smaller target size - int largerDimPattern = std::max(patternSize.height,patternSize.width); - int smallerDimPattern = std::min(patternSize.height,patternSize.width); - bool flagSmallerDim1 = false; - bool flagSmallerDim2 = false; - - if ((largerDimPattern + 1) == max_column - min_column) - { - flagSmallerDim1 = true; - // We found out that in the column direction the target pattern size is reached - // Therefore border column corners do not need a neighbor anymore - // Go through all corners - for (int k = 0; k < (int)quadGroup.size(); ++k) - { - ChessboardQuadPtr& q = quadGroup.at(k); - - for (int l = 0; l < 4; ++l) - { - ChessboardCornerPtr& c = q->corners[l]; - - if (c->column == min_column || c->column == max_column) - { - // Needs no neighbor anymore - c->needsNeighbor = false; - } - } + } + + // Bordercorners don't need any neighbors, if the pattern size in the + // respective direction is reached + // The only time we can make shure that the target pattern size is reached in + // a given dimension, is when the larger side has reached the target size in + // the maximal direction, or if the larger side is larger than the smaller + // target size and the smaller side equals the smaller target size + int largerDimPattern = std::max(patternSize.height, patternSize.width); + int smallerDimPattern = std::min(patternSize.height, patternSize.width); + bool flagSmallerDim1 = false; + bool flagSmallerDim2 = false; + + if ((largerDimPattern + 1) == max_column - min_column) { + flagSmallerDim1 = true; + // We found out that in the column direction the target pattern size is + // reached Therefore border column corners do not need a neighbor anymore Go + // through all corners + for (int k = 0; k < (int)quadGroup.size(); ++k) { + ChessboardQuadPtr& q = quadGroup.at(k); + + for (int l = 0; l < 4; ++l) { + ChessboardCornerPtr& c = q->corners[l]; + + if (c->column == min_column || c->column == max_column) { + // Needs no neighbor anymore + c->needsNeighbor = false; } + } } - - if ((largerDimPattern + 1) == max_row - min_row) - { - flagSmallerDim2 = true; - // We found out that in the column direction the target pattern size is reached - // Therefore border column corners do not need a neighbor anymore - // Go through all corners - for (int k = 0; k < (int)quadGroup.size(); ++k) - { - ChessboardQuadPtr& q = quadGroup.at(k); - - for (int l = 0; l < 4; ++l) - { - ChessboardCornerPtr& c = q->corners[l]; - - if (c->row == min_row || c->row == max_row) - { - // Needs no neighbor anymore - c->needsNeighbor = false; - } - } + } + + if ((largerDimPattern + 1) == max_row - min_row) { + flagSmallerDim2 = true; + // We found out that in the column direction the target pattern size is + // reached Therefore border column corners do not need a neighbor anymore Go + // through all corners + for (int k = 0; k < (int)quadGroup.size(); ++k) { + ChessboardQuadPtr& q = quadGroup.at(k); + + for (int l = 0; l < 4; ++l) { + ChessboardCornerPtr& c = q->corners[l]; + + if (c->row == min_row || c->row == max_row) { + // Needs no neighbor anymore + c->needsNeighbor = false; } + } } - - - // Check the two flags: - // - If one is true and the other false, then the pattern target - // size was reached in in one direction -> We can check, whether the target - // pattern size is also reached in the other direction - // - If both are set to true, then we deal with a square board -> do nothing - // - If both are set to false -> There is a possibility that the larger side is - // larger than the smaller target size -> Check and if true, then check whether - // the other side has the same size as the smaller target size - if ((flagSmallerDim1 == false && flagSmallerDim2 == true)) - { - // Larger target pattern size is in row direction, check wheter smaller target - // pattern size is reached in column direction - if ((smallerDimPattern + 1) == max_column - min_column) - { - for (int k = 0; k < (int)quadGroup.size(); ++k) - { - ChessboardQuadPtr& q = quadGroup.at(k); - - for (int l = 0; l < 4; ++l) - { - ChessboardCornerPtr& c = q->corners[l]; - - if (c->column == min_column || c->column == max_column) - { - // Needs no neighbor anymore - c->needsNeighbor = false; - } - } - } + } + + // Check the two flags: + // - If one is true and the other false, then the pattern target + // size was reached in in one direction -> We can check, whether the + // target pattern size is also reached in the other direction + // - If both are set to true, then we deal with a square board -> do + // nothing + // - If both are set to false -> There is a possibility that the larger + // side is + // larger than the smaller target size -> Check and if true, then check + // whether the other side has the same size as the smaller target size + if ((flagSmallerDim1 == false && flagSmallerDim2 == true)) { + // Larger target pattern size is in row direction, check wheter smaller + // target pattern size is reached in column direction + if ((smallerDimPattern + 1) == max_column - min_column) { + for (int k = 0; k < (int)quadGroup.size(); ++k) { + ChessboardQuadPtr& q = quadGroup.at(k); + + for (int l = 0; l < 4; ++l) { + ChessboardCornerPtr& c = q->corners[l]; + + if (c->column == min_column || c->column == max_column) { + // Needs no neighbor anymore + c->needsNeighbor = false; + } } + } } - - if ((flagSmallerDim1 == true && flagSmallerDim2 == false)) - { - // Larger target pattern size is in column direction, check wheter smaller target - // pattern size is reached in row direction - if ((smallerDimPattern + 1) == max_row - min_row) - { - for (int k = 0; k < (int)quadGroup.size(); ++k) - { - ChessboardQuadPtr& q = quadGroup.at(k); - - for (int l = 0; l < 4; ++l) - { - ChessboardCornerPtr& c = q->corners[l]; - - if (c->row == min_row || c->row == max_row) - { - // Needs no neighbor anymore - c->needsNeighbor = false; - } - } - } + } + + if ((flagSmallerDim1 == true && flagSmallerDim2 == false)) { + // Larger target pattern size is in column direction, check wheter smaller + // target pattern size is reached in row direction + if ((smallerDimPattern + 1) == max_row - min_row) { + for (int k = 0; k < (int)quadGroup.size(); ++k) { + ChessboardQuadPtr& q = quadGroup.at(k); + + for (int l = 0; l < 4; ++l) { + ChessboardCornerPtr& c = q->corners[l]; + + if (c->row == min_row || c->row == max_row) { + // Needs no neighbor anymore + c->needsNeighbor = false; + } } + } } - - if ((flagSmallerDim1 == false && flagSmallerDim2 == false) && smallerDimPattern + 1 < max_column - min_column) - { - // Larger target pattern size is in column direction, check wheter smaller target - // pattern size is reached in row direction - if ((smallerDimPattern + 1) == max_row - min_row) - { - for (int k = 0; k < (int)quadGroup.size(); ++k) - { - ChessboardQuadPtr& q = quadGroup.at(k); - - for (int l = 0; l < 4; ++l) - { - ChessboardCornerPtr& c = q->corners[l]; - - if (c->row == min_row || c->row == max_row) - { - // Needs no neighbor anymore - c->needsNeighbor = false; - } - } - } + } + + if ((flagSmallerDim1 == false && flagSmallerDim2 == false) && + smallerDimPattern + 1 < max_column - min_column) { + // Larger target pattern size is in column direction, check wheter smaller + // target pattern size is reached in row direction + if ((smallerDimPattern + 1) == max_row - min_row) { + for (int k = 0; k < (int)quadGroup.size(); ++k) { + ChessboardQuadPtr& q = quadGroup.at(k); + + for (int l = 0; l < 4; ++l) { + ChessboardCornerPtr& c = q->corners[l]; + + if (c->row == min_row || c->row == max_row) { + // Needs no neighbor anymore + c->needsNeighbor = false; + } } + } } - - if ((flagSmallerDim1 == false && flagSmallerDim2 == false) && smallerDimPattern + 1 < max_row - min_row) - { - // Larger target pattern size is in row direction, check wheter smaller target - // pattern size is reached in column direction - if ((smallerDimPattern + 1) == max_column - min_column) - { - for (int k = 0; k < (int)quadGroup.size(); ++k) - { - ChessboardQuadPtr& q = quadGroup.at(k); - - for (int l = 0; l < 4; ++l) - { - ChessboardCornerPtr& c = q->corners[l]; - - if (c->column == min_column || c->column == max_column) - { - // Needs no neighbor anymore - c->needsNeighbor = false; - } - } - } + } + + if ((flagSmallerDim1 == false && flagSmallerDim2 == false) && + smallerDimPattern + 1 < max_row - min_row) { + // Larger target pattern size is in row direction, check wheter smaller + // target pattern size is reached in column direction + if ((smallerDimPattern + 1) == max_column - min_column) { + for (int k = 0; k < (int)quadGroup.size(); ++k) { + ChessboardQuadPtr& q = quadGroup.at(k); + + for (int l = 0; l < 4; ++l) { + ChessboardCornerPtr& c = q->corners[l]; + + if (c->column == min_column || c->column == max_column) { + // Needs no neighbor anymore + c->needsNeighbor = false; + } } + } } + } } //=========================================================================== // GIVE A GROUP IDX //=========================================================================== -void -Chessboard::findQuadNeighbors(std::vector& quads, int dilation) -{ - // Thresh dilation is used to counter the effect of dilation on the - // distance between 2 neighboring corners. Since the distance below is - // computed as its square, we do here the same. Additionally, we take the - // conservative assumption that dilation was performed using the 3x3 CROSS - // kernel, which coresponds to the 4-neighborhood. - const float thresh_dilation = (float)(2*dilation+3)*(2*dilation+3)*2; // the "*2" is for the x and y component - // the "3" is for initial corner mismatch - - // Find quad neighbors - for (size_t idx = 0; idx < quads.size(); ++idx) - { - ChessboardQuadPtr& curQuad = quads.at(idx); - - // Go through all quadrangles and label them in groups - // For each corner of this quadrangle - for (int i = 0; i < 4; ++i) - { - float minDist = FLT_MAX; - int closestCornerIdx = -1; - ChessboardQuadPtr closestQuad; - - if (curQuad->neighbors[i]) - { - continue; - } - - cv::Point2f pt = curQuad->corners[i]->pt; +void Chessboard::findQuadNeighbors(std::vector& quads, + int dilation) { + // Thresh dilation is used to counter the effect of dilation on the + // distance between 2 neighboring corners. Since the distance below is + // computed as its square, we do here the same. Additionally, we take the + // conservative assumption that dilation was performed using the 3x3 CROSS + // kernel, which coresponds to the 4-neighborhood. + const float thresh_dilation = (float)(2 * dilation + 3) * (2 * dilation + 3) * + 2; // the "*2" is for the x and y component + // the "3" is for initial corner mismatch + + // Find quad neighbors + for (size_t idx = 0; idx < quads.size(); ++idx) { + ChessboardQuadPtr& curQuad = quads.at(idx); + + // Go through all quadrangles and label them in groups + // For each corner of this quadrangle + for (int i = 0; i < 4; ++i) { + float minDist = FLT_MAX; + int closestCornerIdx = -1; + ChessboardQuadPtr closestQuad; + + if (curQuad->neighbors[i]) { + continue; + } + + cv::Point2f pt = curQuad->corners[i]->pt; + + // Find the closest corner in all other quadrangles + for (size_t k = 0; k < quads.size(); ++k) { + if (k == idx) { + continue; + } - // Find the closest corner in all other quadrangles - for (size_t k = 0; k < quads.size(); ++k) - { - if (k == idx) - { - continue; - } + ChessboardQuadPtr& quad = quads.at(k); - ChessboardQuadPtr& quad = quads.at(k); - - for (int j = 0; j < 4; ++j) - { - // If it already has a neighbor - if (quad->neighbors[j]) - { - continue; - } - - cv::Point2f dp = pt - quad->corners[j]->pt; - float dist = dp.dot(dp); - - // The following "if" checks, whether "dist" is the - // shortest so far and smaller than the smallest - // edge length of the current and target quads - if (dist < minDist && - dist <= (curQuad->edge_len + thresh_dilation) && - dist <= (quad->edge_len + thresh_dilation) ) - { - // Check whether conditions are fulfilled - if (matchCorners(curQuad, i, quad, j)) - { - closestCornerIdx = j; - closestQuad = quad; - minDist = dist; - } - } - } + for (int j = 0; j < 4; ++j) { + // If it already has a neighbor + if (quad->neighbors[j]) { + continue; + } + + cv::Point2f dp = pt - quad->corners[j]->pt; + float dist = dp.dot(dp); + + // The following "if" checks, whether "dist" is the + // shortest so far and smaller than the smallest + // edge length of the current and target quads + if (dist < minDist && dist <= (curQuad->edge_len + thresh_dilation) && + dist <= (quad->edge_len + thresh_dilation)) { + // Check whether conditions are fulfilled + if (matchCorners(curQuad, i, quad, j)) { + closestCornerIdx = j; + closestQuad = quad; + minDist = dist; } + } + } + } + + // Have we found a matching corner point? + if (closestCornerIdx >= 0 && minDist < FLT_MAX) { + ChessboardCornerPtr closestCorner = + closestQuad->corners[closestCornerIdx]; + + // Make sure that the closest quad does not have the current + // quad as neighbor already + bool valid = true; + for (int j = 0; j < 4; ++j) { + if (closestQuad->neighbors[j] == curQuad) { + valid = false; + break; + } + } + if (!valid) { + continue; + } - // Have we found a matching corner point? - if (closestCornerIdx >= 0 && minDist < FLT_MAX) - { - ChessboardCornerPtr closestCorner = closestQuad->corners[closestCornerIdx]; - - // Make sure that the closest quad does not have the current - // quad as neighbor already - bool valid = true; - for (int j = 0; j < 4; ++j) - { - if (closestQuad->neighbors[j] == curQuad) - { - valid = false; - break; - } - } - if (!valid) - { - continue; - } - - // We've found one more corner - remember it - closestCorner->pt = (pt + closestCorner->pt) * 0.5f; + // We've found one more corner - remember it + closestCorner->pt = (pt + closestCorner->pt) * 0.5f; - curQuad->count++; - curQuad->neighbors[i] = closestQuad; - curQuad->corners[i] = closestCorner; + curQuad->count++; + curQuad->neighbors[i] = closestQuad; + curQuad->corners[i] = closestCorner; - closestQuad->count++; - closestQuad->neighbors[closestCornerIdx] = curQuad; - closestQuad->corners[closestCornerIdx] = closestCorner; - } - } + closestQuad->count++; + closestQuad->neighbors[closestCornerIdx] = curQuad; + closestQuad->corners[closestCornerIdx] = closestCorner; + } } + } } - - //=========================================================================== // AUGMENT PATTERN WITH ADDITIONAL QUADS //=========================================================================== @@ -1044,939 +899,882 @@ Chessboard::findQuadNeighbors(std::vector& quads, int dilatio // "findQuadNeighbors" // The comparisons between two points and two lines could be computed in their // own function -int -Chessboard::augmentBestRun(std::vector& candidateQuads, int candidateDilation, - std::vector& existingQuads, int existingDilation) -{ - // thresh dilation is used to counter the effect of dilation on the - // distance between 2 neighboring corners. Since the distance below is - // computed as its square, we do here the same. Additionally, we take the - // conservative assumption that dilation was performed using the 3x3 CROSS - // kernel, which coresponds to the 4-neighborhood. - const float thresh_dilation = (2*candidateDilation+3)*(2*existingDilation+3)*2; // the "*2" is for the x and y component - - // Search all old quads which have a neighbor that needs to be linked - for (size_t idx = 0; idx < existingQuads.size(); ++idx) - { - ChessboardQuadPtr& curQuad = existingQuads.at(idx); - - // For each corner of this quadrangle - for (int i = 0; i < 4; ++i) - { - float minDist = FLT_MAX; - int closestCornerIdx = -1; - ChessboardQuadPtr closestQuad; - - // If curQuad corner[i] is already linked, continue - if (!curQuad->corners[i]->needsNeighbor) - { - continue; - } - - cv::Point2f pt = curQuad->corners[i]->pt; - - // Look for a match in all candidateQuads' corners - for (size_t k = 0; k < candidateQuads.size(); ++k) - { - ChessboardQuadPtr& candidateQuad = candidateQuads.at(k); - - // Only look at unlabeled new quads - if (candidateQuad->labeled) - { - continue; - } +int Chessboard::augmentBestRun(std::vector& candidateQuads, + int candidateDilation, + std::vector& existingQuads, + int existingDilation) { + // thresh dilation is used to counter the effect of dilation on the + // distance between 2 neighboring corners. Since the distance below is + // computed as its square, we do here the same. Additionally, we take the + // conservative assumption that dilation was performed using the 3x3 CROSS + // kernel, which coresponds to the 4-neighborhood. + const float thresh_dilation = (2 * candidateDilation + 3) * + (2 * existingDilation + 3) * + 2; // the "*2" is for the x and y component + + // Search all old quads which have a neighbor that needs to be linked + for (size_t idx = 0; idx < existingQuads.size(); ++idx) { + ChessboardQuadPtr& curQuad = existingQuads.at(idx); + + // For each corner of this quadrangle + for (int i = 0; i < 4; ++i) { + float minDist = FLT_MAX; + int closestCornerIdx = -1; + ChessboardQuadPtr closestQuad; + + // If curQuad corner[i] is already linked, continue + if (!curQuad->corners[i]->needsNeighbor) { + continue; + } + + cv::Point2f pt = curQuad->corners[i]->pt; + + // Look for a match in all candidateQuads' corners + for (size_t k = 0; k < candidateQuads.size(); ++k) { + ChessboardQuadPtr& candidateQuad = candidateQuads.at(k); + + // Only look at unlabeled new quads + if (candidateQuad->labeled) { + continue; + } - for (int j = 0; j < 4; ++j) - { - // Only proceed if they are less than dist away from each - // other - cv::Point2f dp = pt - candidateQuad->corners[j]->pt; - float dist = dp.dot(dp); - - if ((dist < minDist) && - dist <= (curQuad->edge_len + thresh_dilation) && - dist <= (candidateQuad->edge_len + thresh_dilation)) - { - if (matchCorners(curQuad, i, candidateQuad, j)) - { - closestCornerIdx = j; - closestQuad = candidateQuad; - minDist = dist; - } - } - } + for (int j = 0; j < 4; ++j) { + // Only proceed if they are less than dist away from each + // other + cv::Point2f dp = pt - candidateQuad->corners[j]->pt; + float dist = dp.dot(dp); + + if ((dist < minDist) && + dist <= (curQuad->edge_len + thresh_dilation) && + dist <= (candidateQuad->edge_len + thresh_dilation)) { + if (matchCorners(curQuad, i, candidateQuad, j)) { + closestCornerIdx = j; + closestQuad = candidateQuad; + minDist = dist; } + } + } + } + + // Have we found a matching corner point? + if (closestCornerIdx >= 0 && minDist < FLT_MAX) { + ChessboardCornerPtr closestCorner = + closestQuad->corners[closestCornerIdx]; + closestCorner->pt = (pt + closestCorner->pt) * 0.5f; + + // We've found one more corner - remember it + // ATTENTION: write the corner x and y coordinates separately, + // else the crucial row/column entries will be overwritten !!! + curQuad->corners[i]->pt = closestCorner->pt; + curQuad->neighbors[i] = closestQuad; + closestQuad->corners[closestCornerIdx]->pt = closestCorner->pt; + + // Label closest quad as labeled. In this way we exclude it + // being considered again during the next loop iteration + closestQuad->labeled = true; + + // We have a new member of the final pattern, copy it over + ChessboardQuadPtr newQuad(new ChessboardQuad); + newQuad->count = 1; + newQuad->edge_len = closestQuad->edge_len; + newQuad->group_idx = curQuad->group_idx; // the same as the current + // quad + newQuad->labeled = false; // do it right afterwards + + curQuad->neighbors[i] = newQuad; + + // We only know one neighbor for sure + newQuad->neighbors[closestCornerIdx] = curQuad; + + for (int j = 0; j < 4; j++) { + newQuad->corners[j].reset(new ChessboardCorner); + newQuad->corners[j]->pt = closestQuad->corners[j]->pt; + } - // Have we found a matching corner point? - if (closestCornerIdx >= 0 && minDist < FLT_MAX) - { - ChessboardCornerPtr closestCorner = closestQuad->corners[closestCornerIdx]; - closestCorner->pt = (pt + closestCorner->pt) * 0.5f; - - // We've found one more corner - remember it - // ATTENTION: write the corner x and y coordinates separately, - // else the crucial row/column entries will be overwritten !!! - curQuad->corners[i]->pt = closestCorner->pt; - curQuad->neighbors[i] = closestQuad; - closestQuad->corners[closestCornerIdx]->pt = closestCorner->pt; - - // Label closest quad as labeled. In this way we exclude it - // being considered again during the next loop iteration - closestQuad->labeled = true; - - // We have a new member of the final pattern, copy it over - ChessboardQuadPtr newQuad(new ChessboardQuad); - newQuad->count = 1; - newQuad->edge_len = closestQuad->edge_len; - newQuad->group_idx = curQuad->group_idx; //the same as the current quad - newQuad->labeled = false; //do it right afterwards - - curQuad->neighbors[i] = newQuad; - - // We only know one neighbor for sure - newQuad->neighbors[closestCornerIdx] = curQuad; - - for (int j = 0; j < 4; j++) - { - newQuad->corners[j].reset(new ChessboardCorner); - newQuad->corners[j]->pt = closestQuad->corners[j]->pt; - } - - existingQuads.push_back(newQuad); + existingQuads.push_back(newQuad); - // Start the function again - return -1; - } - } + // Start the function again + return -1; + } } + } - // Finished, don't start the function again - return 1; + // Finished, don't start the function again + return 1; } - - //=========================================================================== // GENERATE QUADRANGLES //=========================================================================== -void -Chessboard::generateQuads(std::vector& quads, - cv::Mat& image, int flags, - int dilation, bool firstRun) -{ - // Empirical lower bound for the size of allowable quadrangles. - // MARTIN, modified: Added "*0.1" in order to find smaller quads. - int minSize = lround(image.cols * image.rows * .03 * 0.01 * 0.92 * 0.1); - - std::vector< std::vector > contours; - std::vector hierarchy; - - // Initialize contour retrieving routine - cv::findContours(image, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE); - - std::vector< std::vector > quadContours; - - for (size_t i = 0; i < contours.size(); ++i) - { - // Reject contours with a too small perimeter and contours which are - // completely surrounded by another contour - // MARTIN: If this function is called during PART 1, then the parameter "first run" - // is set to "true". This guarantees, that only "nice" squares are detected. - // During PART 2, we allow the polygonal approximation function below to - // approximate more freely, which can result in recognized "squares" that are in - // reality multiple blurred and sticked together squares. - std::vector& contour = contours.at(i); - - if (hierarchy[i][3] == -1 || cv::contourArea(contour) < minSize) - { - continue; - } - - int min_approx_level = 1, max_approx_level; - if (firstRun) - { - max_approx_level = 3; - } - else - { - max_approx_level = MAX_CONTOUR_APPROX; - } +void Chessboard::generateQuads(std::vector& quads, + cv::Mat& image, int flags, int dilation, + bool firstRun) { + // Empirical lower bound for the size of allowable quadrangles. + // MARTIN, modified: Added "*0.1" in order to find smaller quads. + int minSize = lround(image.cols * image.rows * .03 * 0.01 * 0.92 * 0.1); + + std::vector > contours; + std::vector hierarchy; + + // Initialize contour retrieving routine + cv::findContours(image, contours, hierarchy, CV_RETR_CCOMP, + CV_CHAIN_APPROX_SIMPLE); + + std::vector > quadContours; + + for (size_t i = 0; i < contours.size(); ++i) { + // Reject contours with a too small perimeter and contours which are + // completely surrounded by another contour + // MARTIN: If this function is called during PART 1, then the parameter + // "first run" is set to "true". This guarantees, that only "nice" squares + // are detected. During PART 2, we allow the polygonal approximation + // function below to approximate more freely, which can result in recognized + // "squares" that are in reality multiple blurred and sticked together + // squares. + std::vector& contour = contours.at(i); + + if (hierarchy[i][3] == -1 || cv::contourArea(contour) < minSize) { + continue; + } - std::vector approxContour; - for (int approx_level = min_approx_level; approx_level <= max_approx_level; approx_level++) - { - cv::approxPolyDP(contour, approxContour, approx_level, true); + int min_approx_level = 1, max_approx_level; + if (firstRun) { + max_approx_level = 3; + } else { + max_approx_level = MAX_CONTOUR_APPROX; + } - if (approxContour.size() == 4) - { - break; - } - } + std::vector approxContour; + for (int approx_level = min_approx_level; approx_level <= max_approx_level; + approx_level++) { + cv::approxPolyDP(contour, approxContour, approx_level, true); - // Reject non-quadrangles - if (approxContour.size() == 4 && cv::isContourConvex(approxContour)) - { - double p = cv::arcLength(approxContour, true); - double area = cv::contourArea(approxContour); + if (approxContour.size() == 4) { + break; + } + } - cv::Point pt[4]; - for (int i = 0; i < 4; i++) - { - pt[i] = approxContour[i]; - } + // Reject non-quadrangles + if (approxContour.size() == 4 && cv::isContourConvex(approxContour)) { + double p = cv::arcLength(approxContour, true); + double area = cv::contourArea(approxContour); + + cv::Point pt[4]; + for (int i = 0; i < 4; i++) { + pt[i] = approxContour[i]; + } + + cv::Point dp = pt[0] - pt[2]; + double d1 = sqrt(dp.dot(dp)); + + dp = pt[1] - pt[3]; + double d2 = sqrt(dp.dot(dp)); + + // PHILIPG: Only accept those quadrangles which are more + // square than rectangular and which are big enough + dp = pt[0] - pt[1]; + double d3 = sqrt(dp.dot(dp)); + dp = pt[1] - pt[2]; + double d4 = sqrt(dp.dot(dp)); + + if (!(flags & CV_CALIB_CB_FILTER_QUADS) || + (d3 * 4 > d4 && d4 * 4 > d3 && d3 * d4 < area * 1.5 && + area > minSize && d1 >= 0.15 * p && d2 >= 0.15 * p)) { + quadContours.push_back(approxContour); + } + } + } - cv::Point dp = pt[0] - pt[2]; - double d1 = sqrt(dp.dot(dp)); + // Allocate quad & corner buffers + quads.resize(quadContours.size()); - dp = pt[1] - pt[3]; - double d2 = sqrt(dp.dot(dp)); + // Create array of quads structures + for (size_t idx = 0; idx < quadContours.size(); ++idx) { + ChessboardQuadPtr& q = quads.at(idx); + std::vector& contour = quadContours.at(idx); - // PHILIPG: Only accept those quadrangles which are more - // square than rectangular and which are big enough - dp = pt[0] - pt[1]; - double d3 = sqrt(dp.dot(dp)); - dp = pt[1] - pt[2]; - double d4 = sqrt(dp.dot(dp)); + // Reset group ID + q.reset(new ChessboardQuad); + assert(contour.size() == 4); - if (!(flags & CV_CALIB_CB_FILTER_QUADS) || - (d3*4 > d4 && d4*4 > d3 && d3*d4 < area*1.5 && area > minSize && - d1 >= 0.15 * p && d2 >= 0.15 * p)) - { - quadContours.push_back(approxContour); - } - } + for (int i = 0; i < 4; ++i) { + cv::Point2f pt = contour.at(i); + ChessboardCornerPtr corner(new ChessboardCorner); + corner->pt = pt; + q->corners[i] = corner; } - // Allocate quad & corner buffers - quads.resize(quadContours.size()); - - // Create array of quads structures - for (size_t idx = 0; idx < quadContours.size(); ++idx) - { - ChessboardQuadPtr& q = quads.at(idx); - std::vector& contour = quadContours.at(idx); - - // Reset group ID - q.reset(new ChessboardQuad); - assert(contour.size() == 4); - - for (int i = 0; i < 4; ++i) - { - cv::Point2f pt = contour.at(i); - ChessboardCornerPtr corner(new ChessboardCorner); - corner->pt = pt; - q->corners[i] = corner; - } - - for (int i = 0; i < 4; ++i) - { - cv::Point2f dp = q->corners[i]->pt - q->corners[(i+1)&3]->pt; - float d = dp.dot(dp); - if (q->edge_len > d) - { - q->edge_len = d; - } - } + for (int i = 0; i < 4; ++i) { + cv::Point2f dp = q->corners[i]->pt - q->corners[(i + 1) & 3]->pt; + float d = dp.dot(dp); + if (q->edge_len > d) { + q->edge_len = d; + } } + } } -bool -Chessboard::checkQuadGroup(std::vector& quads, - std::vector& corners, - cv::Size patternSize) -{ - // Initialize - bool flagRow = false; - bool flagColumn = false; - int height = -1; - int width = -1; - - // Compute the minimum and maximum row / column ID - // (it is unlikely that more than 8bit checkers are used per dimension) - int min_row = 127; - int max_row = -127; - int min_col = 127; - int max_col = -127; - - for (size_t i = 0; i < quads.size(); ++i) - { - ChessboardQuadPtr& q = quads.at(i); - - for (int j = 0; j < 4; ++j) - { - ChessboardCornerPtr& c = q->corners[j]; - - if (c->row > max_row) - { - max_row = c->row; - } - if (c->row < min_row) - { - min_row = c->row; - } - if (c->column > max_col) - { - max_col = c->column; - } - if (c->column < min_col) - { - min_col = c->column; - } - } +bool Chessboard::checkQuadGroup(std::vector& quads, + std::vector& corners, + cv::Size patternSize) { + // Initialize + bool flagRow = false; + bool flagColumn = false; + int height = -1; + int width = -1; + + // Compute the minimum and maximum row / column ID + // (it is unlikely that more than 8bit checkers are used per dimension) + int min_row = 127; + int max_row = -127; + int min_col = 127; + int max_col = -127; + + for (size_t i = 0; i < quads.size(); ++i) { + ChessboardQuadPtr& q = quads.at(i); + + for (int j = 0; j < 4; ++j) { + ChessboardCornerPtr& c = q->corners[j]; + + if (c->row > max_row) { + max_row = c->row; + } + if (c->row < min_row) { + min_row = c->row; + } + if (c->column > max_col) { + max_col = c->column; + } + if (c->column < min_col) { + min_col = c->column; + } } - - // If in a given direction the target pattern size is reached, we know exactly how - // the checkerboard is oriented. - // Else we need to prepare enough "dummy" corners for the worst case. - for (size_t i = 0; i < quads.size(); ++i) - { - ChessboardQuadPtr& q = quads.at(i); - - for (int j = 0; j < 4; ++j) - { - ChessboardCornerPtr& c = q->corners[j]; - - if (c->column == max_col && c->row != min_row && c->row != max_row && !c->needsNeighbor) - { - flagColumn = true; - } - if (c->row == max_row && c->column != min_col && c->column != max_col && !c->needsNeighbor) - { - flagRow = true; - } - } + } + + // If in a given direction the target pattern size is reached, we know exactly + // how the checkerboard is oriented. Else we need to prepare enough "dummy" + // corners for the worst case. + for (size_t i = 0; i < quads.size(); ++i) { + ChessboardQuadPtr& q = quads.at(i); + + for (int j = 0; j < 4; ++j) { + ChessboardCornerPtr& c = q->corners[j]; + + if (c->column == max_col && c->row != min_row && c->row != max_row && + !c->needsNeighbor) { + flagColumn = true; + } + if (c->row == max_row && c->column != min_col && c->column != max_col && + !c->needsNeighbor) { + flagRow = true; + } } - - if (flagColumn) - { - if (max_col - min_col == patternSize.width + 1) - { - width = patternSize.width; - height = patternSize.height; - } - else - { - width = patternSize.height; - height = patternSize.width; - } + } + + if (flagColumn) { + if (max_col - min_col == patternSize.width + 1) { + width = patternSize.width; + height = patternSize.height; + } else { + width = patternSize.height; + height = patternSize.width; } - else if (flagRow) - { - if (max_row - min_row == patternSize.width + 1) - { - height = patternSize.width; - width = patternSize.height; - } - else - { - height = patternSize.height; - width = patternSize.width; - } - } - else - { - // If target pattern size is not reached in at least one of the two - // directions, then we do not know where the remaining corners are - // located. Account for this. - width = std::max(patternSize.width, patternSize.height); - height = std::max(patternSize.width, patternSize.height); + } else if (flagRow) { + if (max_row - min_row == patternSize.width + 1) { + height = patternSize.width; + width = patternSize.height; + } else { + height = patternSize.height; + width = patternSize.width; } + } else { + // If target pattern size is not reached in at least one of the two + // directions, then we do not know where the remaining corners are + // located. Account for this. + width = std::max(patternSize.width, patternSize.height); + height = std::max(patternSize.width, patternSize.height); + } + + ++min_row; + ++min_col; + max_row = min_row + height - 1; + max_col = min_col + width - 1; + + corners.clear(); + + int linkedBorderCorners = 0; + + // Write the corners in increasing order to the output file + for (int i = min_row; i <= max_row; ++i) { + for (int j = min_col; j <= max_col; ++j) { + // Reset the iterator + int iter = 1; + + for (int k = 0; k < (int)quads.size(); ++k) { + ChessboardQuadPtr& quad = quads.at(k); + + for (int l = 0; l < 4; ++l) { + ChessboardCornerPtr& c = quad->corners[l]; + + if (c->row == i && c->column == j) { + bool boardEdge = false; + if (i == min_row || i == max_row || j == min_col || j == max_col) { + boardEdge = true; + } - ++min_row; - ++min_col; - max_row = min_row + height - 1; - max_col = min_col + width - 1; + if ((iter == 1 && boardEdge) || (iter == 2 && !boardEdge)) { + // The respective row and column have been found + corners.push_back(quad->corners[l]); + } - corners.clear(); + if (iter == 2 && boardEdge) { + ++linkedBorderCorners; + } - int linkedBorderCorners = 0; - - // Write the corners in increasing order to the output file - for (int i = min_row; i <= max_row; ++i) - { - for (int j = min_col; j <= max_col; ++j) - { - // Reset the iterator - int iter = 1; - - for (int k = 0; k < (int)quads.size(); ++k) - { - ChessboardQuadPtr& quad = quads.at(k); - - for (int l = 0; l < 4; ++l) - { - ChessboardCornerPtr& c = quad->corners[l]; - - if (c->row == i && c->column == j) - { - bool boardEdge = false; - if (i == min_row || i == max_row || - j == min_col || j == max_col) - { - boardEdge = true; - } - - if ((iter == 1 && boardEdge) || (iter == 2 && !boardEdge)) - { - // The respective row and column have been found - corners.push_back(quad->corners[l]); - } - - if (iter == 2 && boardEdge) - { - ++linkedBorderCorners; - } - - // If the iterator is larger than two, this means that more than - // two corners have the same row / column entries. Then some - // linking errors must have occured and we should not use the found - // pattern - if (iter > 2) - { - return false; - } - - iter++; - } - } + // If the iterator is larger than two, this means that more than + // two corners have the same row / column entries. Then some + // linking errors must have occured and we should not use the found + // pattern + if (iter > 2) { + return false; } + + iter++; + } } + } } - - if ((int)corners.size() != patternSize.width * patternSize.height || - linkedBorderCorners < (patternSize.width * 2 + patternSize.height * 2 - 2) * 0.75f) - { - return false; + } + + if ((int)corners.size() != patternSize.width * patternSize.height || + linkedBorderCorners < + (patternSize.width * 2 + patternSize.height * 2 - 2) * 0.75f) { + return false; + } + + // check that no corners lie at image boundary + float border = 5.0f; + for (int i = 0; i < (int)corners.size(); ++i) { + ChessboardCornerPtr& c = corners.at(i); + + if (c->pt.x < border || c->pt.x > mImage.cols - border || + c->pt.y < border || c->pt.y > mImage.rows - border) { + return false; } + } - // check that no corners lie at image boundary - float border = 5.0f; - for (int i = 0; i < (int)corners.size(); ++i) - { - ChessboardCornerPtr& c = corners.at(i); - - if (c->pt.x < border || c->pt.x > mImage.cols - border || - c->pt.y < border || c->pt.y > mImage.rows - border) - { - return false; - } - } + // check if we need to transpose the board + if (width != patternSize.width) { + std::swap(width, height); - // check if we need to transpose the board - if (width != patternSize.width) - { - std::swap(width, height); + std::vector outputCorners; + outputCorners.resize(corners.size()); - std::vector outputCorners; - outputCorners.resize(corners.size()); + for (int i = 0; i < height; ++i) { + for (int j = 0; j < width; ++j) { + outputCorners.at(i * width + j) = corners.at(j * height + i); + } + } - for (int i = 0; i < height; ++i) - { - for (int j = 0; j < width; ++j) - { - outputCorners.at(i * width + j) = corners.at(j * height + i); - } - } + corners = outputCorners; + } - corners = outputCorners; - } + // check if we need to revert the order in each row + cv::Point2f p0 = corners.at(0)->pt; + cv::Point2f p1 = corners.at(width - 1)->pt; + cv::Point2f p2 = corners.at(width)->pt; - // check if we need to revert the order in each row - cv::Point2f p0 = corners.at(0)->pt; - cv::Point2f p1 = corners.at(width-1)->pt; - cv::Point2f p2 = corners.at(width)->pt; - - if ((p1 - p0).cross(p2 - p0) < 0.0f) - { - for (int i = 0; i < height; ++i) - { - for (int j = 0; j < width / 2; ++j) - { - std::swap(corners.at(i * width + j), corners.at(i * width + width - j - 1)); - } - } + if ((p1 - p0).cross(p2 - p0) < 0.0f) { + for (int i = 0; i < height; ++i) { + for (int j = 0; j < width / 2; ++j) { + std::swap(corners.at(i * width + j), + corners.at(i * width + width - j - 1)); + } } + } - p0 = corners.at(0)->pt; - p2 = corners.at(width)->pt; - - // check if we need to rotate the board - if (p2.y < p0.y) - { - std::vector outputCorners; - outputCorners.resize(corners.size()); + p0 = corners.at(0)->pt; + p2 = corners.at(width)->pt; - for (int i = 0; i < height; ++i) - { - for (int j = 0; j < width; ++j) - { - outputCorners.at(i * width + j) = corners.at((height - i - 1) * width + width - j - 1); - } - } + // check if we need to rotate the board + if (p2.y < p0.y) { + std::vector outputCorners; + outputCorners.resize(corners.size()); - corners = outputCorners; + for (int i = 0; i < height; ++i) { + for (int j = 0; j < width; ++j) { + outputCorners.at(i * width + j) = + corners.at((height - i - 1) * width + width - j - 1); + } } - return true; -} + corners = outputCorners; + } -void -Chessboard::getQuadrangleHypotheses(const std::vector< std::vector >& contours, - std::vector< std::pair >& quads, - int classId) const -{ - const float minAspectRatio = 0.2f; - const float maxAspectRatio = 5.0f; - const float minBoxSize = 10.0f; - - for (size_t i = 0; i < contours.size(); ++i) - { - cv::RotatedRect box = cv::minAreaRect(contours.at(i)); - float boxSize = std::max(box.size.width, box.size.height); - if (boxSize < minBoxSize) - { - continue; - } + return true; +} - float aspectRatio = box.size.width / std::max(box.size.height, 1.0f); +void Chessboard::getQuadrangleHypotheses( + const std::vector >& contours, + std::vector >& quads, int classId) const { + const float minAspectRatio = 0.2f; + const float maxAspectRatio = 5.0f; + const float minBoxSize = 10.0f; + + for (size_t i = 0; i < contours.size(); ++i) { + cv::RotatedRect box = cv::minAreaRect(contours.at(i)); + float boxSize = std::max(box.size.width, box.size.height); + if (boxSize < minBoxSize) { + continue; + } - if (aspectRatio < minAspectRatio || aspectRatio > maxAspectRatio) - { - continue; - } + float aspectRatio = box.size.width / std::max(box.size.height, 1.0f); - quads.push_back(std::pair(boxSize, classId)); + if (aspectRatio < minAspectRatio || aspectRatio > maxAspectRatio) { + continue; } -} -bool less_pred(const std::pair& p1, const std::pair& p2) -{ - return p1.first < p2.first; + quads.push_back(std::pair(boxSize, classId)); + } } -void countClasses(const std::vector >& pairs, size_t idx1, size_t idx2, std::vector& counts) -{ - counts.assign(2, 0); - for (size_t i = idx1; i != idx2; ++i) - { - counts[pairs[i].second]++; - } +bool less_pred(const std::pair& p1, + const std::pair& p2) { + return p1.first < p2.first; } -bool -Chessboard::checkChessboard(const cv::Mat& image, cv::Size patternSize) const -{ - const int erosionCount = 1; - const float blackLevel = 20.f; - const float whiteLevel = 130.f; - const float blackWhiteGap = 70.f; +void countClasses(const std::vector >& pairs, size_t idx1, + size_t idx2, std::vector& counts) { + counts.assign(2, 0); + for (size_t i = idx1; i != idx2; ++i) { + counts[pairs[i].second]++; + } +} - cv::Mat white = image.clone(); +bool Chessboard::checkChessboard(const cv::Mat& image, + cv::Size patternSize) const { + const int erosionCount = 1; + const float blackLevel = 20.f; + const float whiteLevel = 130.f; + const float blackWhiteGap = 70.f; - cv::Mat black = image.clone(); + cv::Mat white = image.clone(); - cv::erode(white, white, cv::Mat(), cv::Point(-1,-1), erosionCount); - cv::dilate(black, black, cv::Mat(), cv::Point(-1,-1), erosionCount); + cv::Mat black = image.clone(); - cv::Mat thresh(image.rows, image.cols, CV_8UC1); + cv::erode(white, white, cv::Mat(), cv::Point(-1, -1), erosionCount); + cv::dilate(black, black, cv::Mat(), cv::Point(-1, -1), erosionCount); - bool result = false; - for (float threshLevel = blackLevel; threshLevel < whiteLevel && !result; threshLevel += 20.0f) - { - cv::threshold(white, thresh, threshLevel + blackWhiteGap, 255, CV_THRESH_BINARY); + cv::Mat thresh(image.rows, image.cols, CV_8UC1); - std::vector< std::vector > contours; - std::vector hierarchy; + bool result = false; + for (float threshLevel = blackLevel; threshLevel < whiteLevel && !result; + threshLevel += 20.0f) { + cv::threshold(white, thresh, threshLevel + blackWhiteGap, 255, + CV_THRESH_BINARY); - // Initialize contour retrieving routine - cv::findContours(thresh, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE); + std::vector > contours; + std::vector hierarchy; - std::vector > quads; - getQuadrangleHypotheses(contours, quads, 1); + // Initialize contour retrieving routine + cv::findContours(thresh, contours, hierarchy, CV_RETR_CCOMP, + CV_CHAIN_APPROX_SIMPLE); - cv::threshold(black, thresh, threshLevel, 255, CV_THRESH_BINARY_INV); + std::vector > quads; + getQuadrangleHypotheses(contours, quads, 1); - cv::findContours(thresh, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE); - getQuadrangleHypotheses(contours, quads, 0); + cv::threshold(black, thresh, threshLevel, 255, CV_THRESH_BINARY_INV); - const size_t min_quads_count = patternSize.width * patternSize.height / 2; - std::sort(quads.begin(), quads.end(), less_pred); + cv::findContours(thresh, contours, hierarchy, CV_RETR_CCOMP, + CV_CHAIN_APPROX_SIMPLE); + getQuadrangleHypotheses(contours, quads, 0); - // now check if there are many hypotheses with similar sizes - // do this by floodfill-style algorithm - const float sizeRelDev = 0.4f; + const size_t min_quads_count = patternSize.width * patternSize.height / 2; + std::sort(quads.begin(), quads.end(), less_pred); - for (size_t i = 0; i < quads.size(); ++i) - { - size_t j = i + 1; - for (; j < quads.size(); ++j) - { - if (quads[j].first / quads[i].first > 1.0f + sizeRelDev) - { - break; - } - } + // now check if there are many hypotheses with similar sizes + // do this by floodfill-style algorithm + const float sizeRelDev = 0.4f; - if (j + 1 > min_quads_count + i) - { - // check the number of black and white squares - std::vector counts; - countClasses(quads, i, j, counts); - const int blackCount = lroundf(ceilf(patternSize.width / 2.0f) * ceilf(patternSize.height / 2.0f)); - const int whiteCount = lroundf(floorf(patternSize.width / 2.0f) * floorf(patternSize.height / 2.0f)); - if (counts[0] < blackCount * 0.75f || - counts[1] < whiteCount * 0.75f) - { - continue; - } - result = true; - break; - } + for (size_t i = 0; i < quads.size(); ++i) { + size_t j = i + 1; + for (; j < quads.size(); ++j) { + if (quads[j].first / quads[i].first > 1.0f + sizeRelDev) { + break; + } + } + + if (j + 1 > min_quads_count + i) { + // check the number of black and white squares + std::vector counts; + countClasses(quads, i, j, counts); + const int blackCount = lroundf(ceilf(patternSize.width / 2.0f) * + ceilf(patternSize.height / 2.0f)); + const int whiteCount = lroundf(floorf(patternSize.width / 2.0f) * + floorf(patternSize.height / 2.0f)); + if (counts[0] < blackCount * 0.75f || counts[1] < whiteCount * 0.75f) { + continue; } + result = true; + break; + } } + } - return result; + return result; } -bool -Chessboard::checkBoardMonotony(std::vector& corners, - cv::Size patternSize) -{ - const float threshFactor = 0.2f; - - Spline splineXY, splineYX; - splineXY.setLowBC(Spline::PARABOLIC_RUNOUT_BC); - splineXY.setHighBC(Spline::PARABOLIC_RUNOUT_BC); - splineYX.setLowBC(Spline::PARABOLIC_RUNOUT_BC); - splineYX.setHighBC(Spline::PARABOLIC_RUNOUT_BC); - - // check if each row of corners approximates a cubic spline - for (int i = 0; i < patternSize.height; ++i) - { - splineXY.clear(); - splineYX.clear(); - - cv::Point2f p[3]; - p[0] = corners.at(i * patternSize.width)->pt; - p[1] = corners.at(i * patternSize.width + patternSize.width / 2)->pt; - p[2] = corners.at(i * patternSize.width + patternSize.width - 1)->pt; - - for (int j = 0; j < 3; ++j) - { - splineXY.addPoint(p[j].x, p[j].y); - splineYX.addPoint(p[j].y, p[j].x); - } - - for (int j = 1; j < patternSize.width - 1; ++j) - { - cv::Point2f& p_j = corners.at(i * patternSize.width + j)->pt; - - float thresh = std::numeric_limits::max(); - - // up-neighbor - if (i > 0) - { - cv::Point2f& neighbor = corners.at((i - 1) * patternSize.width + j)->pt; - thresh = fminf(thresh, cv::norm(neighbor - p_j)); - } - // down-neighbor - if (i < patternSize.height - 1) - { - cv::Point2f& neighbor = corners.at((i + 1) * patternSize.width + j)->pt; - thresh = fminf(thresh, cv::norm(neighbor - p_j)); - } - // left-neighbor - { - cv::Point2f& neighbor = corners.at(i * patternSize.width + j - 1)->pt; - thresh = fminf(thresh, cv::norm(neighbor - p_j)); - } - // right-neighbor - { - cv::Point2f& neighbor = corners.at(i * patternSize.width + j + 1)->pt; - thresh = fminf(thresh, cv::norm(neighbor - p_j)); - } - - thresh *= threshFactor; - - if (fminf(fabsf(splineXY(p_j.x) - p_j.y), fabsf(splineYX(p_j.y) - p_j.x)) > thresh) - { - return false; - } - } +bool Chessboard::checkBoardMonotony(std::vector& corners, + cv::Size patternSize) { + const float threshFactor = 0.2f; + + Spline splineXY, splineYX; + splineXY.setLowBC(Spline::PARABOLIC_RUNOUT_BC); + splineXY.setHighBC(Spline::PARABOLIC_RUNOUT_BC); + splineYX.setLowBC(Spline::PARABOLIC_RUNOUT_BC); + splineYX.setHighBC(Spline::PARABOLIC_RUNOUT_BC); + + // check if each row of corners approximates a cubic spline + for (int i = 0; i < patternSize.height; ++i) { + splineXY.clear(); + splineYX.clear(); + + cv::Point2f p[3]; + p[0] = corners.at(i * patternSize.width)->pt; + p[1] = corners.at(i * patternSize.width + patternSize.width / 2)->pt; + p[2] = corners.at(i * patternSize.width + patternSize.width - 1)->pt; + + for (int j = 0; j < 3; ++j) { + splineXY.addPoint(p[j].x, p[j].y); + splineYX.addPoint(p[j].y, p[j].x); } - // check if each column of corners approximates a cubic spline - for (int j = 0; j < patternSize.width; ++j) - { - splineXY.clear(); - splineYX.clear(); - - cv::Point2f p[3]; - p[0] = corners.at(j)->pt; - p[1] = corners.at(patternSize.height / 2 * patternSize.width + j)->pt; - p[2] = corners.at((patternSize.height - 1) * patternSize.width + j)->pt; - - for (int i = 0; i < 3; ++i) - { - splineXY.addPoint(p[i].x, p[i].y); - splineYX.addPoint(p[i].y, p[i].x); - } - - for (int i = 1; i < patternSize.height - 1; ++i) - { - cv::Point2f& p_i = corners.at(i * patternSize.width + j)->pt; - - float thresh = std::numeric_limits::max(); + for (int j = 1; j < patternSize.width - 1; ++j) { + cv::Point2f& p_j = corners.at(i * patternSize.width + j)->pt; + + float thresh = std::numeric_limits::max(); + + // up-neighbor + if (i > 0) { + cv::Point2f& neighbor = corners.at((i - 1) * patternSize.width + j)->pt; + thresh = fminf(thresh, cv::norm(neighbor - p_j)); + } + // down-neighbor + if (i < patternSize.height - 1) { + cv::Point2f& neighbor = corners.at((i + 1) * patternSize.width + j)->pt; + thresh = fminf(thresh, cv::norm(neighbor - p_j)); + } + // left-neighbor + { + cv::Point2f& neighbor = corners.at(i * patternSize.width + j - 1)->pt; + thresh = fminf(thresh, cv::norm(neighbor - p_j)); + } + // right-neighbor + { + cv::Point2f& neighbor = corners.at(i * patternSize.width + j + 1)->pt; + thresh = fminf(thresh, cv::norm(neighbor - p_j)); + } + + thresh *= threshFactor; + + if (fminf(fabsf(splineXY(p_j.x) - p_j.y), + fabsf(splineYX(p_j.y) - p_j.x)) > thresh) { + return false; + } + } + } - // up-neighbor - { - cv::Point2f& neighbor = corners.at((i - 1) * patternSize.width + j)->pt; - thresh = fminf(thresh, cv::norm(neighbor - p_i)); - } - // down-neighbor - { - cv::Point2f& neighbor = corners.at((i + 1) * patternSize.width + j)->pt; - thresh = fminf(thresh, cv::norm(neighbor - p_i)); - } - // left-neighbor - if (j > 0) - { - cv::Point2f& neighbor = corners.at(i * patternSize.width + j - 1)->pt; - thresh = fminf(thresh, cv::norm(neighbor - p_i)); - } - // right-neighbor - if (j < patternSize.width - 1) - { - cv::Point2f& neighbor = corners.at(i * patternSize.width + j + 1)->pt; - thresh = fminf(thresh, cv::norm(neighbor - p_i)); - } + // check if each column of corners approximates a cubic spline + for (int j = 0; j < patternSize.width; ++j) { + splineXY.clear(); + splineYX.clear(); - thresh *= threshFactor; + cv::Point2f p[3]; + p[0] = corners.at(j)->pt; + p[1] = corners.at(patternSize.height / 2 * patternSize.width + j)->pt; + p[2] = corners.at((patternSize.height - 1) * patternSize.width + j)->pt; - if (fminf(fabsf(splineXY(p_i.x) - p_i.y), fabsf(splineYX(p_i.y) - p_i.x)) > thresh) - { - return false; - } - } + for (int i = 0; i < 3; ++i) { + splineXY.addPoint(p[i].x, p[i].y); + splineYX.addPoint(p[i].y, p[i].x); } - return true; -} - -bool -Chessboard::matchCorners(ChessboardQuadPtr& quad1, int corner1, - ChessboardQuadPtr& quad2, int corner2) const -{ - // First Check everything from the viewpoint of the - // current quad compute midpoints of "parallel" quad - // sides 1 - float x1 = (quad1->corners[corner1]->pt.x + quad1->corners[(corner1+1)%4]->pt.x)/2; - float y1 = (quad1->corners[corner1]->pt.y + quad1->corners[(corner1+1)%4]->pt.y)/2; - float x2 = (quad1->corners[(corner1+2)%4]->pt.x + quad1->corners[(corner1+3)%4]->pt.x)/2; - float y2 = (quad1->corners[(corner1+2)%4]->pt.y + quad1->corners[(corner1+3)%4]->pt.y)/2; - // compute midpoints of "parallel" quad sides 2 - float x3 = (quad1->corners[corner1]->pt.x + quad1->corners[(corner1+3)%4]->pt.x)/2; - float y3 = (quad1->corners[corner1]->pt.y + quad1->corners[(corner1+3)%4]->pt.y)/2; - float x4 = (quad1->corners[(corner1+1)%4]->pt.x + quad1->corners[(corner1+2)%4]->pt.x)/2; - float y4 = (quad1->corners[(corner1+1)%4]->pt.y + quad1->corners[(corner1+2)%4]->pt.y)/2; - - // MARTIN: Heuristic - // For corner2 of quad2 to be considered, - // it needs to be on the same side of the two lines as - // corner1. This is given, if the cross product has - // the same sign for both computations below: - float a1 = x1 - x2; - float b1 = y1 - y2; - // the current corner - float c11 = quad1->corners[corner1]->pt.x - x2; - float d11 = quad1->corners[corner1]->pt.y - y2; - // the candidate corner - float c12 = quad2->corners[corner2]->pt.x - x2; - float d12 = quad2->corners[corner2]->pt.y - y2; - float sign11 = a1*d11 - c11*b1; - float sign12 = a1*d12 - c12*b1; - - float a2 = x3 - x4; - float b2 = y3 - y4; - // the current corner - float c21 = quad1->corners[corner1]->pt.x - x4; - float d21 = quad1->corners[corner1]->pt.y - y4; - // the candidate corner - float c22 = quad2->corners[corner2]->pt.x - x4; - float d22 = quad2->corners[corner2]->pt.y - y4; - float sign21 = a2*d21 - c21*b2; - float sign22 = a2*d22 - c22*b2; - - // Also make shure that two border quads of the same row or - // column don't link. Check from the current corner's view, - // whether the corner diagonal from the candidate corner - // is also on the same side of the two lines as the current - // corner and the candidate corner. - float c13 = quad2->corners[(corner2+2)%4]->pt.x - x2; - float d13 = quad2->corners[(corner2+2)%4]->pt.y - y2; - float c23 = quad2->corners[(corner2+2)%4]->pt.x - x4; - float d23 = quad2->corners[(corner2+2)%4]->pt.y - y4; - float sign13 = a1*d13 - c13*b1; - float sign23 = a2*d23 - c23*b2; - - - // Second: Then check everything from the viewpoint of - // the candidate quad. Compute midpoints of "parallel" - // quad sides 1 - float u1 = (quad2->corners[corner2]->pt.x + quad2->corners[(corner2+1)%4]->pt.x)/2; - float v1 = (quad2->corners[corner2]->pt.y + quad2->corners[(corner2+1)%4]->pt.y)/2; - float u2 = (quad2->corners[(corner2+2)%4]->pt.x + quad2->corners[(corner2+3)%4]->pt.x)/2; - float v2 = (quad2->corners[(corner2+2)%4]->pt.y + quad2->corners[(corner2+3)%4]->pt.y)/2; - // compute midpoints of "parallel" quad sides 2 - float u3 = (quad2->corners[corner2]->pt.x + quad2->corners[(corner2+3)%4]->pt.x)/2; - float v3 = (quad2->corners[corner2]->pt.y + quad2->corners[(corner2+3)%4]->pt.y)/2; - float u4 = (quad2->corners[(corner2+1)%4]->pt.x + quad2->corners[(corner2+2)%4]->pt.x)/2; - float v4 = (quad2->corners[(corner2+1)%4]->pt.y + quad2->corners[(corner2+2)%4]->pt.y)/2; - - // MARTIN: Heuristic - // For corner2 of quad2 to be considered, - // it needs to be on the same side of the two lines as - // corner1. This is given, if the cross product has - // the same sign for both computations below: - float a3 = u1 - u2; - float b3 = v1 - v2; - // the current corner - float c31 = quad1->corners[corner1]->pt.x - u2; - float d31 = quad1->corners[corner1]->pt.y - v2; - // the candidate corner - float c32 = quad2->corners[corner2]->pt.x - u2; - float d32 = quad2->corners[corner2]->pt.y - v2; - float sign31 = a3*d31-c31*b3; - float sign32 = a3*d32-c32*b3; - - float a4 = u3 - u4; - float b4 = v3 - v4; - // the current corner - float c41 = quad1->corners[corner1]->pt.x - u4; - float d41 = quad1->corners[corner1]->pt.y - v4; - // the candidate corner - float c42 = quad2->corners[corner2]->pt.x - u4; - float d42 = quad2->corners[corner2]->pt.y - v4; - float sign41 = a4*d41-c41*b4; - float sign42 = a4*d42-c42*b4; - - // Also make sure that two border quads of the same row or - // column don't link. Check from the candidate corner's view, - // whether the corner diagonal from the current corner - // is also on the same side of the two lines as the current - // corner and the candidate corner. - float c33 = quad1->corners[(corner1+2)%4]->pt.x - u2; - float d33 = quad1->corners[(corner1+2)%4]->pt.y - v2; - float c43 = quad1->corners[(corner1+2)%4]->pt.x - u4; - float d43 = quad1->corners[(corner1+2)%4]->pt.y - v4; - float sign33 = a3*d33-c33*b3; - float sign43 = a4*d43-c43*b4; - - - // This time we also need to make shure, that no quad - // is linked to a quad of another dilation run which - // may lie INSIDE it!!! - // Third: Therefore check everything from the viewpoint - // of the current quad compute midpoints of "parallel" - // quad sides 1 - float x5 = quad1->corners[corner1]->pt.x; - float y5 = quad1->corners[corner1]->pt.y; - float x6 = quad1->corners[(corner1+1)%4]->pt.x; - float y6 = quad1->corners[(corner1+1)%4]->pt.y; - // compute midpoints of "parallel" quad sides 2 - float x7 = x5; - float y7 = y5; - float x8 = quad1->corners[(corner1+3)%4]->pt.x; - float y8 = quad1->corners[(corner1+3)%4]->pt.y; - - // MARTIN: Heuristic - // For corner2 of quad2 to be considered, - // it needs to be on the other side of the two lines than - // corner1. This is given, if the cross product has - // a different sign for both computations below: - float a5 = x6 - x5; - float b5 = y6 - y5; - // the current corner - float c51 = quad1->corners[(corner1+2)%4]->pt.x - x5; - float d51 = quad1->corners[(corner1+2)%4]->pt.y - y5; - // the candidate corner - float c52 = quad2->corners[corner2]->pt.x - x5; - float d52 = quad2->corners[corner2]->pt.y - y5; - float sign51 = a5*d51 - c51*b5; - float sign52 = a5*d52 - c52*b5; - - float a6 = x8 - x7; - float b6 = y8 - y7; - // the current corner - float c61 = quad1->corners[(corner1+2)%4]->pt.x - x7; - float d61 = quad1->corners[(corner1+2)%4]->pt.y - y7; - // the candidate corner - float c62 = quad2->corners[corner2]->pt.x - x7; - float d62 = quad2->corners[corner2]->pt.y - y7; - float sign61 = a6*d61 - c61*b6; - float sign62 = a6*d62 - c62*b6; - - - // Fourth: Then check everything from the viewpoint of - // the candidate quad compute midpoints of "parallel" - // quad sides 1 - float u5 = quad2->corners[corner2]->pt.x; - float v5 = quad2->corners[corner2]->pt.y; - float u6 = quad2->corners[(corner2+1)%4]->pt.x; - float v6 = quad2->corners[(corner2+1)%4]->pt.y; - // compute midpoints of "parallel" quad sides 2 - float u7 = u5; - float v7 = v5; - float u8 = quad2->corners[(corner2+3)%4]->pt.x; - float v8 = quad2->corners[(corner2+3)%4]->pt.y; - - // MARTIN: Heuristic - // For corner2 of quad2 to be considered, - // it needs to be on the other side of the two lines than - // corner1. This is given, if the cross product has - // a different sign for both computations below: - float a7 = u6 - u5; - float b7 = v6 - v5; - // the current corner - float c71 = quad1->corners[corner1]->pt.x - u5; - float d71 = quad1->corners[corner1]->pt.y - v5; - // the candidate corner - float c72 = quad2->corners[(corner2+2)%4]->pt.x - u5; - float d72 = quad2->corners[(corner2+2)%4]->pt.y - v5; - float sign71 = a7*d71-c71*b7; - float sign72 = a7*d72-c72*b7; - - float a8 = u8 - u7; - float b8 = v8 - v7; - // the current corner - float c81 = quad1->corners[corner1]->pt.x - u7; - float d81 = quad1->corners[corner1]->pt.y - v7; - // the candidate corner - float c82 = quad2->corners[(corner2+2)%4]->pt.x - u7; - float d82 = quad2->corners[(corner2+2)%4]->pt.y - v7; - float sign81 = a8*d81-c81*b8; - float sign82 = a8*d82-c82*b8; - - // Check whether conditions are fulfilled - if (((sign11 < 0 && sign12 < 0) || (sign11 > 0 && sign12 > 0)) && - ((sign21 < 0 && sign22 < 0) || (sign21 > 0 && sign22 > 0)) && - ((sign31 < 0 && sign32 < 0) || (sign31 > 0 && sign32 > 0)) && - ((sign41 < 0 && sign42 < 0) || (sign41 > 0 && sign42 > 0)) && - ((sign11 < 0 && sign13 < 0) || (sign11 > 0 && sign13 > 0)) && - ((sign21 < 0 && sign23 < 0) || (sign21 > 0 && sign23 > 0)) && - ((sign31 < 0 && sign33 < 0) || (sign31 > 0 && sign33 > 0)) && - ((sign41 < 0 && sign43 < 0) || (sign41 > 0 && sign43 > 0)) && - ((sign51 < 0 && sign52 > 0) || (sign51 > 0 && sign52 < 0)) && - ((sign61 < 0 && sign62 > 0) || (sign61 > 0 && sign62 < 0)) && - ((sign71 < 0 && sign72 > 0) || (sign71 > 0 && sign72 < 0)) && - ((sign81 < 0 && sign82 > 0) || (sign81 > 0 && sign82 < 0))) - { - return true; - } - else - { + for (int i = 1; i < patternSize.height - 1; ++i) { + cv::Point2f& p_i = corners.at(i * patternSize.width + j)->pt; + + float thresh = std::numeric_limits::max(); + + // up-neighbor + { + cv::Point2f& neighbor = corners.at((i - 1) * patternSize.width + j)->pt; + thresh = fminf(thresh, cv::norm(neighbor - p_i)); + } + // down-neighbor + { + cv::Point2f& neighbor = corners.at((i + 1) * patternSize.width + j)->pt; + thresh = fminf(thresh, cv::norm(neighbor - p_i)); + } + // left-neighbor + if (j > 0) { + cv::Point2f& neighbor = corners.at(i * patternSize.width + j - 1)->pt; + thresh = fminf(thresh, cv::norm(neighbor - p_i)); + } + // right-neighbor + if (j < patternSize.width - 1) { + cv::Point2f& neighbor = corners.at(i * patternSize.width + j + 1)->pt; + thresh = fminf(thresh, cv::norm(neighbor - p_i)); + } + + thresh *= threshFactor; + + if (fminf(fabsf(splineXY(p_i.x) - p_i.y), + fabsf(splineYX(p_i.y) - p_i.x)) > thresh) { return false; + } } + } + + return true; } +bool Chessboard::matchCorners(ChessboardQuadPtr& quad1, int corner1, + ChessboardQuadPtr& quad2, int corner2) const { + // First Check everything from the viewpoint of the + // current quad compute midpoints of "parallel" quad + // sides 1 + float x1 = (quad1->corners[corner1]->pt.x + + quad1->corners[(corner1 + 1) % 4]->pt.x) / + 2; + float y1 = (quad1->corners[corner1]->pt.y + + quad1->corners[(corner1 + 1) % 4]->pt.y) / + 2; + float x2 = (quad1->corners[(corner1 + 2) % 4]->pt.x + + quad1->corners[(corner1 + 3) % 4]->pt.x) / + 2; + float y2 = (quad1->corners[(corner1 + 2) % 4]->pt.y + + quad1->corners[(corner1 + 3) % 4]->pt.y) / + 2; + // compute midpoints of "parallel" quad sides 2 + float x3 = (quad1->corners[corner1]->pt.x + + quad1->corners[(corner1 + 3) % 4]->pt.x) / + 2; + float y3 = (quad1->corners[corner1]->pt.y + + quad1->corners[(corner1 + 3) % 4]->pt.y) / + 2; + float x4 = (quad1->corners[(corner1 + 1) % 4]->pt.x + + quad1->corners[(corner1 + 2) % 4]->pt.x) / + 2; + float y4 = (quad1->corners[(corner1 + 1) % 4]->pt.y + + quad1->corners[(corner1 + 2) % 4]->pt.y) / + 2; + + // MARTIN: Heuristic + // For corner2 of quad2 to be considered, + // it needs to be on the same side of the two lines as + // corner1. This is given, if the cross product has + // the same sign for both computations below: + float a1 = x1 - x2; + float b1 = y1 - y2; + // the current corner + float c11 = quad1->corners[corner1]->pt.x - x2; + float d11 = quad1->corners[corner1]->pt.y - y2; + // the candidate corner + float c12 = quad2->corners[corner2]->pt.x - x2; + float d12 = quad2->corners[corner2]->pt.y - y2; + float sign11 = a1 * d11 - c11 * b1; + float sign12 = a1 * d12 - c12 * b1; + + float a2 = x3 - x4; + float b2 = y3 - y4; + // the current corner + float c21 = quad1->corners[corner1]->pt.x - x4; + float d21 = quad1->corners[corner1]->pt.y - y4; + // the candidate corner + float c22 = quad2->corners[corner2]->pt.x - x4; + float d22 = quad2->corners[corner2]->pt.y - y4; + float sign21 = a2 * d21 - c21 * b2; + float sign22 = a2 * d22 - c22 * b2; + + // Also make shure that two border quads of the same row or + // column don't link. Check from the current corner's view, + // whether the corner diagonal from the candidate corner + // is also on the same side of the two lines as the current + // corner and the candidate corner. + float c13 = quad2->corners[(corner2 + 2) % 4]->pt.x - x2; + float d13 = quad2->corners[(corner2 + 2) % 4]->pt.y - y2; + float c23 = quad2->corners[(corner2 + 2) % 4]->pt.x - x4; + float d23 = quad2->corners[(corner2 + 2) % 4]->pt.y - y4; + float sign13 = a1 * d13 - c13 * b1; + float sign23 = a2 * d23 - c23 * b2; + + // Second: Then check everything from the viewpoint of + // the candidate quad. Compute midpoints of "parallel" + // quad sides 1 + float u1 = (quad2->corners[corner2]->pt.x + + quad2->corners[(corner2 + 1) % 4]->pt.x) / + 2; + float v1 = (quad2->corners[corner2]->pt.y + + quad2->corners[(corner2 + 1) % 4]->pt.y) / + 2; + float u2 = (quad2->corners[(corner2 + 2) % 4]->pt.x + + quad2->corners[(corner2 + 3) % 4]->pt.x) / + 2; + float v2 = (quad2->corners[(corner2 + 2) % 4]->pt.y + + quad2->corners[(corner2 + 3) % 4]->pt.y) / + 2; + // compute midpoints of "parallel" quad sides 2 + float u3 = (quad2->corners[corner2]->pt.x + + quad2->corners[(corner2 + 3) % 4]->pt.x) / + 2; + float v3 = (quad2->corners[corner2]->pt.y + + quad2->corners[(corner2 + 3) % 4]->pt.y) / + 2; + float u4 = (quad2->corners[(corner2 + 1) % 4]->pt.x + + quad2->corners[(corner2 + 2) % 4]->pt.x) / + 2; + float v4 = (quad2->corners[(corner2 + 1) % 4]->pt.y + + quad2->corners[(corner2 + 2) % 4]->pt.y) / + 2; + + // MARTIN: Heuristic + // For corner2 of quad2 to be considered, + // it needs to be on the same side of the two lines as + // corner1. This is given, if the cross product has + // the same sign for both computations below: + float a3 = u1 - u2; + float b3 = v1 - v2; + // the current corner + float c31 = quad1->corners[corner1]->pt.x - u2; + float d31 = quad1->corners[corner1]->pt.y - v2; + // the candidate corner + float c32 = quad2->corners[corner2]->pt.x - u2; + float d32 = quad2->corners[corner2]->pt.y - v2; + float sign31 = a3 * d31 - c31 * b3; + float sign32 = a3 * d32 - c32 * b3; + + float a4 = u3 - u4; + float b4 = v3 - v4; + // the current corner + float c41 = quad1->corners[corner1]->pt.x - u4; + float d41 = quad1->corners[corner1]->pt.y - v4; + // the candidate corner + float c42 = quad2->corners[corner2]->pt.x - u4; + float d42 = quad2->corners[corner2]->pt.y - v4; + float sign41 = a4 * d41 - c41 * b4; + float sign42 = a4 * d42 - c42 * b4; + + // Also make sure that two border quads of the same row or + // column don't link. Check from the candidate corner's view, + // whether the corner diagonal from the current corner + // is also on the same side of the two lines as the current + // corner and the candidate corner. + float c33 = quad1->corners[(corner1 + 2) % 4]->pt.x - u2; + float d33 = quad1->corners[(corner1 + 2) % 4]->pt.y - v2; + float c43 = quad1->corners[(corner1 + 2) % 4]->pt.x - u4; + float d43 = quad1->corners[(corner1 + 2) % 4]->pt.y - v4; + float sign33 = a3 * d33 - c33 * b3; + float sign43 = a4 * d43 - c43 * b4; + + // This time we also need to make shure, that no quad + // is linked to a quad of another dilation run which + // may lie INSIDE it!!! + // Third: Therefore check everything from the viewpoint + // of the current quad compute midpoints of "parallel" + // quad sides 1 + float x5 = quad1->corners[corner1]->pt.x; + float y5 = quad1->corners[corner1]->pt.y; + float x6 = quad1->corners[(corner1 + 1) % 4]->pt.x; + float y6 = quad1->corners[(corner1 + 1) % 4]->pt.y; + // compute midpoints of "parallel" quad sides 2 + float x7 = x5; + float y7 = y5; + float x8 = quad1->corners[(corner1 + 3) % 4]->pt.x; + float y8 = quad1->corners[(corner1 + 3) % 4]->pt.y; + + // MARTIN: Heuristic + // For corner2 of quad2 to be considered, + // it needs to be on the other side of the two lines than + // corner1. This is given, if the cross product has + // a different sign for both computations below: + float a5 = x6 - x5; + float b5 = y6 - y5; + // the current corner + float c51 = quad1->corners[(corner1 + 2) % 4]->pt.x - x5; + float d51 = quad1->corners[(corner1 + 2) % 4]->pt.y - y5; + // the candidate corner + float c52 = quad2->corners[corner2]->pt.x - x5; + float d52 = quad2->corners[corner2]->pt.y - y5; + float sign51 = a5 * d51 - c51 * b5; + float sign52 = a5 * d52 - c52 * b5; + + float a6 = x8 - x7; + float b6 = y8 - y7; + // the current corner + float c61 = quad1->corners[(corner1 + 2) % 4]->pt.x - x7; + float d61 = quad1->corners[(corner1 + 2) % 4]->pt.y - y7; + // the candidate corner + float c62 = quad2->corners[corner2]->pt.x - x7; + float d62 = quad2->corners[corner2]->pt.y - y7; + float sign61 = a6 * d61 - c61 * b6; + float sign62 = a6 * d62 - c62 * b6; + + // Fourth: Then check everything from the viewpoint of + // the candidate quad compute midpoints of "parallel" + // quad sides 1 + float u5 = quad2->corners[corner2]->pt.x; + float v5 = quad2->corners[corner2]->pt.y; + float u6 = quad2->corners[(corner2 + 1) % 4]->pt.x; + float v6 = quad2->corners[(corner2 + 1) % 4]->pt.y; + // compute midpoints of "parallel" quad sides 2 + float u7 = u5; + float v7 = v5; + float u8 = quad2->corners[(corner2 + 3) % 4]->pt.x; + float v8 = quad2->corners[(corner2 + 3) % 4]->pt.y; + + // MARTIN: Heuristic + // For corner2 of quad2 to be considered, + // it needs to be on the other side of the two lines than + // corner1. This is given, if the cross product has + // a different sign for both computations below: + float a7 = u6 - u5; + float b7 = v6 - v5; + // the current corner + float c71 = quad1->corners[corner1]->pt.x - u5; + float d71 = quad1->corners[corner1]->pt.y - v5; + // the candidate corner + float c72 = quad2->corners[(corner2 + 2) % 4]->pt.x - u5; + float d72 = quad2->corners[(corner2 + 2) % 4]->pt.y - v5; + float sign71 = a7 * d71 - c71 * b7; + float sign72 = a7 * d72 - c72 * b7; + + float a8 = u8 - u7; + float b8 = v8 - v7; + // the current corner + float c81 = quad1->corners[corner1]->pt.x - u7; + float d81 = quad1->corners[corner1]->pt.y - v7; + // the candidate corner + float c82 = quad2->corners[(corner2 + 2) % 4]->pt.x - u7; + float d82 = quad2->corners[(corner2 + 2) % 4]->pt.y - v7; + float sign81 = a8 * d81 - c81 * b8; + float sign82 = a8 * d82 - c82 * b8; + + // Check whether conditions are fulfilled + if (((sign11 < 0 && sign12 < 0) || (sign11 > 0 && sign12 > 0)) && + ((sign21 < 0 && sign22 < 0) || (sign21 > 0 && sign22 > 0)) && + ((sign31 < 0 && sign32 < 0) || (sign31 > 0 && sign32 > 0)) && + ((sign41 < 0 && sign42 < 0) || (sign41 > 0 && sign42 > 0)) && + ((sign11 < 0 && sign13 < 0) || (sign11 > 0 && sign13 > 0)) && + ((sign21 < 0 && sign23 < 0) || (sign21 > 0 && sign23 > 0)) && + ((sign31 < 0 && sign33 < 0) || (sign31 > 0 && sign33 > 0)) && + ((sign41 < 0 && sign43 < 0) || (sign41 > 0 && sign43 > 0)) && + ((sign51 < 0 && sign52 > 0) || (sign51 > 0 && sign52 < 0)) && + ((sign61 < 0 && sign62 > 0) || (sign61 > 0 && sign62 < 0)) && + ((sign71 < 0 && sign72 > 0) || (sign71 > 0 && sign72 < 0)) && + ((sign81 < 0 && sign82 > 0) || (sign81 > 0 && sign82 < 0))) { + return true; + } else { + return false; + } } + +} // namespace camodocal diff --git a/camera_models/src/code_utils/cv_utils.cc b/camera_models/src/code_utils/cv_utils.cc index 028e6215..732fce78 100644 --- a/camera_models/src/code_utils/cv_utils.cc +++ b/camera_models/src/code_utils/cv_utils.cc @@ -1,13 +1,11 @@ #include -cv_utils::fisheye::PreProcess::PreProcess( const cv::Size _raw_image_size, - const cv::Size _roi_size, - const cv::Point _center, - const float _resize_scale ) -: is_preprocess( false ) -, is_resize_only( false ) -{ - /* clang-format off */ +cv_utils::fisheye::PreProcess::PreProcess(const cv::Size _raw_image_size, + const cv::Size _roi_size, + const cv::Point _center, + const float _resize_scale) + : is_preprocess(false), is_resize_only(false) { + /* clang-format off */ if ( _raw_image_size.width >= _roi_size.width && _raw_image_size.height >= _roi_size.height && _center.x <= _raw_image_size.width @@ -17,67 +15,60 @@ cv_utils::fisheye::PreProcess::PreProcess( const cv::Size _raw_image_size, is_preprocess = true; else is_preprocess = false; - /* clang-format on */ + /* clang-format on */ - if ( is_preprocess // - && _raw_image_size.width == _roi_size.width // - && _raw_image_size.height == _roi_size.height ) - { - is_resize_only = true; - std::cout << "[#INFO] resize_only." << std::endl; - } + if (is_preprocess // + && _raw_image_size.width == _roi_size.width // + && _raw_image_size.height == _roi_size.height) { + is_resize_only = true; + std::cout << "[#INFO] resize_only." << std::endl; + } - if ( is_preprocess ) - resetPreProcess( _roi_size, _center, _resize_scale ); - else - std::cout << "[#ERROR] Parameters error." << std::endl; + if (is_preprocess) + resetPreProcess(_roi_size, _center, _resize_scale); + else + std::cout << "[#ERROR] Parameters error." << std::endl; } -void -cv_utils::fisheye::PreProcess::resetPreProcess( cv::Size _roi_size, cv::Point _center, float _resize_scale ) -{ - if ( _resize_scale < 0 ) - resize_scale = 1.0; - else - resize_scale = _resize_scale; +void cv_utils::fisheye::PreProcess::resetPreProcess(cv::Size _roi_size, + cv::Point _center, + float _resize_scale) { + if (_resize_scale < 0) + resize_scale = 1.0; + else + resize_scale = _resize_scale; - roi_row_start = _center.y - _roi_size.height / 2; - roi_row_end = roi_row_start + _roi_size.height; - roi_col_start = _center.x - _roi_size.width / 2; - roi_col_end = roi_col_start + _roi_size.width; + roi_row_start = _center.y - _roi_size.height / 2; + roi_row_end = roi_row_start + _roi_size.height; + roi_col_start = _center.x - _roi_size.width / 2; + roi_col_end = roi_col_start + _roi_size.width; - std::cout << "[#INFO] ROI row: start " << roi_row_start << " ,end " << roi_row_end << std::endl; - std::cout << "[#INFO] ROI col: start " << roi_col_start << " ,end " << roi_col_end << std::endl; + std::cout << "[#INFO] ROI row: start " << roi_row_start << " ,end " + << roi_row_end << std::endl; + std::cout << "[#INFO] ROI col: start " << roi_col_start << " ,end " + << roi_col_end << std::endl; } -cv::Mat -cv_utils::fisheye::PreProcess::do_preprocess( cv::Mat image_input ) -{ - if ( is_resize_only ) - { - cv::Mat image_input_resized; - cv::resize( image_input, - image_input_resized, - cv::Size( image_input.cols * resize_scale, // - image_input.rows * resize_scale ) ); - return image_input_resized; - } - else if ( is_preprocess ) - { - // std::cout << " is_preprocess true " << std::endl; - cv::Mat image_input_roi = image_input( cv::Range( roi_row_start, roi_row_end ), - cv::Range( roi_col_start, roi_col_end ) ); +cv::Mat cv_utils::fisheye::PreProcess::do_preprocess(cv::Mat image_input) { + if (is_resize_only) { + cv::Mat image_input_resized; + cv::resize(image_input, image_input_resized, + cv::Size(image_input.cols * resize_scale, // + image_input.rows * resize_scale)); + return image_input_resized; + } else if (is_preprocess) { + // std::cout << " is_preprocess true " << std::endl; + cv::Mat image_input_roi = + image_input(cv::Range(roi_row_start, roi_row_end), + cv::Range(roi_col_start, roi_col_end)); - cv::Mat image_input_resized; - cv::resize( image_input_roi, - image_input_resized, - cv::Size( image_input_roi.cols * resize_scale, // - image_input_roi.rows * resize_scale ) ); - return image_input_resized; - } - else - { - // std::cout << " is_preprocess false " << std::endl; - return image_input; - } + cv::Mat image_input_resized; + cv::resize(image_input_roi, image_input_resized, + cv::Size(image_input_roi.cols * resize_scale, // + image_input_roi.rows * resize_scale)); + return image_input_resized; + } else { + // std::cout << " is_preprocess false " << std::endl; + return image_input; + } } diff --git a/camera_models/src/code_utils/math_utils/Polynomial.cpp b/camera_models/src/code_utils/math_utils/Polynomial.cpp index 58f4e0de..c6b639ca 100644 --- a/camera_models/src/code_utils/math_utils/Polynomial.cpp +++ b/camera_models/src/code_utils/math_utils/Polynomial.cpp @@ -1,482 +1,395 @@ #include #include + #include using namespace math_utils; -Polynomial::Polynomial( ) {} - -Polynomial::Polynomial( const int _order ) -{ - m_order = _order; - m_coeff = eigen_utils::Vector::Zero( m_order + 1 ); -} +Polynomial::Polynomial() {} -Polynomial::Polynomial( const eigen_utils::Vector _coeff ) -{ - m_order = _coeff.size( ) - 1; - m_coeff = _coeff; +Polynomial::Polynomial(const int _order) { + m_order = _order; + m_coeff = eigen_utils::Vector::Zero(m_order + 1); } -double -Polynomial::getValue( const double in ) -{ - return Evaluate( in ); +Polynomial::Polynomial(const eigen_utils::Vector _coeff) { + m_order = _coeff.size() - 1; + m_coeff = _coeff; } -eigen_utils::Vector -Polynomial::getRealRoot( double _y ) -{ - eigen_utils::Vector realRoots, imagRoots; +double Polynomial::getValue(const double in) { return Evaluate(in); } - FindRoots( _y, m_coeff, realRoots, imagRoots ); +eigen_utils::Vector Polynomial::getRealRoot(double _y) { + eigen_utils::Vector realRoots, imagRoots; - eigen_utils::Vector realRoot; - for ( int i = 0; i < realRoots.size( ); i++ ) - { + FindRoots(_y, m_coeff, realRoots, imagRoots); - if ( imagRoots( i ) == 0 ) - realRoot = eigen_utils::pushback( realRoot, realRoots( i ) ); - } - // std::cout<<"real roots: "<= x_min && realRoots( i ) <= x_max ) - realRoot = eigen_utils::pushback( realRoot, realRoots( i ) ); - } - std::cout << "real roots: " << realRoot << std::endl; - return realRoot; + eigen_utils::Vector realRoot; + for (int i = 0; i < realRoots.size(); i++) { + if (imagRoots(i) == 0 && realRoots(i) >= x_min && realRoots(i) <= x_max) + realRoot = eigen_utils::pushback(realRoot, realRoots(i)); + } + std::cout << "real roots: " << realRoot << std::endl; + return realRoot; } -double -Polynomial::getOneRealRoot( double _y, double x_min, double x_max ) -{ - eigen_utils::Vector realRoots, imagRoots; - - FindRoots( _y, m_coeff, realRoots, imagRoots ); - - // eigen_utils::Vector realRoot; - // std::cout << " max angle " << x_max ; - for ( int i = 0; i < realRoots.size( ); ++i ) - { - if ( imagRoots( i ) == 0 ) - { - // std::cout << " r " << realRoots(i); - if ( realRoots( i ) >= x_min && realRoots( i ) <= x_max ) - { - return realRoots( i ); - // realRoot = eigen_utils::pushback(realRoot, realRoots(i)); - } - } - if ( i == realRoots.size( ) - 1 ) - return 0; - } - // std::cout << std::endl; +double Polynomial::getOneRealRoot(double _y, double x_min, double x_max) { + eigen_utils::Vector realRoots, imagRoots; + + FindRoots(_y, m_coeff, realRoots, imagRoots); - // TODO:method to get one value - // if (realRoot.size() >= 1) - // return realRoot(0); - // else - // return 0; + // eigen_utils::Vector realRoot; + // std::cout << " max angle " << x_max ; + for (int i = 0; i < realRoots.size(); ++i) { + if (imagRoots(i) == 0) { + // std::cout << " r " << realRoots(i); + if (realRoots(i) >= x_min && realRoots(i) <= x_max) { + return realRoots(i); + // realRoot = eigen_utils::pushback(realRoot, realRoots(i)); + } + } + if (i == realRoots.size() - 1) return 0; + } + // std::cout << std::endl; + + // TODO:method to get one value + // if (realRoot.size() >= 1) + // return realRoot(0); + // else + // return 0; } -eigen_utils::Vector -Polynomial::getValue( const eigen_utils::Vector& in ) -{ - eigen_utils::Vector out( in.size( ) ); +eigen_utils::Vector Polynomial::getValue(const eigen_utils::Vector& in) { + eigen_utils::Vector out(in.size()); - for ( int i = in.size( ) - 1; i >= 0; --i ) - { - out( i ) = Evaluate( in( i ) ); - } + for (int i = in.size() - 1; i >= 0; --i) { + out(i) = Evaluate(in(i)); + } - return out; + return out; } -void -Polynomial::setPolyOrder( int _order ) -{ - m_order = _order; - m_coeff.resize( m_order ); +void Polynomial::setPolyOrder(int _order) { + m_order = _order; + m_coeff.resize(m_order); } -int -Polynomial::getPolyOrder( ) const -{ - return m_order; -} +int Polynomial::getPolyOrder() const { return m_order; } -void -Polynomial::setPolyCoeff( const eigen_utils::Vector& value ) -{ - m_coeff = value; +void Polynomial::setPolyCoeff(const eigen_utils::Vector& value) { + m_coeff = value; } -void -Polynomial::setPolyCoeff( int order_index, double value ) -{ - m_coeff( order_index ) = value; +void Polynomial::setPolyCoeff(int order_index, double value) { + m_coeff(order_index) = value; } -eigen_utils::Vector -Polynomial::getPolyCoeff( ) const -{ - return m_coeff; -} +eigen_utils::Vector Polynomial::getPolyCoeff() const { return m_coeff; } -double -Polynomial::getPolyCoeff( int order_index ) const -{ - return m_coeff( order_index ); +double Polynomial::getPolyCoeff(int order_index) const { + return m_coeff(order_index); } -Polynomial& -Polynomial::operator=( const Polynomial& other ) -{ - if ( this != &other ) - { - m_order = other.m_order; - m_coeff = other.m_coeff; - } +Polynomial& Polynomial::operator=(const Polynomial& other) { + if (this != &other) { + m_order = other.m_order; + m_coeff = other.m_coeff; + } - return *this; + return *this; } -string -Polynomial::toString( ) const -{ - std::ostringstream oss; - oss << "Polynomial :" << std::endl; - oss << "| order|" << getPolyOrder( ) << std::endl; - oss << "| coeff|" << getPolyCoeff( ).transpose( ) << std::endl; - return oss.str( ); +string Polynomial::toString() const { + std::ostringstream oss; + oss << "Polynomial :" << std::endl; + oss << "| order|" << getPolyOrder() << std::endl; + oss << "| coeff|" << getPolyCoeff().transpose() << std::endl; + return oss.str(); } -ostream& -operator<<( ostream& out, const Polynomial& poly ) -{ - out << "Polynomial :" << std::endl; - out << "| order|" << poly.getPolyOrder( ) << std::endl; - out << "| coeff|" << poly.getPolyCoeff( ).transpose( ) << std::endl; - return out; +ostream& operator<<(ostream& out, const Polynomial& poly) { + out << "Polynomial :" << std::endl; + out << "| order|" << poly.getPolyOrder() << std::endl; + out << "| coeff|" << poly.getPolyCoeff().transpose() << std::endl; + return out; } -double -Polynomial::Evaluate( double _x ) -{ - double value_out = 0; +double Polynomial::Evaluate(double _x) { + double value_out = 0; - for ( int i = m_order; i >= 0; --i ) - { - value_out += m_coeff( i ) * pow( _x, i ); - } + for (int i = m_order; i >= 0; --i) { + value_out += m_coeff(i) * pow(_x, i); + } - return value_out; + return value_out; } -bool -Polynomial::FindRoots( const double y, const eigen_utils::Vector& polynomial_in, eigen_utils::Vector& real, eigen_utils::Vector& imaginary ) -{ - if ( polynomial_in.size( ) == 0 ) - return false; - - int degree = polynomial_in.size( ) - 1; - - // count high order zero coefferent - int zero_num = 0; - for ( int i = degree; i >= 0; --i ) - { - if ( polynomial_in( i ) == 0.0 ) - zero_num++; - else - break; - } +bool Polynomial::FindRoots(const double y, + const eigen_utils::Vector& polynomial_in, + eigen_utils::Vector& real, + eigen_utils::Vector& imaginary) { + if (polynomial_in.size() == 0) return false; - degree -= zero_num; - eigen_utils::Vector polynomial_coeff( degree + 1 ); - polynomial_coeff = polynomial_in.segment( 0, degree + 1 ); + int degree = polynomial_in.size() - 1; - polynomial_coeff( 0 ) -= y; - - if ( degree == 0 ) - { - std::cout << " Is the polynomial constant?" << std::endl; - return false; - } - // Linear - if ( degree == 1 ) - { - FindLinearPolynomialRoots( polynomial_coeff, real, imaginary ); - return true; - } - if ( degree == 2 ) - { - FindQuadraticPolynomialRoots( polynomial_coeff, real, imaginary ); - return true; - } - else if ( degree > 2 ) - { - // The degree is now known to be at least 3. For cubic or higher - // roots we use the method of companion matrices. - - // Divide by leading term - const double leading_term = polynomial_coeff( degree ); - // std::cout<< " polynomial_in: " << polynomial_in.transpose() << - // std::endl; - - polynomial_coeff /= leading_term; - - // Build and balance the companion matrix to the polynomial. - eigen_utils::Matrix companion_matrix( degree, degree ); - BuildCompanionMatrix( polynomial_coeff, &companion_matrix ); - // BalanceCompanionMatrix(&companion_matrix); - // Find its (complex) eigenvalues. - Eigen::EigenSolver< eigen_utils::Matrix > solver( companion_matrix, false ); - if ( solver.info( ) != Eigen::Success ) - { - return false; - } - else - { - real = solver.eigenvalues( ).real( ); - imaginary = solver.eigenvalues( ).imag( ); - - return true; - } - } + // count high order zero coefferent + int zero_num = 0; + for (int i = degree; i >= 0; --i) { + if (polynomial_in(i) == 0.0) + zero_num++; else - return false; + break; + } + + degree -= zero_num; + eigen_utils::Vector polynomial_coeff(degree + 1); + polynomial_coeff = polynomial_in.segment(0, degree + 1); + + polynomial_coeff(0) -= y; + + if (degree == 0) { + std::cout << " Is the polynomial constant?" << std::endl; + return false; + } + // Linear + if (degree == 1) { + FindLinearPolynomialRoots(polynomial_coeff, real, imaginary); + return true; + } + if (degree == 2) { + FindQuadraticPolynomialRoots(polynomial_coeff, real, imaginary); + return true; + } else if (degree > 2) { + // The degree is now known to be at least 3. For cubic or higher + // roots we use the method of companion matrices. + + // Divide by leading term + const double leading_term = polynomial_coeff(degree); + // std::cout<< " polynomial_in: " << polynomial_in.transpose() << + // std::endl; + + polynomial_coeff /= leading_term; + + // Build and balance the companion matrix to the polynomial. + eigen_utils::Matrix companion_matrix(degree, degree); + BuildCompanionMatrix(polynomial_coeff, &companion_matrix); + // BalanceCompanionMatrix(&companion_matrix); + // Find its (complex) eigenvalues. + Eigen::EigenSolver solver(companion_matrix, false); + if (solver.info() != Eigen::Success) { + return false; + } else { + real = solver.eigenvalues().real(); + imaginary = solver.eigenvalues().imag(); + + return true; + } + } else + return false; } -void -Polynomial::FindLinearPolynomialRoots( const eigen_utils::Vector& polynomial, eigen_utils::Vector& real, eigen_utils::Vector& imag ) -{ - real.resize( 1 ); - imag.resize( 1 ); +void Polynomial::FindLinearPolynomialRoots( + const eigen_utils::Vector& polynomial, eigen_utils::Vector& real, + eigen_utils::Vector& imag) { + real.resize(1); + imag.resize(1); - real( 0 ) = -polynomial( 0 ) / polynomial( 1 ); - imag( 0 ) = 0; + real(0) = -polynomial(0) / polynomial(1); + imag(0) = 0; } -void -Polynomial::FindQuadraticPolynomialRoots( const eigen_utils::Vector& polynomial, - eigen_utils::Vector& real, - eigen_utils::Vector& imag ) -{ - const double a = polynomial( 2 ); - const double b = polynomial( 1 ); - const double c = polynomial( 0 ); - const double D = b * b - 4 * a * c; - const double sqrt_D = sqrt( fabs( D ) ); - - real.resize( 2 ); - imag.resize( 2 ); - - // Real roots. - if ( D >= 0 ) - { - // Stable quadratic roots according to BKP Horn. - // http://people.csail.mit.edu/bkph/articles/Quadratics.pdf - if ( b >= 0 ) - { - real( 0 ) = ( -b - sqrt_D ) / ( 2.0 * a ); - imag( 0 ) = 0; - - real( 1 ) = ( 2.0 * c ) / ( -b - sqrt_D ); - imag( 1 ) = 0; - return; - } - else - { - real( 0 ) = ( 2.0 * c ) / ( -b + sqrt_D ); - imag( 0 ) = 0; - - real( 1 ) = ( -b + sqrt_D ) / ( 2.0 * a ); - imag( 1 ) = 0; - return; - } - } - else - { - // Use the normal quadratic formula for the complex case. - real( 0 ) = -b / ( 2.0 * a ); - imag( 0 ) = sqrt_D / ( 2.0 * a ); - - real( 1 ) = -b / ( 2.0 * a ); - imag( 1 ) = -sqrt_D / ( 2.0 * a ); - return; +void Polynomial::FindQuadraticPolynomialRoots( + const eigen_utils::Vector& polynomial, eigen_utils::Vector& real, + eigen_utils::Vector& imag) { + const double a = polynomial(2); + const double b = polynomial(1); + const double c = polynomial(0); + const double D = b * b - 4 * a * c; + const double sqrt_D = sqrt(fabs(D)); + + real.resize(2); + imag.resize(2); + + // Real roots. + if (D >= 0) { + // Stable quadratic roots according to BKP Horn. + // http://people.csail.mit.edu/bkph/articles/Quadratics.pdf + if (b >= 0) { + real(0) = (-b - sqrt_D) / (2.0 * a); + imag(0) = 0; + + real(1) = (2.0 * c) / (-b - sqrt_D); + imag(1) = 0; + return; + } else { + real(0) = (2.0 * c) / (-b + sqrt_D); + imag(0) = 0; + + real(1) = (-b + sqrt_D) / (2.0 * a); + imag(1) = 0; + return; } + } else { + // Use the normal quadratic formula for the complex case. + real(0) = -b / (2.0 * a); + imag(0) = sqrt_D / (2.0 * a); + + real(1) = -b / (2.0 * a); + imag(1) = -sqrt_D / (2.0 * a); + return; + } } -void -Polynomial::BuildCompanionMatrix( const eigen_utils::Vector& polynomial, eigen_utils::Matrix* companion_matrix_ptr ) -{ - eigen_utils::Matrix& companion_matrix = *companion_matrix_ptr; +void Polynomial::BuildCompanionMatrix( + const eigen_utils::Vector& polynomial, + eigen_utils::Matrix* companion_matrix_ptr) { + eigen_utils::Matrix& companion_matrix = *companion_matrix_ptr; - const int degree = polynomial.size( ) - 1; + const int degree = polynomial.size() - 1; - // companion_matrix.resize(degree, degree); - companion_matrix.setZero( ); - companion_matrix.diagonal( -1 ).setOnes( ); - companion_matrix.col( degree - 1 ) = -polynomial.head( degree ); + // companion_matrix.resize(degree, degree); + companion_matrix.setZero(); + companion_matrix.diagonal(-1).setOnes(); + companion_matrix.col(degree - 1) = -polynomial.head(degree); } -void -Polynomial::BalanceCompanionMatrix( eigen_utils::Matrix* companion_matrix_ptr ) -{ - eigen_utils::Matrix& companion_matrix = *companion_matrix_ptr; - eigen_utils::Matrix companion_matrix_offdiagonal = companion_matrix; - companion_matrix_offdiagonal.diagonal( ).setZero( ); - - const int degree = companion_matrix.rows( ); - - // gamma <= 1 controls how much a change in the scaling has to - // lower the 1-norm of the companion matrix to be accepted. - // - // gamma = 1 seems to lead to cycles (numerical issues?), so - // we set it slightly lower. - const double gamma = 0.9; - - // Greedily scale row/column pairs until there is no change. - bool scaling_has_changed; - do - { - scaling_has_changed = false; - - for ( int i = 0; i < degree; ++i ) - { - const double row_norm = companion_matrix_offdiagonal.row( i ).lpNorm< 1 >( ); - const double col_norm = companion_matrix_offdiagonal.col( i ).lpNorm< 1 >( ); - - // Decompose row_norm/col_norm into mantissa * 2^exponent, - // where 0.5 <= mantissa < 1. Discard mantissa (return value - // of frexp), as only the exponent is needed. - int exponent = 0; - std::frexp( row_norm / col_norm, &exponent ); - exponent /= 2; - - if ( exponent != 0 ) - { - const double scaled_col_norm = std::ldexp( col_norm, exponent ); - const double scaled_row_norm = std::ldexp( row_norm, -exponent ); - if ( scaled_col_norm + scaled_row_norm < gamma * ( col_norm + row_norm ) ) - { - // Accept the new scaling. (Multiplication by powers of 2 - // should not - // introduce rounding errors (ignoring non-normalized - // numbers and - // over- or underflow)) - scaling_has_changed = true; - companion_matrix_offdiagonal.row( i ) *= std::ldexp( 1.0, -exponent ); - companion_matrix_offdiagonal.col( i ) *= std::ldexp( 1.0, exponent ); - } - } +void Polynomial::BalanceCompanionMatrix( + eigen_utils::Matrix* companion_matrix_ptr) { + eigen_utils::Matrix& companion_matrix = *companion_matrix_ptr; + eigen_utils::Matrix companion_matrix_offdiagonal = companion_matrix; + companion_matrix_offdiagonal.diagonal().setZero(); + + const int degree = companion_matrix.rows(); + + // gamma <= 1 controls how much a change in the scaling has to + // lower the 1-norm of the companion matrix to be accepted. + // + // gamma = 1 seems to lead to cycles (numerical issues?), so + // we set it slightly lower. + const double gamma = 0.9; + + // Greedily scale row/column pairs until there is no change. + bool scaling_has_changed; + do { + scaling_has_changed = false; + + for (int i = 0; i < degree; ++i) { + const double row_norm = companion_matrix_offdiagonal.row(i).lpNorm<1>(); + const double col_norm = companion_matrix_offdiagonal.col(i).lpNorm<1>(); + + // Decompose row_norm/col_norm into mantissa * 2^exponent, + // where 0.5 <= mantissa < 1. Discard mantissa (return value + // of frexp), as only the exponent is needed. + int exponent = 0; + std::frexp(row_norm / col_norm, &exponent); + exponent /= 2; + + if (exponent != 0) { + const double scaled_col_norm = std::ldexp(col_norm, exponent); + const double scaled_row_norm = std::ldexp(row_norm, -exponent); + if (scaled_col_norm + scaled_row_norm < gamma * (col_norm + row_norm)) { + // Accept the new scaling. (Multiplication by powers of 2 + // should not + // introduce rounding errors (ignoring non-normalized + // numbers and + // over- or underflow)) + scaling_has_changed = true; + companion_matrix_offdiagonal.row(i) *= std::ldexp(1.0, -exponent); + companion_matrix_offdiagonal.col(i) *= std::ldexp(1.0, exponent); } - } while ( scaling_has_changed ); + } + } + } while (scaling_has_changed); - companion_matrix_offdiagonal.diagonal( ) = companion_matrix.diagonal( ); - companion_matrix = companion_matrix_offdiagonal; + companion_matrix_offdiagonal.diagonal() = companion_matrix.diagonal(); + companion_matrix = companion_matrix_offdiagonal; } -PolynomialFit::PolynomialFit( int _order ) -: Polynomial( _order ) -{ - poly = new Polynomial( _order ); - samples.clear( ); - data_size = 0; +PolynomialFit::PolynomialFit(int _order) : Polynomial(_order) { + poly = new Polynomial(_order); + samples.clear(); + data_size = 0; } -PolynomialFit::PolynomialFit( int _order, eigen_utils::Vector _x, eigen_utils::Vector _y ) -: Polynomial( _order ) -{ - poly = new Polynomial( _order ); - samples.clear( ); - data_size = 0; +PolynomialFit::PolynomialFit(int _order, eigen_utils::Vector _x, + eigen_utils::Vector _y) + : Polynomial(_order) { + poly = new Polynomial(_order); + samples.clear(); + data_size = 0; - loadSamples( _x, _y ); + loadSamples(_x, _y); } -void -PolynomialFit::loadSamples( const eigen_utils::Vector& x, const eigen_utils::Vector& y ) -{ - if ( x.size( ) != y.size( ) ) - return; - - for ( int i = x.size( ) - 1; i >= 0; --i ) - { - Sample sample; - sample.x = x( i ); - sample.y = y( i ); - samples.push_back( sample ); - data_size++; - } -} +void PolynomialFit::loadSamples(const eigen_utils::Vector& x, + const eigen_utils::Vector& y) { + if (x.size() != y.size()) return; -void -PolynomialFit::loadSample( const Sample sample ) -{ - samples.push_back( sample ); + for (int i = x.size() - 1; i >= 0; --i) { + Sample sample; + sample.x = x(i); + sample.y = y(i); + samples.push_back(sample); data_size++; + } } -void -PolynomialFit::clearSamples( ) -{ - samples.clear( ); - data_size = 0; +void PolynomialFit::loadSample(const Sample sample) { + samples.push_back(sample); + data_size++; } -eigen_utils::Vector -PolynomialFit::getCoeff( ) -{ - coeff = this->Fit( ); - poly->setPolyCoeff( coeff ); - - return coeff; +void PolynomialFit::clearSamples() { + samples.clear(); + data_size = 0; } -Polynomial& -PolynomialFit::getPolynomial( ) -{ - return *poly; -} +eigen_utils::Vector PolynomialFit::getCoeff() { + coeff = this->Fit(); + poly->setPolyCoeff(coeff); -eigen_utils::Vector -PolynomialFit::Fit( ) -{ - int num_constraints = data_size; + return coeff; +} - const int degree = this->getPolyOrder( ); +Polynomial& PolynomialFit::getPolynomial() { return *poly; } - eigen_utils::Matrix lhs = eigen_utils::Matrix::Zero( num_constraints, num_constraints ); - eigen_utils::Vector rhs = eigen_utils::Vector::Zero( num_constraints ); +eigen_utils::Vector PolynomialFit::Fit() { + int num_constraints = data_size; - int row = 0; + const int degree = this->getPolyOrder(); - for ( int i = 0; i < data_size; ++i ) - { - const Sample& sample = samples[i]; + eigen_utils::Matrix lhs = + eigen_utils::Matrix::Zero(num_constraints, num_constraints); + eigen_utils::Vector rhs = eigen_utils::Vector::Zero(num_constraints); - for ( int j = 0; j <= degree; ++j ) - { - lhs( row, j ) = pow( sample.x, degree - j ); - } + int row = 0; - rhs( row ) = sample.y; + for (int i = 0; i < data_size; ++i) { + const Sample& sample = samples[i]; - ++row; + for (int j = 0; j <= degree; ++j) { + lhs(row, j) = pow(sample.x, degree - j); } - return ( lhs.fullPivLu( ).solve( rhs ).segment( 0, degree + 1 ) ).reverse( ); + rhs(row) = sample.y; + + ++row; + } + + return (lhs.fullPivLu().solve(rhs).segment(0, degree + 1)).reverse(); } diff --git a/camera_models/src/code_utils/poly_test.cpp b/camera_models/src/code_utils/poly_test.cpp index 657493b7..c96b8f08 100644 --- a/camera_models/src/code_utils/poly_test.cpp +++ b/camera_models/src/code_utils/poly_test.cpp @@ -1,137 +1,137 @@ -#include -#include - -#include #include - +#include #include +#include +#include + using namespace std; using namespace Eigen; -void fit_test() -{ +void fit_test() { eigen_utils::Vector xx(101); eigen_utils::Vector out(101); - xx<< - 0 ,0.0261799 ,0.0523599 ,0.0785398 , 0.10472 , 0.1309 , 0.15708 , 0.18326 , - 0.20944 , 0.235619 , 0.261799 ,0.287979 ,0.314159 , 0.340339 ,0.366519 , - 0.392699 , 0.418879 , 0.445059 ,0.471239 ,0.497419 , 0.523599 ,0.549779 , - 0.575959 , 0.602139 , 0.628319 ,0.654498 ,0.680678 , 0.706858 ,0.733038 , - 0.759218, 0.785398, 0.811578 , 0.837758 , 0.863938, 0.890118 , 0.916298 , - 0.942478, 0.968658, 0.994838 , 1.02102 , 1.0472, 1.07338 , 1.09956 , - 1.12574 , 1.15192 , 1.1781 , 1.20428 , 1.23046 , 1.25664 , 1.28282 , - 1.309 , 1.33518 , 1.36136 , 1.38754 , 1.41372 , 1.4399 , 1.46608 , - 1.49226 , 1.51844 , 1.54462 , 1.5708 , 1.59698 , 1.62316 , 1.64934 , - 1.67552 , 1.7017 , 1.72788 , 1.75406 , 1.78024 , 1.80642 , 1.8326 , - 1.85878 , 1.88496 , 1.91114 , 1.93732 , 1.9635 , 1.98968 , 2.01586 , - 2.04204 , 2.06822 , 2.0944 , 2.12058 , 2.14675 , 2.17293 , 2.19911 , - 2.22529 , 2.25147 , 2.27765 , 2.30383 , 2.33001 , 2.35619 , 2.38237 , - 2.40855 , 2.43473 , 2.46091 , 2.48709 , 2.51327 , 2.53945 , 2.56563 , - 2.59181 , 2.61799 ; - - out<< 0, - 0.0261771 ,0.0523467, 0.0785066, 0.104654 , 0.130789 , 0.156907, 0.183008 , 0.209091, 0.235154 , 0.261196 , - 0.287216 ,0.313212 , 0.339184 , 0.365131 ,0.391052 ,0.416945 , 0.44281 ,0.468646 , 0.494452 ,0.520226 , - 0.545968 ,0.571676 , 0.597349 , 0.622986 ,0.648584 ,0.674144 , 0.699662 ,0.725137 , 0.750567 ,0.775951 , - 0.801285 ,0.826568 , 0.851797 , 0.876968 , 0.90208 ,0.927128 , 0.95211 ,0.977021 , 1.00186 , 1.02661 , - 1.05129 ,1.07587 , 1.10036 , 1.12475 ,1.14903 ,1.17319 , 1.19724 ,1.22115 , 1.24492 ,1.26855 , - 1.29201 , 1.3153 , 1.33841 , 1.36132 ,1.38402 , 1.4065 , 1.42873 , 1.4507 , 1.4724 ,1.49379 , - 1.51486 ,1.53558 , 1.55594 , 1.57589 ,1.59541 ,1.61448 , 1.63305 ,1.65109 , 1.66857 ,1.68543 , - 1.70165 ,1.71716 , 1.73193 , 1.7459 , 1.759 ,1.77119 , 1.7824 ,1.79256 , 1.80159 ,1.80943 , - 1.81599 ,1.82119 , 1.82494 , 1.82713 ,1.82767 ,1.82646 , 1.82338 ,1.81831 , 1.81112 ,1.80168 , - 1.78986 ,1.77551 , 1.75847 , 1.73857 ,1.71566 ,1.68955 , 1.66005 ,1.62696 , 1.59008 ,1.54919 ; - - - math_utils::PolynomialFit polyfit(24,out, xx); - math_utils::Polynomial poly = polyfit.getCoeff(); - std::cout << "polyfit :"<getPolyCoeff().transpose() <getPolyCoeff().transpose() << std::endl; std::cout << ":----------------------------------------:" << std::endl; eigen_utils::Vector realroot = poly.getRealRoot(0.0); - std::cout << " Roots :"< -namespace camodocal -{ +namespace camodocal { -bool -EigenQuaternionParameterization::Plus(const double* x, - const double* delta, - double* x_plus_delta) const -{ - const double norm_delta = - sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]); - if (norm_delta > 0.0) - { - const double sin_delta_by_delta = (sin(norm_delta) / norm_delta); - double q_delta[4]; - q_delta[0] = sin_delta_by_delta * delta[0]; - q_delta[1] = sin_delta_by_delta * delta[1]; - q_delta[2] = sin_delta_by_delta * delta[2]; - q_delta[3] = cos(norm_delta); - EigenQuaternionProduct(q_delta, x, x_plus_delta); +bool EigenQuaternionParameterization::Plus(const double* x, const double* delta, + double* x_plus_delta) const { + const double norm_delta = + sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]); + if (norm_delta > 0.0) { + const double sin_delta_by_delta = (sin(norm_delta) / norm_delta); + double q_delta[4]; + q_delta[0] = sin_delta_by_delta * delta[0]; + q_delta[1] = sin_delta_by_delta * delta[1]; + q_delta[2] = sin_delta_by_delta * delta[2]; + q_delta[3] = cos(norm_delta); + EigenQuaternionProduct(q_delta, x, x_plus_delta); + } else { + for (int i = 0; i < 4; ++i) { + x_plus_delta[i] = x[i]; } - else - { - for (int i = 0; i < 4; ++i) - { - x_plus_delta[i] = x[i]; - } - } - return true; + } + return true; } -bool -EigenQuaternionParameterization::ComputeJacobian(const double* x, - double* jacobian) const -{ - jacobian[0] = x[3]; jacobian[1] = x[2]; jacobian[2] = -x[1]; // NOLINT - jacobian[3] = -x[2]; jacobian[4] = x[3]; jacobian[5] = x[0]; // NOLINT - jacobian[6] = x[1]; jacobian[7] = -x[0]; jacobian[8] = x[3]; // NOLINT - jacobian[9] = -x[0]; jacobian[10] = -x[1]; jacobian[11] = -x[2]; // NOLINT - return true; +bool EigenQuaternionParameterization::ComputeJacobian(const double* x, + double* jacobian) const { + jacobian[0] = x[3]; + jacobian[1] = x[2]; + jacobian[2] = -x[1]; // NOLINT + jacobian[3] = -x[2]; + jacobian[4] = x[3]; + jacobian[5] = x[0]; // NOLINT + jacobian[6] = x[1]; + jacobian[7] = -x[0]; + jacobian[8] = x[3]; // NOLINT + jacobian[9] = -x[0]; + jacobian[10] = -x[1]; + jacobian[11] = -x[2]; // NOLINT + return true; } -} +} // namespace camodocal diff --git a/camera_models/src/gpl/gpl.cc b/camera_models/src/gpl/gpl.cc index 9142a3a7..fde21411 100644 --- a/camera_models/src/gpl/gpl.cc +++ b/camera_models/src/gpl/gpl.cc @@ -7,8 +7,8 @@ #include #endif - -// source: https://stackoverflow.com/questions/5167269/clock-gettime-alternative-in-mac-os-x +// source: +// https://stackoverflow.com/questions/5167269/clock-gettime-alternative-in-mac-os-x #ifdef __APPLE__ #include #define ORWL_NANO (+1.0E-9) @@ -20,7 +20,7 @@ static uint64_t orwl_timestart = 0; struct timespec orwl_gettime(void) { // be more careful in a multithreaded environement if (!orwl_timestart) { - mach_timebase_info_data_t tb = { 0 }; + mach_timebase_info_data_t tb = {0}; mach_timebase_info(&tb); orwl_timebase = tb.numer; orwl_timebase /= tb.denom; @@ -32,8 +32,7 @@ struct timespec orwl_gettime(void) { t.tv_nsec = diff - (t.tv_sec * ORWL_GIGA); return t; } -#endif // __APPLE__ - +#endif // __APPLE__ const double WGS84_A = 6378137.0; const double WGS84_ECCSQ = 0.00669437999013; @@ -43,886 +42,698 @@ const double WGS84_ECCSQ = 0.00669437999013; #define fminf(x, y) (((x) < (y)) ? (x) : (y)) #endif -namespace camodocal -{ +namespace camodocal { -double hypot3(double x, double y, double z) -{ - return sqrt(square(x) + square(y) + square(z)); +double hypot3(double x, double y, double z) { + return sqrt(square(x) + square(y) + square(z)); } -float hypot3f(float x, float y, float z) -{ - return sqrtf(square(x) + square(y) + square(z)); +float hypot3f(float x, float y, float z) { + return sqrtf(square(x) + square(y) + square(z)); } -double d2r(double deg) -{ - return deg / 180.0 * M_PI; -} +double d2r(double deg) { return deg / 180.0 * M_PI; } -float d2r(float deg) -{ - return deg / 180.0f * M_PI; -} +float d2r(float deg) { return deg / 180.0f * M_PI; } -double r2d(double rad) -{ - return rad / M_PI * 180.0; -} +double r2d(double rad) { return rad / M_PI * 180.0; } -float r2d(float rad) -{ - return rad / M_PI * 180.0f; -} +float r2d(float rad) { return rad / M_PI * 180.0f; } -double sinc(double theta) -{ - return sin(theta) / theta; -} +double sinc(double theta) { return sin(theta) / theta; } #ifdef _WIN32 #include #include #include LARGE_INTEGER -getFILETIMEoffset() -{ - SYSTEMTIME s; - FILETIME f; - LARGE_INTEGER t; - - s.wYear = 1970; - s.wMonth = 1; - s.wDay = 1; - s.wHour = 0; - s.wMinute = 0; - s.wSecond = 0; - s.wMilliseconds = 0; - SystemTimeToFileTime(&s, &f); - t.QuadPart = f.dwHighDateTime; - t.QuadPart <<= 32; - t.QuadPart |= f.dwLowDateTime; - return (t); +getFILETIMEoffset() { + SYSTEMTIME s; + FILETIME f; + LARGE_INTEGER t; + + s.wYear = 1970; + s.wMonth = 1; + s.wDay = 1; + s.wHour = 0; + s.wMinute = 0; + s.wSecond = 0; + s.wMilliseconds = 0; + SystemTimeToFileTime(&s, &f); + t.QuadPart = f.dwHighDateTime; + t.QuadPart <<= 32; + t.QuadPart |= f.dwLowDateTime; + return (t); } -int -clock_gettime(int X, struct timespec *tp) -{ - LARGE_INTEGER t; - FILETIME f; - double microseconds; - static LARGE_INTEGER offset; - static double frequencyToMicroseconds; - static int initialized = 0; - static BOOL usePerformanceCounter = 0; - - if (!initialized) { - LARGE_INTEGER performanceFrequency; - initialized = 1; - usePerformanceCounter = QueryPerformanceFrequency(&performanceFrequency); - if (usePerformanceCounter) { - QueryPerformanceCounter(&offset); - frequencyToMicroseconds = (double)performanceFrequency.QuadPart / 1000000.; - } else { - offset = getFILETIMEoffset(); - frequencyToMicroseconds = 10.; - } - } - if (usePerformanceCounter) QueryPerformanceCounter(&t); - else { - GetSystemTimeAsFileTime(&f); - t.QuadPart = f.dwHighDateTime; - t.QuadPart <<= 32; - t.QuadPart |= f.dwLowDateTime; +int clock_gettime(int X, struct timespec* tp) { + LARGE_INTEGER t; + FILETIME f; + double microseconds; + static LARGE_INTEGER offset; + static double frequencyToMicroseconds; + static int initialized = 0; + static BOOL usePerformanceCounter = 0; + + if (!initialized) { + LARGE_INTEGER performanceFrequency; + initialized = 1; + usePerformanceCounter = QueryPerformanceFrequency(&performanceFrequency); + if (usePerformanceCounter) { + QueryPerformanceCounter(&offset); + frequencyToMicroseconds = + (double)performanceFrequency.QuadPart / 1000000.; + } else { + offset = getFILETIMEoffset(); + frequencyToMicroseconds = 10.; } + } + if (usePerformanceCounter) + QueryPerformanceCounter(&t); + else { + GetSystemTimeAsFileTime(&f); + t.QuadPart = f.dwHighDateTime; + t.QuadPart <<= 32; + t.QuadPart |= f.dwLowDateTime; + } - t.QuadPart -= offset.QuadPart; - microseconds = (double)t.QuadPart / frequencyToMicroseconds; - t.QuadPart = microseconds; - tp->tv_sec = t.QuadPart / 1000000; - tp->tv_nsec = (t.QuadPart % 1000000) * 1000; - return (0); + t.QuadPart -= offset.QuadPart; + microseconds = (double)t.QuadPart / frequencyToMicroseconds; + t.QuadPart = microseconds; + tp->tv_sec = t.QuadPart / 1000000; + tp->tv_nsec = (t.QuadPart % 1000000) * 1000; + return (0); } #endif -unsigned long long timeInMicroseconds(void) -{ - struct timespec tp; +unsigned long long timeInMicroseconds(void) { + struct timespec tp; #ifdef __APPLE__ - tp = orwl_gettime(); + tp = orwl_gettime(); #else - clock_gettime(CLOCK_REALTIME, &tp); + clock_gettime(CLOCK_REALTIME, &tp); #endif - return tp.tv_sec * 1000000 + tp.tv_nsec / 1000; + return tp.tv_sec * 1000000 + tp.tv_nsec / 1000; } -double timeInSeconds(void) -{ - struct timespec tp; +double timeInSeconds(void) { + struct timespec tp; #ifdef __APPLE__ - tp = orwl_gettime(); + tp = orwl_gettime(); #else - clock_gettime(CLOCK_REALTIME, &tp); + clock_gettime(CLOCK_REALTIME, &tp); #endif - return static_cast(tp.tv_sec) + - static_cast(tp.tv_nsec) / 1000000000.0; + return static_cast(tp.tv_sec) + + static_cast(tp.tv_nsec) / 1000000000.0; } -float colormapAutumn[128][3] = -{ - {1.0f,0.f,0.f}, - {1.0f,0.007874f,0.f}, - {1.0f,0.015748f,0.f}, - {1.0f,0.023622f,0.f}, - {1.0f,0.031496f,0.f}, - {1.0f,0.03937f,0.f}, - {1.0f,0.047244f,0.f}, - {1.0f,0.055118f,0.f}, - {1.0f,0.062992f,0.f}, - {1.0f,0.070866f,0.f}, - {1.0f,0.07874f,0.f}, - {1.0f,0.086614f,0.f}, - {1.0f,0.094488f,0.f}, - {1.0f,0.10236f,0.f}, - {1.0f,0.11024f,0.f}, - {1.0f,0.11811f,0.f}, - {1.0f,0.12598f,0.f}, - {1.0f,0.13386f,0.f}, - {1.0f,0.14173f,0.f}, - {1.0f,0.14961f,0.f}, - {1.0f,0.15748f,0.f}, - {1.0f,0.16535f,0.f}, - {1.0f,0.17323f,0.f}, - {1.0f,0.1811f,0.f}, - {1.0f,0.18898f,0.f}, - {1.0f,0.19685f,0.f}, - {1.0f,0.20472f,0.f}, - {1.0f,0.2126f,0.f}, - {1.0f,0.22047f,0.f}, - {1.0f,0.22835f,0.f}, - {1.0f,0.23622f,0.f}, - {1.0f,0.24409f,0.f}, - {1.0f,0.25197f,0.f}, - {1.0f,0.25984f,0.f}, - {1.0f,0.26772f,0.f}, - {1.0f,0.27559f,0.f}, - {1.0f,0.28346f,0.f}, - {1.0f,0.29134f,0.f}, - {1.0f,0.29921f,0.f}, - {1.0f,0.30709f,0.f}, - {1.0f,0.31496f,0.f}, - {1.0f,0.32283f,0.f}, - {1.0f,0.33071f,0.f}, - {1.0f,0.33858f,0.f}, - {1.0f,0.34646f,0.f}, - {1.0f,0.35433f,0.f}, - {1.0f,0.3622f,0.f}, - {1.0f,0.37008f,0.f}, - {1.0f,0.37795f,0.f}, - {1.0f,0.38583f,0.f}, - {1.0f,0.3937f,0.f}, - {1.0f,0.40157f,0.f}, - {1.0f,0.40945f,0.f}, - {1.0f,0.41732f,0.f}, - {1.0f,0.4252f,0.f}, - {1.0f,0.43307f,0.f}, - {1.0f,0.44094f,0.f}, - {1.0f,0.44882f,0.f}, - {1.0f,0.45669f,0.f}, - {1.0f,0.46457f,0.f}, - {1.0f,0.47244f,0.f}, - {1.0f,0.48031f,0.f}, - {1.0f,0.48819f,0.f}, - {1.0f,0.49606f,0.f}, - {1.0f,0.50394f,0.f}, - {1.0f,0.51181f,0.f}, - {1.0f,0.51969f,0.f}, - {1.0f,0.52756f,0.f}, - {1.0f,0.53543f,0.f}, - {1.0f,0.54331f,0.f}, - {1.0f,0.55118f,0.f}, - {1.0f,0.55906f,0.f}, - {1.0f,0.56693f,0.f}, - {1.0f,0.5748f,0.f}, - {1.0f,0.58268f,0.f}, - {1.0f,0.59055f,0.f}, - {1.0f,0.59843f,0.f}, - {1.0f,0.6063f,0.f}, - {1.0f,0.61417f,0.f}, - {1.0f,0.62205f,0.f}, - {1.0f,0.62992f,0.f}, - {1.0f,0.6378f,0.f}, - {1.0f,0.64567f,0.f}, - {1.0f,0.65354f,0.f}, - {1.0f,0.66142f,0.f}, - {1.0f,0.66929f,0.f}, - {1.0f,0.67717f,0.f}, - {1.0f,0.68504f,0.f}, - {1.0f,0.69291f,0.f}, - {1.0f,0.70079f,0.f}, - {1.0f,0.70866f,0.f}, - {1.0f,0.71654f,0.f}, - {1.0f,0.72441f,0.f}, - {1.0f,0.73228f,0.f}, - {1.0f,0.74016f,0.f}, - {1.0f,0.74803f,0.f}, - {1.0f,0.75591f,0.f}, - {1.0f,0.76378f,0.f}, - {1.0f,0.77165f,0.f}, - {1.0f,0.77953f,0.f}, - {1.0f,0.7874f,0.f}, - {1.0f,0.79528f,0.f}, - {1.0f,0.80315f,0.f}, - {1.0f,0.81102f,0.f}, - {1.0f,0.8189f,0.f}, - {1.0f,0.82677f,0.f}, - {1.0f,0.83465f,0.f}, - {1.0f,0.84252f,0.f}, - {1.0f,0.85039f,0.f}, - {1.0f,0.85827f,0.f}, - {1.0f,0.86614f,0.f}, - {1.0f,0.87402f,0.f}, - {1.0f,0.88189f,0.f}, - {1.0f,0.88976f,0.f}, - {1.0f,0.89764f,0.f}, - {1.0f,0.90551f,0.f}, - {1.0f,0.91339f,0.f}, - {1.0f,0.92126f,0.f}, - {1.0f,0.92913f,0.f}, - {1.0f,0.93701f,0.f}, - {1.0f,0.94488f,0.f}, - {1.0f,0.95276f,0.f}, - {1.0f,0.96063f,0.f}, - {1.0f,0.9685f,0.f}, - {1.0f,0.97638f,0.f}, - {1.0f,0.98425f,0.f}, - {1.0f,0.99213f,0.f}, - {1.0f,1.0f,0.0f} -}; - - -float colormapJet[128][3] = -{ - {0.0f,0.0f,0.53125f}, - {0.0f,0.0f,0.5625f}, - {0.0f,0.0f,0.59375f}, - {0.0f,0.0f,0.625f}, - {0.0f,0.0f,0.65625f}, - {0.0f,0.0f,0.6875f}, - {0.0f,0.0f,0.71875f}, - {0.0f,0.0f,0.75f}, - {0.0f,0.0f,0.78125f}, - {0.0f,0.0f,0.8125f}, - {0.0f,0.0f,0.84375f}, - {0.0f,0.0f,0.875f}, - {0.0f,0.0f,0.90625f}, - {0.0f,0.0f,0.9375f}, - {0.0f,0.0f,0.96875f}, - {0.0f,0.0f,1.0f}, - {0.0f,0.03125f,1.0f}, - {0.0f,0.0625f,1.0f}, - {0.0f,0.09375f,1.0f}, - {0.0f,0.125f,1.0f}, - {0.0f,0.15625f,1.0f}, - {0.0f,0.1875f,1.0f}, - {0.0f,0.21875f,1.0f}, - {0.0f,0.25f,1.0f}, - {0.0f,0.28125f,1.0f}, - {0.0f,0.3125f,1.0f}, - {0.0f,0.34375f,1.0f}, - {0.0f,0.375f,1.0f}, - {0.0f,0.40625f,1.0f}, - {0.0f,0.4375f,1.0f}, - {0.0f,0.46875f,1.0f}, - {0.0f,0.5f,1.0f}, - {0.0f,0.53125f,1.0f}, - {0.0f,0.5625f,1.0f}, - {0.0f,0.59375f,1.0f}, - {0.0f,0.625f,1.0f}, - {0.0f,0.65625f,1.0f}, - {0.0f,0.6875f,1.0f}, - {0.0f,0.71875f,1.0f}, - {0.0f,0.75f,1.0f}, - {0.0f,0.78125f,1.0f}, - {0.0f,0.8125f,1.0f}, - {0.0f,0.84375f,1.0f}, - {0.0f,0.875f,1.0f}, - {0.0f,0.90625f,1.0f}, - {0.0f,0.9375f,1.0f}, - {0.0f,0.96875f,1.0f}, - {0.0f,1.0f,1.0f}, - {0.03125f,1.0f,0.96875f}, - {0.0625f,1.0f,0.9375f}, - {0.09375f,1.0f,0.90625f}, - {0.125f,1.0f,0.875f}, - {0.15625f,1.0f,0.84375f}, - {0.1875f,1.0f,0.8125f}, - {0.21875f,1.0f,0.78125f}, - {0.25f,1.0f,0.75f}, - {0.28125f,1.0f,0.71875f}, - {0.3125f,1.0f,0.6875f}, - {0.34375f,1.0f,0.65625f}, - {0.375f,1.0f,0.625f}, - {0.40625f,1.0f,0.59375f}, - {0.4375f,1.0f,0.5625f}, - {0.46875f,1.0f,0.53125f}, - {0.5f,1.0f,0.5f}, - {0.53125f,1.0f,0.46875f}, - {0.5625f,1.0f,0.4375f}, - {0.59375f,1.0f,0.40625f}, - {0.625f,1.0f,0.375f}, - {0.65625f,1.0f,0.34375f}, - {0.6875f,1.0f,0.3125f}, - {0.71875f,1.0f,0.28125f}, - {0.75f,1.0f,0.25f}, - {0.78125f,1.0f,0.21875f}, - {0.8125f,1.0f,0.1875f}, - {0.84375f,1.0f,0.15625f}, - {0.875f,1.0f,0.125f}, - {0.90625f,1.0f,0.09375f}, - {0.9375f,1.0f,0.0625f}, - {0.96875f,1.0f,0.03125f}, - {1.0f,1.0f,0.0f}, - {1.0f,0.96875f,0.0f}, - {1.0f,0.9375f,0.0f}, - {1.0f,0.90625f,0.0f}, - {1.0f,0.875f,0.0f}, - {1.0f,0.84375f,0.0f}, - {1.0f,0.8125f,0.0f}, - {1.0f,0.78125f,0.0f}, - {1.0f,0.75f,0.0f}, - {1.0f,0.71875f,0.0f}, - {1.0f,0.6875f,0.0f}, - {1.0f,0.65625f,0.0f}, - {1.0f,0.625f,0.0f}, - {1.0f,0.59375f,0.0f}, - {1.0f,0.5625f,0.0f}, - {1.0f,0.53125f,0.0f}, - {1.0f,0.5f,0.0f}, - {1.0f,0.46875f,0.0f}, - {1.0f,0.4375f,0.0f}, - {1.0f,0.40625f,0.0f}, - {1.0f,0.375f,0.0f}, - {1.0f,0.34375f,0.0f}, - {1.0f,0.3125f,0.0f}, - {1.0f,0.28125f,0.0f}, - {1.0f,0.25f,0.0f}, - {1.0f,0.21875f,0.0f}, - {1.0f,0.1875f,0.0f}, - {1.0f,0.15625f,0.0f}, - {1.0f,0.125f,0.0f}, - {1.0f,0.09375f,0.0f}, - {1.0f,0.0625f,0.0f}, - {1.0f,0.03125f,0.0f}, - {1.0f,0.0f,0.0f}, - {0.96875f,0.0f,0.0f}, - {0.9375f,0.0f,0.0f}, - {0.90625f,0.0f,0.0f}, - {0.875f,0.0f,0.0f}, - {0.84375f,0.0f,0.0f}, - {0.8125f,0.0f,0.0f}, - {0.78125f,0.0f,0.0f}, - {0.75f,0.0f,0.0f}, - {0.71875f,0.0f,0.0f}, - {0.6875f,0.0f,0.0f}, - {0.65625f,0.0f,0.0f}, - {0.625f,0.0f,0.0f}, - {0.59375f,0.0f,0.0f}, - {0.5625f,0.0f,0.0f}, - {0.53125f,0.0f,0.0f}, - {0.5f,0.0f,0.0f} -}; +float colormapAutumn[128][3] = { + {1.0f, 0.f, 0.f}, {1.0f, 0.007874f, 0.f}, {1.0f, 0.015748f, 0.f}, + {1.0f, 0.023622f, 0.f}, {1.0f, 0.031496f, 0.f}, {1.0f, 0.03937f, 0.f}, + {1.0f, 0.047244f, 0.f}, {1.0f, 0.055118f, 0.f}, {1.0f, 0.062992f, 0.f}, + {1.0f, 0.070866f, 0.f}, {1.0f, 0.07874f, 0.f}, {1.0f, 0.086614f, 0.f}, + {1.0f, 0.094488f, 0.f}, {1.0f, 0.10236f, 0.f}, {1.0f, 0.11024f, 0.f}, + {1.0f, 0.11811f, 0.f}, {1.0f, 0.12598f, 0.f}, {1.0f, 0.13386f, 0.f}, + {1.0f, 0.14173f, 0.f}, {1.0f, 0.14961f, 0.f}, {1.0f, 0.15748f, 0.f}, + {1.0f, 0.16535f, 0.f}, {1.0f, 0.17323f, 0.f}, {1.0f, 0.1811f, 0.f}, + {1.0f, 0.18898f, 0.f}, {1.0f, 0.19685f, 0.f}, {1.0f, 0.20472f, 0.f}, + {1.0f, 0.2126f, 0.f}, {1.0f, 0.22047f, 0.f}, {1.0f, 0.22835f, 0.f}, + {1.0f, 0.23622f, 0.f}, {1.0f, 0.24409f, 0.f}, {1.0f, 0.25197f, 0.f}, + {1.0f, 0.25984f, 0.f}, {1.0f, 0.26772f, 0.f}, {1.0f, 0.27559f, 0.f}, + {1.0f, 0.28346f, 0.f}, {1.0f, 0.29134f, 0.f}, {1.0f, 0.29921f, 0.f}, + {1.0f, 0.30709f, 0.f}, {1.0f, 0.31496f, 0.f}, {1.0f, 0.32283f, 0.f}, + {1.0f, 0.33071f, 0.f}, {1.0f, 0.33858f, 0.f}, {1.0f, 0.34646f, 0.f}, + {1.0f, 0.35433f, 0.f}, {1.0f, 0.3622f, 0.f}, {1.0f, 0.37008f, 0.f}, + {1.0f, 0.37795f, 0.f}, {1.0f, 0.38583f, 0.f}, {1.0f, 0.3937f, 0.f}, + {1.0f, 0.40157f, 0.f}, {1.0f, 0.40945f, 0.f}, {1.0f, 0.41732f, 0.f}, + {1.0f, 0.4252f, 0.f}, {1.0f, 0.43307f, 0.f}, {1.0f, 0.44094f, 0.f}, + {1.0f, 0.44882f, 0.f}, {1.0f, 0.45669f, 0.f}, {1.0f, 0.46457f, 0.f}, + {1.0f, 0.47244f, 0.f}, {1.0f, 0.48031f, 0.f}, {1.0f, 0.48819f, 0.f}, + {1.0f, 0.49606f, 0.f}, {1.0f, 0.50394f, 0.f}, {1.0f, 0.51181f, 0.f}, + {1.0f, 0.51969f, 0.f}, {1.0f, 0.52756f, 0.f}, {1.0f, 0.53543f, 0.f}, + {1.0f, 0.54331f, 0.f}, {1.0f, 0.55118f, 0.f}, {1.0f, 0.55906f, 0.f}, + {1.0f, 0.56693f, 0.f}, {1.0f, 0.5748f, 0.f}, {1.0f, 0.58268f, 0.f}, + {1.0f, 0.59055f, 0.f}, {1.0f, 0.59843f, 0.f}, {1.0f, 0.6063f, 0.f}, + {1.0f, 0.61417f, 0.f}, {1.0f, 0.62205f, 0.f}, {1.0f, 0.62992f, 0.f}, + {1.0f, 0.6378f, 0.f}, {1.0f, 0.64567f, 0.f}, {1.0f, 0.65354f, 0.f}, + {1.0f, 0.66142f, 0.f}, {1.0f, 0.66929f, 0.f}, {1.0f, 0.67717f, 0.f}, + {1.0f, 0.68504f, 0.f}, {1.0f, 0.69291f, 0.f}, {1.0f, 0.70079f, 0.f}, + {1.0f, 0.70866f, 0.f}, {1.0f, 0.71654f, 0.f}, {1.0f, 0.72441f, 0.f}, + {1.0f, 0.73228f, 0.f}, {1.0f, 0.74016f, 0.f}, {1.0f, 0.74803f, 0.f}, + {1.0f, 0.75591f, 0.f}, {1.0f, 0.76378f, 0.f}, {1.0f, 0.77165f, 0.f}, + {1.0f, 0.77953f, 0.f}, {1.0f, 0.7874f, 0.f}, {1.0f, 0.79528f, 0.f}, + {1.0f, 0.80315f, 0.f}, {1.0f, 0.81102f, 0.f}, {1.0f, 0.8189f, 0.f}, + {1.0f, 0.82677f, 0.f}, {1.0f, 0.83465f, 0.f}, {1.0f, 0.84252f, 0.f}, + {1.0f, 0.85039f, 0.f}, {1.0f, 0.85827f, 0.f}, {1.0f, 0.86614f, 0.f}, + {1.0f, 0.87402f, 0.f}, {1.0f, 0.88189f, 0.f}, {1.0f, 0.88976f, 0.f}, + {1.0f, 0.89764f, 0.f}, {1.0f, 0.90551f, 0.f}, {1.0f, 0.91339f, 0.f}, + {1.0f, 0.92126f, 0.f}, {1.0f, 0.92913f, 0.f}, {1.0f, 0.93701f, 0.f}, + {1.0f, 0.94488f, 0.f}, {1.0f, 0.95276f, 0.f}, {1.0f, 0.96063f, 0.f}, + {1.0f, 0.9685f, 0.f}, {1.0f, 0.97638f, 0.f}, {1.0f, 0.98425f, 0.f}, + {1.0f, 0.99213f, 0.f}, {1.0f, 1.0f, 0.0f}}; + +float colormapJet[128][3] = { + {0.0f, 0.0f, 0.53125f}, {0.0f, 0.0f, 0.5625f}, + {0.0f, 0.0f, 0.59375f}, {0.0f, 0.0f, 0.625f}, + {0.0f, 0.0f, 0.65625f}, {0.0f, 0.0f, 0.6875f}, + {0.0f, 0.0f, 0.71875f}, {0.0f, 0.0f, 0.75f}, + {0.0f, 0.0f, 0.78125f}, {0.0f, 0.0f, 0.8125f}, + {0.0f, 0.0f, 0.84375f}, {0.0f, 0.0f, 0.875f}, + {0.0f, 0.0f, 0.90625f}, {0.0f, 0.0f, 0.9375f}, + {0.0f, 0.0f, 0.96875f}, {0.0f, 0.0f, 1.0f}, + {0.0f, 0.03125f, 1.0f}, {0.0f, 0.0625f, 1.0f}, + {0.0f, 0.09375f, 1.0f}, {0.0f, 0.125f, 1.0f}, + {0.0f, 0.15625f, 1.0f}, {0.0f, 0.1875f, 1.0f}, + {0.0f, 0.21875f, 1.0f}, {0.0f, 0.25f, 1.0f}, + {0.0f, 0.28125f, 1.0f}, {0.0f, 0.3125f, 1.0f}, + {0.0f, 0.34375f, 1.0f}, {0.0f, 0.375f, 1.0f}, + {0.0f, 0.40625f, 1.0f}, {0.0f, 0.4375f, 1.0f}, + {0.0f, 0.46875f, 1.0f}, {0.0f, 0.5f, 1.0f}, + {0.0f, 0.53125f, 1.0f}, {0.0f, 0.5625f, 1.0f}, + {0.0f, 0.59375f, 1.0f}, {0.0f, 0.625f, 1.0f}, + {0.0f, 0.65625f, 1.0f}, {0.0f, 0.6875f, 1.0f}, + {0.0f, 0.71875f, 1.0f}, {0.0f, 0.75f, 1.0f}, + {0.0f, 0.78125f, 1.0f}, {0.0f, 0.8125f, 1.0f}, + {0.0f, 0.84375f, 1.0f}, {0.0f, 0.875f, 1.0f}, + {0.0f, 0.90625f, 1.0f}, {0.0f, 0.9375f, 1.0f}, + {0.0f, 0.96875f, 1.0f}, {0.0f, 1.0f, 1.0f}, + {0.03125f, 1.0f, 0.96875f}, {0.0625f, 1.0f, 0.9375f}, + {0.09375f, 1.0f, 0.90625f}, {0.125f, 1.0f, 0.875f}, + {0.15625f, 1.0f, 0.84375f}, {0.1875f, 1.0f, 0.8125f}, + {0.21875f, 1.0f, 0.78125f}, {0.25f, 1.0f, 0.75f}, + {0.28125f, 1.0f, 0.71875f}, {0.3125f, 1.0f, 0.6875f}, + {0.34375f, 1.0f, 0.65625f}, {0.375f, 1.0f, 0.625f}, + {0.40625f, 1.0f, 0.59375f}, {0.4375f, 1.0f, 0.5625f}, + {0.46875f, 1.0f, 0.53125f}, {0.5f, 1.0f, 0.5f}, + {0.53125f, 1.0f, 0.46875f}, {0.5625f, 1.0f, 0.4375f}, + {0.59375f, 1.0f, 0.40625f}, {0.625f, 1.0f, 0.375f}, + {0.65625f, 1.0f, 0.34375f}, {0.6875f, 1.0f, 0.3125f}, + {0.71875f, 1.0f, 0.28125f}, {0.75f, 1.0f, 0.25f}, + {0.78125f, 1.0f, 0.21875f}, {0.8125f, 1.0f, 0.1875f}, + {0.84375f, 1.0f, 0.15625f}, {0.875f, 1.0f, 0.125f}, + {0.90625f, 1.0f, 0.09375f}, {0.9375f, 1.0f, 0.0625f}, + {0.96875f, 1.0f, 0.03125f}, {1.0f, 1.0f, 0.0f}, + {1.0f, 0.96875f, 0.0f}, {1.0f, 0.9375f, 0.0f}, + {1.0f, 0.90625f, 0.0f}, {1.0f, 0.875f, 0.0f}, + {1.0f, 0.84375f, 0.0f}, {1.0f, 0.8125f, 0.0f}, + {1.0f, 0.78125f, 0.0f}, {1.0f, 0.75f, 0.0f}, + {1.0f, 0.71875f, 0.0f}, {1.0f, 0.6875f, 0.0f}, + {1.0f, 0.65625f, 0.0f}, {1.0f, 0.625f, 0.0f}, + {1.0f, 0.59375f, 0.0f}, {1.0f, 0.5625f, 0.0f}, + {1.0f, 0.53125f, 0.0f}, {1.0f, 0.5f, 0.0f}, + {1.0f, 0.46875f, 0.0f}, {1.0f, 0.4375f, 0.0f}, + {1.0f, 0.40625f, 0.0f}, {1.0f, 0.375f, 0.0f}, + {1.0f, 0.34375f, 0.0f}, {1.0f, 0.3125f, 0.0f}, + {1.0f, 0.28125f, 0.0f}, {1.0f, 0.25f, 0.0f}, + {1.0f, 0.21875f, 0.0f}, {1.0f, 0.1875f, 0.0f}, + {1.0f, 0.15625f, 0.0f}, {1.0f, 0.125f, 0.0f}, + {1.0f, 0.09375f, 0.0f}, {1.0f, 0.0625f, 0.0f}, + {1.0f, 0.03125f, 0.0f}, {1.0f, 0.0f, 0.0f}, + {0.96875f, 0.0f, 0.0f}, {0.9375f, 0.0f, 0.0f}, + {0.90625f, 0.0f, 0.0f}, {0.875f, 0.0f, 0.0f}, + {0.84375f, 0.0f, 0.0f}, {0.8125f, 0.0f, 0.0f}, + {0.78125f, 0.0f, 0.0f}, {0.75f, 0.0f, 0.0f}, + {0.71875f, 0.0f, 0.0f}, {0.6875f, 0.0f, 0.0f}, + {0.65625f, 0.0f, 0.0f}, {0.625f, 0.0f, 0.0f}, + {0.59375f, 0.0f, 0.0f}, {0.5625f, 0.0f, 0.0f}, + {0.53125f, 0.0f, 0.0f}, {0.5f, 0.0f, 0.0f}}; void colorDepthImage(cv::Mat& imgDepth, cv::Mat& imgColoredDepth, - float minRange, float maxRange) -{ - imgColoredDepth = cv::Mat::zeros(imgDepth.size(), CV_8UC3); - - for (int i = 0; i < imgColoredDepth.rows; ++i) - { - const float* depth = imgDepth.ptr(i); - unsigned char* pixel = imgColoredDepth.ptr(i); - for (int j = 0; j < imgColoredDepth.cols; ++j) - { - if (depth[j] != 0) - { - int idx = fminf(depth[j] - minRange, maxRange - minRange) / (maxRange - minRange) * 127.0f; - idx = 127 - idx; - - pixel[0] = colormapJet[idx][2] * 255.0f; - pixel[1] = colormapJet[idx][1] * 255.0f; - pixel[2] = colormapJet[idx][0] * 255.0f; - } - - pixel += 3; - } - } + float minRange, float maxRange) { + imgColoredDepth = cv::Mat::zeros(imgDepth.size(), CV_8UC3); + + for (int i = 0; i < imgColoredDepth.rows; ++i) { + const float* depth = imgDepth.ptr(i); + unsigned char* pixel = imgColoredDepth.ptr(i); + for (int j = 0; j < imgColoredDepth.cols; ++j) { + if (depth[j] != 0) { + int idx = fminf(depth[j] - minRange, maxRange - minRange) / + (maxRange - minRange) * 127.0f; + idx = 127 - idx; + + pixel[0] = colormapJet[idx][2] * 255.0f; + pixel[1] = colormapJet[idx][1] * 255.0f; + pixel[2] = colormapJet[idx][0] * 255.0f; + } + + pixel += 3; + } + } } -bool colormap(const std::string& name, unsigned char idx, - float& r, float& g, float& b) -{ - if (name.compare("jet") == 0) - { - float* color = colormapJet[idx]; +bool colormap(const std::string& name, unsigned char idx, float& r, float& g, + float& b) { + if (name.compare("jet") == 0) { + float* color = colormapJet[idx]; - r = color[0]; - g = color[1]; - b = color[2]; + r = color[0]; + g = color[1]; + b = color[2]; - return true; - } - else if (name.compare("autumn") == 0) - { - float* color = colormapAutumn[idx]; + return true; + } else if (name.compare("autumn") == 0) { + float* color = colormapAutumn[idx]; - r = color[0]; - g = color[1]; - b = color[2]; + r = color[0]; + g = color[1]; + b = color[2]; - return true; - } + return true; + } - return false; + return false; } -std::vector bresLine(int x0, int y0, int x1, int y1) -{ - // Bresenham's line algorithm - // Find cells intersected by line between (x0,y0) and (x1,y1) +std::vector bresLine(int x0, int y0, int x1, int y1) { + // Bresenham's line algorithm + // Find cells intersected by line between (x0,y0) and (x1,y1) - std::vector cells; + std::vector cells; - int dx = std::abs(x1 - x0); - int dy = std::abs(y1 - y0); + int dx = std::abs(x1 - x0); + int dy = std::abs(y1 - y0); - int sx = (x0 < x1) ? 1 : -1; - int sy = (y0 < y1) ? 1 : -1; + int sx = (x0 < x1) ? 1 : -1; + int sy = (y0 < y1) ? 1 : -1; - int err = dx - dy; + int err = dx - dy; - while (1) - { - cells.push_back(cv::Point2i(x0, y0)); + while (1) { + cells.push_back(cv::Point2i(x0, y0)); - if (x0 == x1 && y0 == y1) - { - break; - } + if (x0 == x1 && y0 == y1) { + break; + } - int e2 = 2 * err; - if (e2 > -dy) - { - err -= dy; - x0 += sx; - } - if (e2 < dx) - { - err += dx; - y0 += sy; - } - } + int e2 = 2 * err; + if (e2 > -dy) { + err -= dy; + x0 += sx; + } + if (e2 < dx) { + err += dx; + y0 += sy; + } + } - return cells; + return cells; } -std::vector bresCircle(int x0, int y0, int r) -{ - // Bresenham's circle algorithm - // Find cells intersected by circle with center (x0,y0) and radius r - - std::vector< std::vector > mask(2 * r + 1); - - for (int i = 0; i < 2 * r + 1; ++i) - { - mask[i].resize(2 * r + 1); - for (int j = 0; j < 2 * r + 1; ++j) - { - mask[i][j] = false; - } - } - - int f = 1 - r; - int ddF_x = 1; - int ddF_y = -2 * r; - int x = 0; - int y = r; - - std::vector line; - - line = bresLine(x0, y0 - r, x0, y0 + r); - for (std::vector::iterator it = line.begin(); it != line.end(); ++it) - { - mask[it->x - x0 + r][it->y - y0 + r] = true; - } - - line = bresLine(x0 - r, y0, x0 + r, y0); - for (std::vector::iterator it = line.begin(); it != line.end(); ++it) - { - mask[it->x - x0 + r][it->y - y0 + r] = true; - } - - while (x < y) - { - if (f >= 0) - { - y--; - ddF_y += 2; - f += ddF_y; - } - - x++; - ddF_x += 2; - f += ddF_x; - - line = bresLine(x0 - x, y0 + y, x0 + x, y0 + y); - for (std::vector::iterator it = line.begin(); it != line.end(); ++it) - { - mask[it->x - x0 + r][it->y - y0 + r] = true; - } - - line = bresLine(x0 - x, y0 - y, x0 + x, y0 - y); - for (std::vector::iterator it = line.begin(); it != line.end(); ++it) - { - mask[it->x - x0 + r][it->y - y0 + r] = true; - } - - line = bresLine(x0 - y, y0 + x, x0 + y, y0 + x); - for (std::vector::iterator it = line.begin(); it != line.end(); ++it) - { - mask[it->x - x0 + r][it->y - y0 + r] = true; - } - - line = bresLine(x0 - y, y0 - x, x0 + y, y0 - x); - for (std::vector::iterator it = line.begin(); it != line.end(); ++it) - { - mask[it->x - x0 + r][it->y - y0 + r] = true; - } - } - - std::vector cells; - for (int i = 0; i < 2 * r + 1; ++i) - { - for (int j = 0; j < 2 * r + 1; ++j) - { - if (mask[i][j]) - { - cells.push_back(cv::Point2i(i - r + x0, j - r + y0)); - } - } - } - - return cells; -} +std::vector bresCircle(int x0, int y0, int r) { + // Bresenham's circle algorithm + // Find cells intersected by circle with center (x0,y0) and radius r -void -fitCircle(const std::vector& points, - double& centerX, double& centerY, double& radius) -{ - // D. Umbach, and K. Jones, A Few Methods for Fitting Circles to Data, - // IEEE Transactions on Instrumentation and Measurement, 2000 - // We use the modified least squares method. - double sum_x = 0.0; - double sum_y = 0.0; - double sum_xx = 0.0; - double sum_xy = 0.0; - double sum_yy = 0.0; - double sum_xxx = 0.0; - double sum_xxy = 0.0; - double sum_xyy = 0.0; - double sum_yyy = 0.0; - - int n = points.size(); - for (int i = 0; i < n; ++i) - { - double x = points.at(i).x; - double y = points.at(i).y; - - sum_x += x; - sum_y += y; - sum_xx += x * x; - sum_xy += x * y; - sum_yy += y * y; - sum_xxx += x * x * x; - sum_xxy += x * x * y; - sum_xyy += x * y * y; - sum_yyy += y * y * y; + std::vector > mask(2 * r + 1); + + for (int i = 0; i < 2 * r + 1; ++i) { + mask[i].resize(2 * r + 1); + for (int j = 0; j < 2 * r + 1; ++j) { + mask[i][j] = false; } + } - double A = n * sum_xx - square(sum_x); - double B = n * sum_xy - sum_x * sum_y; - double C = n * sum_yy - square(sum_y); - double D = 0.5 * (n * sum_xyy - sum_x * sum_yy + n * sum_xxx - sum_x * sum_xx); - double E = 0.5 * (n * sum_xxy - sum_y * sum_xx + n * sum_yyy - sum_y * sum_yy); + int f = 1 - r; + int ddF_x = 1; + int ddF_y = -2 * r; + int x = 0; + int y = r; - centerX = (D * C - B * E) / (A * C - square(B)); - centerY = (A * E - B * D) / (A * C - square(B)); + std::vector line; - double sum_r = 0.0; - for (int i = 0; i < n; ++i) - { - double x = points.at(i).x; - double y = points.at(i).y; + line = bresLine(x0, y0 - r, x0, y0 + r); + for (std::vector::iterator it = line.begin(); it != line.end(); + ++it) { + mask[it->x - x0 + r][it->y - y0 + r] = true; + } + + line = bresLine(x0 - r, y0, x0 + r, y0); + for (std::vector::iterator it = line.begin(); it != line.end(); + ++it) { + mask[it->x - x0 + r][it->y - y0 + r] = true; + } - sum_r += hypot(x - centerX, y - centerY); + while (x < y) { + if (f >= 0) { + y--; + ddF_y += 2; + f += ddF_y; } - radius = sum_r / n; -} + x++; + ddF_x += 2; + f += ddF_x; -std::vector -intersectCircles(double x1, double y1, double r1, - double x2, double y2, double r2) -{ - std::vector ipts; - - double d = hypot(x1 - x2, y1 - y2); - if (d > r1 + r2) - { - // circles are separate - return ipts; + line = bresLine(x0 - x, y0 + y, x0 + x, y0 + y); + for (std::vector::iterator it = line.begin(); it != line.end(); + ++it) { + mask[it->x - x0 + r][it->y - y0 + r] = true; } - if (d < fabs(r1 - r2)) - { - // one circle is contained within the other - return ipts; + + line = bresLine(x0 - x, y0 - y, x0 + x, y0 - y); + for (std::vector::iterator it = line.begin(); it != line.end(); + ++it) { + mask[it->x - x0 + r][it->y - y0 + r] = true; } - double a = (square(r1) - square(r2) + square(d)) / (2.0 * d); - double h = sqrt(square(r1) - square(a)); + line = bresLine(x0 - y, y0 + x, x0 + y, y0 + x); + for (std::vector::iterator it = line.begin(); it != line.end(); + ++it) { + mask[it->x - x0 + r][it->y - y0 + r] = true; + } - double x3 = x1 + a * (x2 - x1) / d; - double y3 = y1 + a * (y2 - y1) / d; + line = bresLine(x0 - y, y0 - x, x0 + y, y0 - x); + for (std::vector::iterator it = line.begin(); it != line.end(); + ++it) { + mask[it->x - x0 + r][it->y - y0 + r] = true; + } + } - if (h < 1e-10) - { - // two circles touch at one point - ipts.push_back(cv::Point2d(x3, y3)); - return ipts; + std::vector cells; + for (int i = 0; i < 2 * r + 1; ++i) { + for (int j = 0; j < 2 * r + 1; ++j) { + if (mask[i][j]) { + cells.push_back(cv::Point2i(i - r + x0, j - r + y0)); + } } + } - ipts.push_back(cv::Point2d(x3 + h * (y2 - y1) / d, - y3 - h * (x2 - x1) / d)); - ipts.push_back(cv::Point2d(x3 - h * (y2 - y1) / d, - y3 + h * (x2 - x1) / d)); + return cells; +} + +void fitCircle(const std::vector& points, double& centerX, + double& centerY, double& radius) { + // D. Umbach, and K. Jones, A Few Methods for Fitting Circles to Data, + // IEEE Transactions on Instrumentation and Measurement, 2000 + // We use the modified least squares method. + double sum_x = 0.0; + double sum_y = 0.0; + double sum_xx = 0.0; + double sum_xy = 0.0; + double sum_yy = 0.0; + double sum_xxx = 0.0; + double sum_xxy = 0.0; + double sum_xyy = 0.0; + double sum_yyy = 0.0; + + int n = points.size(); + for (int i = 0; i < n; ++i) { + double x = points.at(i).x; + double y = points.at(i).y; + + sum_x += x; + sum_y += y; + sum_xx += x * x; + sum_xy += x * y; + sum_yy += y * y; + sum_xxx += x * x * x; + sum_xxy += x * x * y; + sum_xyy += x * y * y; + sum_yyy += y * y * y; + } + + double A = n * sum_xx - square(sum_x); + double B = n * sum_xy - sum_x * sum_y; + double C = n * sum_yy - square(sum_y); + double D = + 0.5 * (n * sum_xyy - sum_x * sum_yy + n * sum_xxx - sum_x * sum_xx); + double E = + 0.5 * (n * sum_xxy - sum_y * sum_xx + n * sum_yyy - sum_y * sum_yy); + + centerX = (D * C - B * E) / (A * C - square(B)); + centerY = (A * E - B * D) / (A * C - square(B)); + + double sum_r = 0.0; + for (int i = 0; i < n; ++i) { + double x = points.at(i).x; + double y = points.at(i).y; + + sum_r += hypot(x - centerX, y - centerY); + } + + radius = sum_r / n; +} + +std::vector intersectCircles(double x1, double y1, double r1, + double x2, double y2, double r2) { + std::vector ipts; + + double d = hypot(x1 - x2, y1 - y2); + if (d > r1 + r2) { + // circles are separate + return ipts; + } + if (d < fabs(r1 - r2)) { + // one circle is contained within the other + return ipts; + } + + double a = (square(r1) - square(r2) + square(d)) / (2.0 * d); + double h = sqrt(square(r1) - square(a)); + + double x3 = x1 + a * (x2 - x1) / d; + double y3 = y1 + a * (y2 - y1) / d; + + if (h < 1e-10) { + // two circles touch at one point + ipts.push_back(cv::Point2d(x3, y3)); return ipts; + } + + ipts.push_back(cv::Point2d(x3 + h * (y2 - y1) / d, y3 - h * (x2 - x1) / d)); + ipts.push_back(cv::Point2d(x3 - h * (y2 - y1) / d, y3 + h * (x2 - x1) / d)); + return ipts; } -char -UTMLetterDesignator(double latitude) -{ - // This routine determines the correct UTM letter designator for the given latitude - // returns 'Z' if latitude is outside the UTM limits of 84N to 80S - // Written by Chuck Gantz- chuck.gantz@globalstar.com - char letterDesignator; - - if ((84.0 >= latitude) && (latitude >= 72.0)) letterDesignator = 'X'; - else if ((72.0 > latitude) && (latitude >= 64.0)) letterDesignator = 'W'; - else if ((64.0 > latitude) && (latitude >= 56.0)) letterDesignator = 'V'; - else if ((56.0 > latitude) && (latitude >= 48.0)) letterDesignator = 'U'; - else if ((48.0 > latitude) && (latitude >= 40.0)) letterDesignator = 'T'; - else if ((40.0 > latitude) && (latitude >= 32.0)) letterDesignator = 'S'; - else if ((32.0 > latitude) && (latitude >= 24.0)) letterDesignator = 'R'; - else if ((24.0 > latitude) && (latitude >= 16.0)) letterDesignator = 'Q'; - else if ((16.0 > latitude) && (latitude >= 8.0)) letterDesignator = 'P'; - else if (( 8.0 > latitude) && (latitude >= 0.0)) letterDesignator = 'N'; - else if (( 0.0 > latitude) && (latitude >= -8.0)) letterDesignator = 'M'; - else if ((-8.0 > latitude) && (latitude >= -16.0)) letterDesignator = 'L'; - else if ((-16.0 > latitude) && (latitude >= -24.0)) letterDesignator = 'K'; - else if ((-24.0 > latitude) && (latitude >= -32.0)) letterDesignator = 'J'; - else if ((-32.0 > latitude) && (latitude >= -40.0)) letterDesignator = 'H'; - else if ((-40.0 > latitude) && (latitude >= -48.0)) letterDesignator = 'G'; - else if ((-48.0 > latitude) && (latitude >= -56.0)) letterDesignator = 'F'; - else if ((-56.0 > latitude) && (latitude >= -64.0)) letterDesignator = 'E'; - else if ((-64.0 > latitude) && (latitude >= -72.0)) letterDesignator = 'D'; - else if ((-72.0 > latitude) && (latitude >= -80.0)) letterDesignator = 'C'; - else letterDesignator = 'Z'; //This is here as an error flag to show that the Latitude is outside the UTM limits - - return letterDesignator; +char UTMLetterDesignator(double latitude) { + // This routine determines the correct UTM letter designator for the given + // latitude returns 'Z' if latitude is outside the UTM limits of 84N to 80S + // Written by Chuck Gantz- chuck.gantz@globalstar.com + char letterDesignator; + + if ((84.0 >= latitude) && (latitude >= 72.0)) + letterDesignator = 'X'; + else if ((72.0 > latitude) && (latitude >= 64.0)) + letterDesignator = 'W'; + else if ((64.0 > latitude) && (latitude >= 56.0)) + letterDesignator = 'V'; + else if ((56.0 > latitude) && (latitude >= 48.0)) + letterDesignator = 'U'; + else if ((48.0 > latitude) && (latitude >= 40.0)) + letterDesignator = 'T'; + else if ((40.0 > latitude) && (latitude >= 32.0)) + letterDesignator = 'S'; + else if ((32.0 > latitude) && (latitude >= 24.0)) + letterDesignator = 'R'; + else if ((24.0 > latitude) && (latitude >= 16.0)) + letterDesignator = 'Q'; + else if ((16.0 > latitude) && (latitude >= 8.0)) + letterDesignator = 'P'; + else if ((8.0 > latitude) && (latitude >= 0.0)) + letterDesignator = 'N'; + else if ((0.0 > latitude) && (latitude >= -8.0)) + letterDesignator = 'M'; + else if ((-8.0 > latitude) && (latitude >= -16.0)) + letterDesignator = 'L'; + else if ((-16.0 > latitude) && (latitude >= -24.0)) + letterDesignator = 'K'; + else if ((-24.0 > latitude) && (latitude >= -32.0)) + letterDesignator = 'J'; + else if ((-32.0 > latitude) && (latitude >= -40.0)) + letterDesignator = 'H'; + else if ((-40.0 > latitude) && (latitude >= -48.0)) + letterDesignator = 'G'; + else if ((-48.0 > latitude) && (latitude >= -56.0)) + letterDesignator = 'F'; + else if ((-56.0 > latitude) && (latitude >= -64.0)) + letterDesignator = 'E'; + else if ((-64.0 > latitude) && (latitude >= -72.0)) + letterDesignator = 'D'; + else if ((-72.0 > latitude) && (latitude >= -80.0)) + letterDesignator = 'C'; + else + letterDesignator = 'Z'; // This is here as an error flag to show that the + // Latitude is outside the UTM limits + + return letterDesignator; } -void -LLtoUTM(double latitude, double longitude, - double& utmNorthing, double& utmEasting, std::string& utmZone) -{ - // converts lat/long to UTM coords. Equations from USGS Bulletin 1532 - // East Longitudes are positive, West longitudes are negative. - // North latitudes are positive, South latitudes are negative - // Lat and Long are in decimal degrees - // Written by Chuck Gantz- chuck.gantz@globalstar.com +void LLtoUTM(double latitude, double longitude, double& utmNorthing, + double& utmEasting, std::string& utmZone) { + // converts lat/long to UTM coords. Equations from USGS Bulletin 1532 + // East Longitudes are positive, West longitudes are negative. + // North latitudes are positive, South latitudes are negative + // Lat and Long are in decimal degrees + // Written by Chuck Gantz- chuck.gantz@globalstar.com - double k0 = 0.9996; + double k0 = 0.9996; - double LongOrigin; - double eccPrimeSquared; - double N, T, C, A, M; + double LongOrigin; + double eccPrimeSquared; + double N, T, C, A, M; - double LatRad = latitude * M_PI / 180.0; - double LongRad = longitude * M_PI / 180.0; - double LongOriginRad; + double LatRad = latitude * M_PI / 180.0; + double LongRad = longitude * M_PI / 180.0; + double LongOriginRad; - int ZoneNumber = static_cast((longitude + 180.0) / 6.0) + 1; + int ZoneNumber = static_cast((longitude + 180.0) / 6.0) + 1; - if (latitude >= 56.0 && latitude < 64.0 && - longitude >= 3.0 && longitude < 12.0) { - ZoneNumber = 32; - } + if (latitude >= 56.0 && latitude < 64.0 && longitude >= 3.0 && + longitude < 12.0) { + ZoneNumber = 32; + } - // Special zones for Svalbard - if (latitude >= 72.0 && latitude < 84.0) { - if ( longitude >= 0.0 && longitude < 9.0) ZoneNumber = 31; - else if (longitude >= 9.0 && longitude < 21.0) ZoneNumber = 33; - else if (longitude >= 21.0 && longitude < 33.0) ZoneNumber = 35; - else if (longitude >= 33.0 && longitude < 42.0) ZoneNumber = 37; - } - LongOrigin = static_cast((ZoneNumber - 1) * 6 - 180 + 3); //+3 puts origin in middle of zone - LongOriginRad = LongOrigin * M_PI / 180.0; - - // compute the UTM Zone from the latitude and longitude - std::ostringstream oss; - oss << ZoneNumber << UTMLetterDesignator(latitude); - utmZone = oss.str(); - - eccPrimeSquared = WGS84_ECCSQ / (1.0 - WGS84_ECCSQ); - - N = WGS84_A / sqrt(1.0 - WGS84_ECCSQ * sin(LatRad) * sin(LatRad)); - T = tan(LatRad) * tan(LatRad); - C = eccPrimeSquared * cos(LatRad) * cos(LatRad); - A = cos(LatRad) * (LongRad - LongOriginRad); - - M = WGS84_A * ((1.0 - WGS84_ECCSQ / 4.0 - - 3.0 * WGS84_ECCSQ * WGS84_ECCSQ / 64.0 - - 5.0 * WGS84_ECCSQ * WGS84_ECCSQ * WGS84_ECCSQ / 256.0) - * LatRad - - (3.0 * WGS84_ECCSQ / 8.0 - + 3.0 * WGS84_ECCSQ * WGS84_ECCSQ / 32.0 - + 45.0 * WGS84_ECCSQ * WGS84_ECCSQ * WGS84_ECCSQ / 1024.0) - * sin(2.0 * LatRad) - + (15.0 * WGS84_ECCSQ * WGS84_ECCSQ / 256.0 - + 45.0 * WGS84_ECCSQ * WGS84_ECCSQ * WGS84_ECCSQ / 1024.0) - * sin(4.0 * LatRad) - - (35.0 * WGS84_ECCSQ * WGS84_ECCSQ * WGS84_ECCSQ / 3072.0) - * sin(6.0 * LatRad)); - - utmEasting = k0 * N * (A + (1.0 - T + C) * A * A * A / 6.0 - + (5.0 - 18.0 * T + T * T + 72.0 * C - - 58.0 * eccPrimeSquared) - * A * A * A * A * A / 120.0) - + 500000.0; - - utmNorthing = k0 * (M + N * tan(LatRad) * - (A * A / 2.0 + - (5.0 - T + 9.0 * C + 4.0 * C * C) * A * A * A * A / 24.0 - + (61.0 - 58.0 * T + T * T + 600.0 * C - - 330.0 * eccPrimeSquared) - * A * A * A * A * A * A / 720.0)); - if (latitude < 0.0) { - utmNorthing += 10000000.0; //10000000 meter offset for southern hemisphere - } + // Special zones for Svalbard + if (latitude >= 72.0 && latitude < 84.0) { + if (longitude >= 0.0 && longitude < 9.0) + ZoneNumber = 31; + else if (longitude >= 9.0 && longitude < 21.0) + ZoneNumber = 33; + else if (longitude >= 21.0 && longitude < 33.0) + ZoneNumber = 35; + else if (longitude >= 33.0 && longitude < 42.0) + ZoneNumber = 37; + } + LongOrigin = static_cast((ZoneNumber - 1) * 6 - 180 + + 3); //+3 puts origin in middle of zone + LongOriginRad = LongOrigin * M_PI / 180.0; + + // compute the UTM Zone from the latitude and longitude + std::ostringstream oss; + oss << ZoneNumber << UTMLetterDesignator(latitude); + utmZone = oss.str(); + + eccPrimeSquared = WGS84_ECCSQ / (1.0 - WGS84_ECCSQ); + + N = WGS84_A / sqrt(1.0 - WGS84_ECCSQ * sin(LatRad) * sin(LatRad)); + T = tan(LatRad) * tan(LatRad); + C = eccPrimeSquared * cos(LatRad) * cos(LatRad); + A = cos(LatRad) * (LongRad - LongOriginRad); + + M = WGS84_A * + ((1.0 - WGS84_ECCSQ / 4.0 - 3.0 * WGS84_ECCSQ * WGS84_ECCSQ / 64.0 - + 5.0 * WGS84_ECCSQ * WGS84_ECCSQ * WGS84_ECCSQ / 256.0) * + LatRad - + (3.0 * WGS84_ECCSQ / 8.0 + 3.0 * WGS84_ECCSQ * WGS84_ECCSQ / 32.0 + + 45.0 * WGS84_ECCSQ * WGS84_ECCSQ * WGS84_ECCSQ / 1024.0) * + sin(2.0 * LatRad) + + (15.0 * WGS84_ECCSQ * WGS84_ECCSQ / 256.0 + + 45.0 * WGS84_ECCSQ * WGS84_ECCSQ * WGS84_ECCSQ / 1024.0) * + sin(4.0 * LatRad) - + (35.0 * WGS84_ECCSQ * WGS84_ECCSQ * WGS84_ECCSQ / 3072.0) * + sin(6.0 * LatRad)); + + utmEasting = + k0 * N * + (A + (1.0 - T + C) * A * A * A / 6.0 + + (5.0 - 18.0 * T + T * T + 72.0 * C - 58.0 * eccPrimeSquared) * A * + A * A * A * A / 120.0) + + 500000.0; + + utmNorthing = + k0 * (M + N * tan(LatRad) * + (A * A / 2.0 + + (5.0 - T + 9.0 * C + 4.0 * C * C) * A * A * A * A / 24.0 + + (61.0 - 58.0 * T + T * T + 600.0 * C - + 330.0 * eccPrimeSquared) * + A * A * A * A * A * A / 720.0)); + if (latitude < 0.0) { + utmNorthing += 10000000.0; // 10000000 meter offset for southern hemisphere + } } -void -UTMtoLL(double utmNorthing, double utmEasting, const std::string& utmZone, - double& latitude, double& longitude) -{ - // converts UTM coords to lat/long. Equations from USGS Bulletin 1532 - // East Longitudes are positive, West longitudes are negative. - // North latitudes are positive, South latitudes are negative - // Lat and Long are in decimal degrees. - // Written by Chuck Gantz- chuck.gantz@globalstar.com - - double k0 = 0.9996; - double eccPrimeSquared; - double e1 = (1.0 - sqrt(1.0 - WGS84_ECCSQ)) / (1.0 + sqrt(1.0 - WGS84_ECCSQ)); - double N1, T1, C1, R1, D, M; - double LongOrigin; - double mu, phi1, phi1Rad; - double x, y; - int ZoneNumber; - char ZoneLetter; - bool NorthernHemisphere; - - x = utmEasting - 500000.0; //remove 500,000 meter offset for longitude - y = utmNorthing; - - std::istringstream iss(utmZone); - iss >> ZoneNumber >> ZoneLetter; - if ((static_cast(ZoneLetter) - static_cast('N')) >= 0) { - NorthernHemisphere = true;//point is in northern hemisphere - } else { - NorthernHemisphere = false;//point is in southern hemisphere - y -= 10000000.0;//remove 10,000,000 meter offset used for southern hemisphere - } +void UTMtoLL(double utmNorthing, double utmEasting, const std::string& utmZone, + double& latitude, double& longitude) { + // converts UTM coords to lat/long. Equations from USGS Bulletin 1532 + // East Longitudes are positive, West longitudes are negative. + // North latitudes are positive, South latitudes are negative + // Lat and Long are in decimal degrees. + // Written by Chuck Gantz- chuck.gantz@globalstar.com + + double k0 = 0.9996; + double eccPrimeSquared; + double e1 = (1.0 - sqrt(1.0 - WGS84_ECCSQ)) / (1.0 + sqrt(1.0 - WGS84_ECCSQ)); + double N1, T1, C1, R1, D, M; + double LongOrigin; + double mu, phi1, phi1Rad; + double x, y; + int ZoneNumber; + char ZoneLetter; + bool NorthernHemisphere; + + x = utmEasting - 500000.0; // remove 500,000 meter offset for longitude + y = utmNorthing; + + std::istringstream iss(utmZone); + iss >> ZoneNumber >> ZoneLetter; + if ((static_cast(ZoneLetter) - static_cast('N')) >= 0) { + NorthernHemisphere = true; // point is in northern hemisphere + } else { + NorthernHemisphere = false; // point is in southern hemisphere + y -= 10000000.0; // remove 10,000,000 meter offset used for southern + // hemisphere + } - LongOrigin = (ZoneNumber - 1.0) * 6.0 - 180.0 + 3.0; //+3 puts origin in middle of zone - - eccPrimeSquared = WGS84_ECCSQ / (1.0 - WGS84_ECCSQ); - - M = y / k0; - mu = M / (WGS84_A * (1.0 - WGS84_ECCSQ / 4.0 - - 3.0 * WGS84_ECCSQ * WGS84_ECCSQ / 64.0 - - 5.0 * WGS84_ECCSQ * WGS84_ECCSQ * WGS84_ECCSQ / 256.0)); - - phi1Rad = mu + (3.0 * e1 / 2.0 - 27.0 * e1 * e1 * e1 / 32.0) * sin(2.0 * mu) - + (21.0 * e1 * e1 / 16.0 - 55.0 * e1 * e1 * e1 * e1 / 32.0) - * sin(4.0 * mu) - + (151.0 * e1 * e1 * e1 / 96.0) * sin(6.0 * mu); - phi1 = phi1Rad / M_PI * 180.0; - - N1 = WGS84_A / sqrt(1.0 - WGS84_ECCSQ * sin(phi1Rad) * sin(phi1Rad)); - T1 = tan(phi1Rad) * tan(phi1Rad); - C1 = eccPrimeSquared * cos(phi1Rad) * cos(phi1Rad); - R1 = WGS84_A * (1.0 - WGS84_ECCSQ) / - pow(1.0 - WGS84_ECCSQ * sin(phi1Rad) * sin(phi1Rad), 1.5); - D = x / (N1 * k0); - - latitude = phi1Rad - (N1 * tan(phi1Rad) / R1) - * (D * D / 2.0 - (5.0 + 3.0 * T1 + 10.0 * C1 - 4.0 * C1 * C1 - - 9.0 * eccPrimeSquared) * D * D * D * D / 24.0 - + (61.0 + 90.0 * T1 + 298.0 * C1 + 45.0 * T1 * T1 - - 252.0 * eccPrimeSquared - 3.0 * C1 * C1) - * D * D * D * D * D * D / 720.0); - latitude *= 180.0 / M_PI; - - longitude = (D - (1.0 + 2.0 * T1 + C1) * D * D * D / 6.0 - + (5.0 - 2.0 * C1 + 28.0 * T1 - 3.0 * C1 * C1 - + 8.0 * eccPrimeSquared + 24.0 * T1 * T1) - * D * D * D * D * D / 120.0) / cos(phi1Rad); - longitude = LongOrigin + longitude / M_PI * 180.0; + LongOrigin = (ZoneNumber - 1.0) * 6.0 - 180.0 + + 3.0; //+3 puts origin in middle of zone + + eccPrimeSquared = WGS84_ECCSQ / (1.0 - WGS84_ECCSQ); + + M = y / k0; + mu = M / (WGS84_A * + (1.0 - WGS84_ECCSQ / 4.0 - 3.0 * WGS84_ECCSQ * WGS84_ECCSQ / 64.0 - + 5.0 * WGS84_ECCSQ * WGS84_ECCSQ * WGS84_ECCSQ / 256.0)); + + phi1Rad = mu + (3.0 * e1 / 2.0 - 27.0 * e1 * e1 * e1 / 32.0) * sin(2.0 * mu) + + (21.0 * e1 * e1 / 16.0 - 55.0 * e1 * e1 * e1 * e1 / 32.0) * + sin(4.0 * mu) + + (151.0 * e1 * e1 * e1 / 96.0) * sin(6.0 * mu); + phi1 = phi1Rad / M_PI * 180.0; + + N1 = WGS84_A / sqrt(1.0 - WGS84_ECCSQ * sin(phi1Rad) * sin(phi1Rad)); + T1 = tan(phi1Rad) * tan(phi1Rad); + C1 = eccPrimeSquared * cos(phi1Rad) * cos(phi1Rad); + R1 = WGS84_A * (1.0 - WGS84_ECCSQ) / + pow(1.0 - WGS84_ECCSQ * sin(phi1Rad) * sin(phi1Rad), 1.5); + D = x / (N1 * k0); + + latitude = phi1Rad - (N1 * tan(phi1Rad) / R1) * + (D * D / 2.0 - + (5.0 + 3.0 * T1 + 10.0 * C1 - 4.0 * C1 * C1 - + 9.0 * eccPrimeSquared) * + D * D * D * D / 24.0 + + (61.0 + 90.0 * T1 + 298.0 * C1 + 45.0 * T1 * T1 - + 252.0 * eccPrimeSquared - 3.0 * C1 * C1) * + D * D * D * D * D * D / 720.0); + latitude *= 180.0 / M_PI; + + longitude = (D - (1.0 + 2.0 * T1 + C1) * D * D * D / 6.0 + + (5.0 - 2.0 * C1 + 28.0 * T1 - 3.0 * C1 * C1 + + 8.0 * eccPrimeSquared + 24.0 * T1 * T1) * + D * D * D * D * D / 120.0) / + cos(phi1Rad); + longitude = LongOrigin + longitude / M_PI * 180.0; } -long int -timestampDiff(uint64_t t1, uint64_t t2) -{ - if (t2 > t1) - { - uint64_t d = t2 - t1; - - if (d > std::numeric_limits::max()) - { - return std::numeric_limits::max(); - } - else - { - return d; - } +long int timestampDiff(uint64_t t1, uint64_t t2) { + if (t2 > t1) { + uint64_t d = t2 - t1; + + if (d > std::numeric_limits::max()) { + return std::numeric_limits::max(); + } else { + return d; } - else - { - uint64_t d = t1 - t2; - - if (d > std::numeric_limits::max()) - { - return std::numeric_limits::min(); - } - else - { - return - static_cast(d); - } + } else { + uint64_t d = t1 - t2; + + if (d > std::numeric_limits::max()) { + return std::numeric_limits::min(); + } else { + return -static_cast(d); } + } } -} +} // namespace camodocal diff --git a/camera_models/src/intrinsic_calib.cc b/camera_models/src/intrinsic_calib.cc index 7590a649..867c29e1 100644 --- a/camera_models/src/intrinsic_calib.cc +++ b/camera_models/src/intrinsic_calib.cc @@ -12,273 +12,246 @@ #include "camodocal/chessboard/Chessboard.h" #include "camodocal/gpl/gpl.h" -int -main( int argc, char** argv ) -{ - cv::Size boardSize; - float squareSize; - std::string inputDir; - std::string cameraModel; - std::string cameraName; - std::string prefix; - std::string fileExtension; - bool useOpenCV; - bool viewResults; - bool verbose; - - //========= Handling Program options ========= - boost::program_options::options_description desc( "Allowed options" ); - desc.add_options( )( "help", "produce help message" )( - "width,w", - boost::program_options::value< int >( &boardSize.width )->default_value( 8 ), - "Number of inner corners on the chessboard pattern in x direction" )( - "height,h", - boost::program_options::value< int >( &boardSize.height )->default_value( 12 ), - "Number of inner corners on the chessboard pattern in y direction" )( - "size,s", - boost::program_options::value< float >( &squareSize )->default_value( 7.f ), - "Size of one square in mm" )( "input,i", - boost::program_options::value< std::string >( &inputDir )->default_value( "calibrationdata" ), - "Input directory containing chessboard images" )( - "prefix,p", - boost::program_options::value< std::string >( &prefix )->default_value( "left-" ), - "Prefix of images" )( "file-extension,e", - boost::program_options::value< std::string >( &fileExtension )->default_value( ".png" ), - "File extension of images" )( - "camera-model", - boost::program_options::value< std::string >( &cameraModel )->default_value( "mei" ), - "Camera model: kannala-brandt | mei | pinhole" )( - "camera-name", - boost::program_options::value< std::string >( &cameraName )->default_value( "camera" ), - "Name of camera" )( "opencv", - boost::program_options::bool_switch( &useOpenCV )->default_value( true ), - "Use OpenCV to detect corners" )( - "view-results", - boost::program_options::bool_switch( &viewResults )->default_value( false ), - "View results" )( "verbose,v", - boost::program_options::bool_switch( &verbose )->default_value( true ), - "Verbose output" ); - - boost::program_options::positional_options_description pdesc; - pdesc.add( "input", 1 ); - - boost::program_options::variables_map vm; - boost::program_options::store( boost::program_options::command_line_parser( argc, argv ) - .options( desc ) - .positional( pdesc ) - .run( ), - vm ); - boost::program_options::notify( vm ); - - if ( vm.count( "help" ) ) - { - std::cout << desc << std::endl; - return 1; +int main(int argc, char** argv) { + cv::Size boardSize; + float squareSize; + std::string inputDir; + std::string cameraModel; + std::string cameraName; + std::string prefix; + std::string fileExtension; + bool useOpenCV; + bool viewResults; + bool verbose; + + //========= Handling Program options ========= + boost::program_options::options_description desc("Allowed options"); + desc.add_options()("help", "produce help message")( + "width,w", + boost::program_options::value(&boardSize.width)->default_value(8), + "Number of inner corners on the chessboard pattern in x direction")( + "height,h", + boost::program_options::value(&boardSize.height)->default_value(12), + "Number of inner corners on the chessboard pattern in y direction")( + "size,s", + boost::program_options::value(&squareSize)->default_value(7.f), + "Size of one square in mm")( + "input,i", + boost::program_options::value(&inputDir)->default_value( + "calibrationdata"), + "Input directory containing chessboard images")( + "prefix,p", + boost::program_options::value(&prefix)->default_value( + "left-"), + "Prefix of images")( + "file-extension,e", + boost::program_options::value(&fileExtension) + ->default_value(".png"), + "File extension of images")( + "camera-model", + boost::program_options::value(&cameraModel) + ->default_value("mei"), + "Camera model: kannala-brandt | mei | pinhole")( + "camera-name", + boost::program_options::value(&cameraName) + ->default_value("camera"), + "Name of camera")( + "opencv", + boost::program_options::bool_switch(&useOpenCV)->default_value(true), + "Use OpenCV to detect corners")( + "view-results", + boost::program_options::bool_switch(&viewResults)->default_value(false), + "View results")( + "verbose,v", + boost::program_options::bool_switch(&verbose)->default_value(true), + "Verbose output"); + + boost::program_options::positional_options_description pdesc; + pdesc.add("input", 1); + + boost::program_options::variables_map vm; + boost::program_options::store( + boost::program_options::command_line_parser(argc, argv) + .options(desc) + .positional(pdesc) + .run(), + vm); + boost::program_options::notify(vm); + + if (vm.count("help")) { + std::cout << desc << std::endl; + return 1; + } + + if (!boost::filesystem::exists(inputDir) && + !boost::filesystem::is_directory(inputDir)) { + std::cerr << "# ERROR: Cannot find input directory " << inputDir << "." + << std::endl; + return 1; + } + + camodocal::Camera::ModelType modelType; + if (boost::iequals(cameraModel, "kannala-brandt")) { + modelType = camodocal::Camera::KANNALA_BRANDT; + } else if (boost::iequals(cameraModel, "mei")) { + modelType = camodocal::Camera::MEI; + } else if (boost::iequals(cameraModel, "pinhole")) { + modelType = camodocal::Camera::PINHOLE; + } else if (boost::iequals(cameraModel, "pinhole_full")) { + modelType = camodocal::Camera::PINHOLE_FULL; + } else if (boost::iequals(cameraModel, "scaramuzza")) { + modelType = camodocal::Camera::SCARAMUZZA; + } else { + std::cerr << "# ERROR: Unknown camera model: " << cameraModel << std::endl; + return 1; + } + + switch (modelType) { + case camodocal::Camera::KANNALA_BRANDT: + std::cout << "# INFO: Camera model: Kannala-Brandt" << std::endl; + break; + case camodocal::Camera::MEI: + std::cout << "# INFO: Camera model: Mei" << std::endl; + break; + case camodocal::Camera::PINHOLE: + std::cout << "# INFO: Camera model: Pinhole" << std::endl; + break; + case camodocal::Camera::PINHOLE_FULL: + std::cout << "# INFO: Camera model: PinholeFull" << std::endl; + break; + case camodocal::Camera::SCARAMUZZA: + std::cout << "# INFO: Camera model: Scaramuzza-Omnidirect" << std::endl; + break; + } + + // look for images in input directory + std::vector imageFilenames; + boost::filesystem::directory_iterator itr; + for (boost::filesystem::directory_iterator itr(inputDir); + itr != boost::filesystem::directory_iterator(); ++itr) { + if (!boost::filesystem::is_regular_file(itr->status())) { + continue; } - if ( !boost::filesystem::exists( inputDir ) && !boost::filesystem::is_directory( inputDir ) ) - { - std::cerr << "# ERROR: Cannot find input directory " << inputDir << "." << std::endl; - return 1; - } + std::string filename = itr->path().filename().string(); - camodocal::Camera::ModelType modelType; - if ( boost::iequals( cameraModel, "kannala-brandt" ) ) - { - modelType = camodocal::Camera::KANNALA_BRANDT; - } - else if ( boost::iequals( cameraModel, "mei" ) ) - { - modelType = camodocal::Camera::MEI; - } - else if ( boost::iequals( cameraModel, "pinhole" ) ) - { - modelType = camodocal::Camera::PINHOLE; - } - else if ( boost::iequals( cameraModel, "pinhole_full" ) ) - { - modelType = camodocal::Camera::PINHOLE_FULL; - } - else if ( boost::iequals( cameraModel, "scaramuzza" ) ) - { - modelType = camodocal::Camera::SCARAMUZZA; - } - else - { - std::cerr << "# ERROR: Unknown camera model: " << cameraModel << std::endl; - return 1; + // check if prefix matches + if (!prefix.empty()) { + if (filename.compare(0, prefix.length(), prefix) != 0) { + continue; + } } - switch ( modelType ) - { - case camodocal::Camera::KANNALA_BRANDT: - std::cout << "# INFO: Camera model: Kannala-Brandt" << std::endl; - break; - case camodocal::Camera::MEI: - std::cout << "# INFO: Camera model: Mei" << std::endl; - break; - case camodocal::Camera::PINHOLE: - std::cout << "# INFO: Camera model: Pinhole" << std::endl; - break; - case camodocal::Camera::PINHOLE_FULL: - std::cout << "# INFO: Camera model: PinholeFull" << std::endl; - break; - case camodocal::Camera::SCARAMUZZA: - std::cout << "# INFO: Camera model: Scaramuzza-Omnidirect" << std::endl; - break; + // check if file extension matches + if (filename.compare(filename.length() - fileExtension.length(), + fileExtension.length(), fileExtension) != 0) { + continue; } - // look for images in input directory - std::vector< std::string > imageFilenames; - boost::filesystem::directory_iterator itr; - for ( boost::filesystem::directory_iterator itr( inputDir ); - itr != boost::filesystem::directory_iterator( ); - ++itr ) - { - if ( !boost::filesystem::is_regular_file( itr->status( ) ) ) - { - continue; - } - - std::string filename = itr->path( ).filename( ).string( ); - - // check if prefix matches - if ( !prefix.empty( ) ) - { - if ( filename.compare( 0, prefix.length( ), prefix ) != 0 ) - { - continue; - } - } - - // check if file extension matches - if ( filename.compare( filename.length( ) - fileExtension.length( ), fileExtension.length( ), fileExtension ) - != 0 ) - { - continue; - } - - imageFilenames.push_back( itr->path( ).string( ) ); - - if ( verbose ) - { - std::cerr << "# INFO: Adding " << imageFilenames.back( ) << std::endl; - } - } + imageFilenames.push_back(itr->path().string()); - if ( imageFilenames.empty( ) ) - { - std::cerr << "# ERROR: No chessboard images found." << std::endl; - return 1; + if (verbose) { + std::cerr << "# INFO: Adding " << imageFilenames.back() << std::endl; } + } - if ( verbose ) - { - std::cerr << "# INFO: # images: " << imageFilenames.size( ) << std::endl; - } + if (imageFilenames.empty()) { + std::cerr << "# ERROR: No chessboard images found." << std::endl; + return 1; + } - std::sort( imageFilenames.begin( ), imageFilenames.end( ) ); + if (verbose) { + std::cerr << "# INFO: # images: " << imageFilenames.size() << std::endl; + } - cv::Mat image = cv::imread( imageFilenames.front( ), -1 ); - const cv::Size frameSize = image.size( ); + std::sort(imageFilenames.begin(), imageFilenames.end()); - camodocal::CameraCalibration calibration( modelType, cameraName, frameSize, boardSize, squareSize ); - calibration.setVerbose( verbose ); + cv::Mat image = cv::imread(imageFilenames.front(), -1); + const cv::Size frameSize = image.size(); - std::vector< bool > chessboardFound( imageFilenames.size( ), false ); - for ( size_t i = 0; i < imageFilenames.size( ); ++i ) - { - image = cv::imread( imageFilenames.at( i ), -1 ); + camodocal::CameraCalibration calibration(modelType, cameraName, frameSize, + boardSize, squareSize); + calibration.setVerbose(verbose); - camodocal::Chessboard chessboard( boardSize, image ); + std::vector chessboardFound(imageFilenames.size(), false); + for (size_t i = 0; i < imageFilenames.size(); ++i) { + image = cv::imread(imageFilenames.at(i), -1); - chessboard.findCorners( useOpenCV ); - if ( chessboard.cornersFound( ) ) - { - if ( verbose ) - { - std::cerr << "# INFO: Detected chessboard in image " << i + 1 << ", " - << imageFilenames.at( i ) << std::endl; - } + camodocal::Chessboard chessboard(boardSize, image); - calibration.addChessboardData( chessboard.getCorners( ) ); + chessboard.findCorners(useOpenCV); + if (chessboard.cornersFound()) { + if (verbose) { + std::cerr << "# INFO: Detected chessboard in image " << i + 1 << ", " + << imageFilenames.at(i) << std::endl; + } - cv::Mat sketch; - chessboard.getSketch( ).copyTo( sketch ); + calibration.addChessboardData(chessboard.getCorners()); - cv::imshow( "Image", sketch ); - cv::waitKey( 50 ); - } - else if ( verbose ) - { - std::cerr << "# INFO: Did not detect chessboard in image " << i + 1 << std::endl; - } - chessboardFound.at( i ) = chessboard.cornersFound( ); - } - cv::destroyWindow( "Image" ); + cv::Mat sketch; + chessboard.getSketch().copyTo(sketch); - if ( calibration.sampleCount( ) < 10 ) - { - std::cerr << "# ERROR: Insufficient number of detected chessboards." << std::endl; - return 1; + cv::imshow("Image", sketch); + cv::waitKey(50); + } else if (verbose) { + std::cerr << "# INFO: Did not detect chessboard in image " << i + 1 + << std::endl; } - - if ( verbose ) - { - std::cerr << "# INFO: Calibrating..." << std::endl; + chessboardFound.at(i) = chessboard.cornersFound(); + } + cv::destroyWindow("Image"); + + if (calibration.sampleCount() < 10) { + std::cerr << "# ERROR: Insufficient number of detected chessboards." + << std::endl; + return 1; + } + + if (verbose) { + std::cerr << "# INFO: Calibrating..." << std::endl; + } + + double startTime = camodocal::timeInSeconds(); + + calibration.calibrate(); + calibration.writeParams(cameraName + "_camera_calib.yaml"); + calibration.writeChessboardData(cameraName + "_chessboard_data.dat"); + + if (verbose) { + std::cout << "# INFO: Calibration took a total time of " << std::fixed + << std::setprecision(3) << camodocal::timeInSeconds() - startTime + << " sec.\n"; + } + + if (verbose) { + std::cerr << "# INFO: Wrote calibration file to " + << cameraName + "_camera_calib.yaml" << std::endl; + } + + if (viewResults) { + std::vector cbImages; + std::vector cbImageFilenames; + + for (size_t i = 0; i < imageFilenames.size(); ++i) { + if (!chessboardFound.at(i)) { + continue; + } + + cbImages.push_back(cv::imread(imageFilenames.at(i), -1)); + cbImageFilenames.push_back(imageFilenames.at(i)); } - double startTime = camodocal::timeInSeconds( ); - - calibration.calibrate( ); - calibration.writeParams( cameraName + "_camera_calib.yaml" ); - calibration.writeChessboardData( cameraName + "_chessboard_data.dat" ); - - if ( verbose ) - { - std::cout << "# INFO: Calibration took a total time of " << std::fixed - << std::setprecision( 3 ) << camodocal::timeInSeconds( ) - startTime << " sec.\n"; - } - - if ( verbose ) - { - std::cerr << "# INFO: Wrote calibration file to " - << cameraName + "_camera_calib.yaml" << std::endl; - } + // visualize observed and reprojected points + calibration.drawResults(cbImages); - if ( viewResults ) - { - std::vector< cv::Mat > cbImages; - std::vector< std::string > cbImageFilenames; - - for ( size_t i = 0; i < imageFilenames.size( ); ++i ) - { - if ( !chessboardFound.at( i ) ) - { - continue; - } - - cbImages.push_back( cv::imread( imageFilenames.at( i ), -1 ) ); - cbImageFilenames.push_back( imageFilenames.at( i ) ); - } - - // visualize observed and reprojected points - calibration.drawResults( cbImages ); - - for ( size_t i = 0; i < cbImages.size( ); ++i ) - { - cv::putText( cbImages.at( i ), - cbImageFilenames.at( i ), - cv::Point( 10, 20 ), - cv::FONT_HERSHEY_COMPLEX, - 0.5, - cv::Scalar( 255, 255, 255 ), - 1, - CV_AA ); - cv::imshow( "Image", cbImages.at( i ) ); - cv::waitKey( 0 ); - } + for (size_t i = 0; i < cbImages.size(); ++i) { + cv::putText(cbImages.at(i), cbImageFilenames.at(i), cv::Point(10, 20), + cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(255, 255, 255), 1, + CV_AA); + cv::imshow("Image", cbImages.at(i)); + cv::waitKey(0); } + } - return 0; + return 0; } diff --git a/camera_models/src/sparse_graph/Transform.cc b/camera_models/src/sparse_graph/Transform.cc index db979e9f..ccefe400 100644 --- a/camera_models/src/sparse_graph/Transform.cc +++ b/camera_models/src/sparse_graph/Transform.cc @@ -1,77 +1,44 @@ #include -namespace camodocal -{ +namespace camodocal { -Transform::Transform() -{ - m_q.setIdentity(); - m_t.setZero(); +Transform::Transform() { + m_q.setIdentity(); + m_t.setZero(); } -Transform::Transform(const Eigen::Matrix4d& H) -{ - m_q = Eigen::Quaterniond(H.block<3,3>(0,0)); - m_t = H.block<3,1>(0,3); +Transform::Transform(const Eigen::Matrix4d& H) { + m_q = Eigen::Quaterniond(H.block<3, 3>(0, 0)); + m_t = H.block<3, 1>(0, 3); } -Eigen::Quaterniond& -Transform::rotation(void) -{ - return m_q; -} +Eigen::Quaterniond& Transform::rotation(void) { return m_q; } -const Eigen::Quaterniond& -Transform::rotation(void) const -{ - return m_q; -} +const Eigen::Quaterniond& Transform::rotation(void) const { return m_q; } -double* -Transform::rotationData(void) -{ - return m_q.coeffs().data(); -} +double* Transform::rotationData(void) { return m_q.coeffs().data(); } -const double* const -Transform::rotationData(void) const -{ - return m_q.coeffs().data(); +const double* const Transform::rotationData(void) const { + return m_q.coeffs().data(); } -Eigen::Vector3d& -Transform::translation(void) -{ - return m_t; -} +Eigen::Vector3d& Transform::translation(void) { return m_t; } -const Eigen::Vector3d& -Transform::translation(void) const -{ - return m_t; -} +const Eigen::Vector3d& Transform::translation(void) const { return m_t; } -double* -Transform::translationData(void) -{ - return m_t.data(); -} +double* Transform::translationData(void) { return m_t.data(); } -const double* const -Transform::translationData(void) const -{ - return m_t.data(); +const double* const Transform::translationData(void) const { + return m_t.data(); } -Eigen::Matrix4d -Transform::toMatrix(void) const -{ - Eigen::Matrix4d H; - H.setIdentity(); - H.block<3,3>(0,0) = m_q.toRotationMatrix(); - H.block<3,1>(0,3) = m_t; +Eigen::Matrix4d Transform::toMatrix(void) const { + Eigen::Matrix4d H; + H.setIdentity(); + H.block<3, 3>(0, 0) = m_q.toRotationMatrix(); + H.block<3, 1>(0, 3) = m_t; - return H; + return H; } -} +} // namespace camodocal diff --git a/config/GRACO/GRACO_calib.yaml b/config/GRACO/GRACO_calib.yaml new file mode 100644 index 00000000..01a54619 --- /dev/null +++ b/config/GRACO/GRACO_calib.yaml @@ -0,0 +1,36 @@ +# General sensor definitions +cam0: + rostopic: /camera_left/image_raw + rate_hz: 20 + camera_model: pinhole + distortion_model: radtan + # intrinsics: [927.5410810796335, 925.389148860952, 811.8599402206614, 559.2929057540543] # Raw: fu, fv, cu, cv + intrinsics: [371.01643243, 370.15565954, 324.74397609, 223.7171623 ] # fu, fv, cu, cv; resize 0.4 + distortion_coeffs: [-0.09786697008306534, 0.08078150351404789, 5.543660982644687e-05, + 0.00018974368443100656] # k1, k2, r1, r2 + # resolution: [1600, 1100] # Raw + resolution: [640, 440] # Resize 0.4 + sensor_type: Flir-BFS-U3-16S7C + T_cam_imu: + - [-0.99995218, -0.00522539, -0.00826584, 0.21743355] + - [-0.00827815, 0.00233898, 0.999963 , 0.04673507] + - [-0.00520586, 0.99998361, -0.00238212, -0.01191991] + - [0. , 0. , 0. , 1. ] + +cam1: + rostopic: /camera_right/image_raw + rate_hz: 20 + camera_model: pinhole + distortion_model: radtan + # intrinsics: [928.5576204918095, 926.479793310928, 796.9882335822198, 551.2880386404536] # Raw: fu, fv, cu, cv + intrinsics: [371.4230482 , 370.59191732, 318.79529343, 220.51521546] # fu, fv, cu, cv + distortion_coeffs: [-0.09901101241806308, 0.0836128720669121, 0.0003842138637237634, + 0.00015862322536168435] # k1, k2, r1, r2 + # resolution: [1600, 1100] # Raw + resolution: [640, 440] # Resize 0.4 + sensor_type: Flir-BFS-U3-16S7C + T_cam_imu: + - [-0.99995748, -0.00616692, 0.00685641, -0.21651824] + - [0.00684926, 0.00118102, 0.99997585, 0.04683164] + - [-0.00617487, 0.99998029, -0.00113873, -0.01111243] + - [0. , 0. , 0. , 1. ] diff --git a/config/GRACO/GRACO_calib_ground.yaml b/config/GRACO/GRACO_calib_ground.yaml new file mode 100644 index 00000000..56f12fae --- /dev/null +++ b/config/GRACO/GRACO_calib_ground.yaml @@ -0,0 +1,36 @@ +# General sensor definitions +cam0: + rostopic: /camera_left/image_raw + rate_hz: 20 + camera_model: pinhole + distortion_model: radtan + # intrinsics: [940.862825677534, 938.554923506332, 799.1626975233576, 559.295406893583] # Raw: fu, fv, cu, cv + intrinsics: [376.34513027, 375.4219694 , 319.66507901, 223.71816276] # fu, fv, cu, cv; resize 0.4 + distortion_coeffs: [-0.1008504099655989, 0.08905706623788286, 0.0007516966627205781, + -0.0011958374307601393] # k1, k2, r1, r2 + # resolution: [1600, 1100] # Raw + resolution: [640, 440] # Resize 0.4 + sensor_type: Flir-BFS-U3-16S7C + T_cam_imu: + - [ 0.99985436, -0.00116148, -0.0170267 , -0.11655291] + - [ 0.01702167, -0.0042153 , 0.99984624, 0.01614558] + - [ -0.00123307, -0.99999044, -0.00419492, 0.07950961] + - [ 0. , 0. , 0. , 1. ] + +cam1: + rostopic: /camera_right/image_raw + rate_hz: 20 + camera_model: pinhole + distortion_model: radtan + # intrinsics: [934.5190744321391, 932.525429508503, 792.8073165035943, 562.7061769000949] # Raw: fu, fv, cu, cv + intrinsics: [373.80762977, 373.0101718 , 317.1229266 , 225.08247076] # fu, fv, cu, cv + distortion_coeffs: [-0.10093604150569942, 0.08966460684307566, 0.0006752623328139636, + -0.0015031965111152103] # k1, k2, r1, r2 + # resolution: [1600, 1100] # Raw + resolution: [640, 440] # Resize 0.4 + sensor_type: Flir-BFS-U3-16S7C + T_cam_imu: + - [0.99979217, -0.02037659, 0.00063456, 0.11568842] + - [-0.0007412 , -0.00522643, 0.99998607, 0.01651153] + - [-0.02037299, -0.99977872, -0.00524045, 0.07766165] + - [0. , 0. , 0. , 1. ] diff --git a/config/GRACO/GRACO_multi.yaml b/config/GRACO/GRACO_multi.yaml new file mode 100644 index 00000000..d8167aa7 --- /dev/null +++ b/config/GRACO/GRACO_multi.yaml @@ -0,0 +1,146 @@ +%YAML:1.0 + +#inputs +imu: 1 +is_fisheye: 0 +imu_topic: "/gnss/imu" +image0_topic: "/camera_left/image_raw_resize" +image1_topic: "/camera_right/image_raw_resize" +is_compressed_images: 1 + +imu_freq: 125 +image_freq: 20 +frame_step: 2 + +#Camera configuration +camera_configuration: 0 #STEREO_PINHOLE = 0, STEREO_FISHEYE = 1, PINHOLE_DEPTH = 2, FOURCORNER_FISHEYE = 3 +num_of_cam: 2 +image_width: 640 +image_height: 440 +calib_file_path: "GRACO_calib.yaml" + +#estimation +estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. +estimate_td: 0 # online estimate time offset between camera and imu +td: 0.00 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock) +estimation_mode: 0 #0:single; 1:solve all; 2: distributed; 3:server +double_counting_common_feature: 0 +#0:single; 1:solve all; 2: distributed; 3:server + +#optimization parameters +max_solver_time: 0.08 # max solver itration time (ms), to guarantee real time +max_num_iterations: 8 # max solver itrations, to guarantee real time +consensus_max_steps: 1 +timout_wait_sync: 50 +rho_landmark: 1.0 +rho_frame_T: 100 +rho_frame_theta: 100 +relaxation_alpha: 0. +#Note these two param must set to 0 when lazy_broadcast_keyframe=1 +consensus_sync_for_averaging: 0 +consensus_sync_to_start: 0 #Is sync on start of the solving.. +consensus_trigger_time_err_us: 100 + +#depth fusing +depth_far_thres: 3.0 # The max depth in frontend +depth_near_thres: 0.3 +fuse_dep: 0 #if fuse depth measurement +max_depth_to_fuse: 5.0 +min_depth_to_fuse: 0.3 + +#Multi-drone +track_remote_netvlad_thres: 0.8 + +#Initialization +init_method: 0 #0 IMU, 1 PnP +depth_estimate_baseline: -0.01 +add_vel_ba_prior: 0 + +#sliding window +max_sld_win_size: 11 +landmark_estimate_tracks: 4 #when use depth or stereo, 3 is OK. +min_solve_frames: 5 + +#solver +multiple_thread: 1 + +#outlier rejection +thres_outlier : 10.0 +perform_outlier_rejection_num: 10 +tri_max_err: 1000 + +#Marginalization +enable_marginalization: 1 +margin_sparse_solver: 0 +always_fixed_first_pose: 0 +remove_base_when_margin_remote: 2 +# When set to 2, will use the all relevant measurements of the removing frames to compute the prior, +# and the baseFrame (where!=removeID) will not be removed. This may lead to double counting of this baseFrame measurement: but it's stable. +# When set to 1, will remove the baseframe's measurements of those measurements which is not base on current frame. +# set to 0 those measurements (which on a landmark's baseFrame is not been removed) will be ignore. + +#feature tracker parameters +max_cnt: 250 # max feature number in feature tracking +max_superpoint_cnt: 250 # max feature extraction by superpoint +max_solve_cnt: 250 +max_solve_measurements: 10000 +check_essential: 1 +enable_lk_optical_flow: 1 #enable lk opticalflow featuretrack to enhance ego-motion estimation. +remote_min_match_num: 40 +enable_superglue_local: 0 +enable_superglue_remote: 0 +ransacReprojThreshold: 10.0 +enable_search_local_aera: 1 +search_local_max_dist: 0.1 +feature_min_dist: 15 +parallex_thres: 0.06 +knn_match_ratio: 0.9 # This apply to superpoint feature track & loop clouse detection. +track_from_keyframe: 0 + +#CNN +cnn_use_onnx: 1 +enable_pca_superpoint: 1 +superpoint_pca_dims: 64 +cnn_enable_tensorrt: 0 +enable_pca_netvlad: 1 +netvlad_pca_dims: 1024 + +acc_n: 1.8744963e-01 # accelerometer measurement noise standard deviation. #0.2 0.04 +gyr_n: 5.4259815e-03 # gyroscope measurement noise standard deviation. #0.05 0.004 +acc_w: 6.4808910e-03 # accelerometer bias random work noise standard deviation. #0.002 +gyr_w: 1.0949571e-04 # gyroscope bias random work noise standard deviation. #4.0e-5 +g_norm: 9.805 # gravity magnitude + +#Loop Closure Detection +loop_detection_netvlad_thres: 0.8 +enable_homography_test: 0 +loop_inlier_feature_num: 30 +accept_loop_max_yaw: 10 +accept_loop_max_pos: 10.0 +lazy_broadcast_keyframe: 0 # 0: greedy broadcast; 1: compact mode broadcast +nearby_drone_dist: 3.0 +gravity_check_thres: 0.1 +enable_pcm: 1 +pcm_thres: 3.5 + +#PGO +pgo_solver_time: 1.0 +solver_timer_freq: 0.2 +pgo_mode: 1 +pgo_rho_frame_T: 0.01 +pgo_rho_frame_theta: 2.0 +pgo_eta_k: 1.0 +write_g2o: 1 +g2o_output_path: "output.g2o" +write_pgo_to_file: 1 +debug_save_g2o_only: 0 + +#outputs +output_path: "/home/xuhao/output/" +debug_print_sldwin: 0 +debug_print_states: 0 +enable_perf_output: 0 +print_network_status: 0 +debug_write_margin_matrix: 0 +show_track_id: 0 +write_tracking_image_to_file: 0 \ No newline at end of file diff --git a/config/GRACO/GRACO_single.yaml b/config/GRACO/GRACO_single.yaml new file mode 100644 index 00000000..91d9a238 --- /dev/null +++ b/config/GRACO/GRACO_single.yaml @@ -0,0 +1,146 @@ +%YAML:1.0 + +#inputs +imu: 1 +is_fisheye: 0 +imu_topic: "/gnss/imu" +image0_topic: "/camera_left/image_raw_resize" +image1_topic: "/camera_right/image_raw_resize" +is_compressed_images: 1 + +imu_freq: 125 +image_freq: 20 +frame_step: 2 + +#Camera configuration +camera_configuration: 0 #STEREO_PINHOLE = 0, STEREO_FISHEYE = 1, PINHOLE_DEPTH = 2, FOURCORNER_FISHEYE = 3 +num_of_cam: 2 +image_width: 640 +image_height: 440 +calib_file_path: "GRACO_calib.yaml" + +#estimation +estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. +estimate_td: 0 # online estimate time offset between camera and imu +td: 0.00 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock) +estimation_mode: 0 #0:single; 1:solve all; 2: distributed; 3:server +double_counting_common_feature: 0 +#0:single; 1:solve all; 2: distributed; 3:server + +#optimization parameters +max_solver_time: 0.08 # max solver itration time (ms), to guarantee real time +max_num_iterations: 8 # max solver itrations, to guarantee real time +consensus_max_steps: 1 +timout_wait_sync: 50 +rho_landmark: 1.0 +rho_frame_T: 100 +rho_frame_theta: 100 +relaxation_alpha: 0. +#Note these two param must set to 0 when lazy_broadcast_keyframe=1 +consensus_sync_for_averaging: 0 +consensus_sync_to_start: 0 #Is sync on start of the solving.. +consensus_trigger_time_err_us: 100 + +#depth fusing +depth_far_thres: 3.0 # The max depth in frontend +depth_near_thres: 0.3 +fuse_dep: 0 #if fuse depth measurement +max_depth_to_fuse: 5.0 +min_depth_to_fuse: 0.3 + +#Multi-drone +track_remote_netvlad_thres: 0.8 + +#Initialization +init_method: 0 #0 IMU, 1 PnP +depth_estimate_baseline: -0.01 +add_vel_ba_prior: 0 + +#sliding window +max_sld_win_size: 11 +landmark_estimate_tracks: 4 #when use depth or stereo, 3 is OK. +min_solve_frames: 5 + +#solver +multiple_thread: 1 + +#outlier rejection +thres_outlier : 10.0 +perform_outlier_rejection_num: 10 +tri_max_err: 1000 + +#Marginalization +enable_marginalization: 1 +margin_sparse_solver: 0 +always_fixed_first_pose: 0 +remove_base_when_margin_remote: 2 +# When set to 2, will use the all relevant measurements of the removing frames to compute the prior, +# and the baseFrame (where!=removeID) will not be removed. This may lead to double counting of this baseFrame measurement: but it's stable. +# When set to 1, will remove the baseframe's measurements of those measurements which is not base on current frame. +# set to 0 those measurements (which on a landmark's baseFrame is not been removed) will be ignore. + +#feature tracker parameters +max_cnt: 250 # max feature number in feature tracking +max_superpoint_cnt: 250 # max feature extraction by superpoint +max_solve_cnt: 250 +max_solve_measurements: 10000 +check_essential: 1 +enable_lk_optical_flow: 1 #enable lk opticalflow featuretrack to enhance ego-motion estimation. +remote_min_match_num: 40 +enable_superglue_local: 0 +enable_superglue_remote: 0 +ransacReprojThreshold: 10.0 +enable_search_local_aera: 1 +search_local_max_dist: 0.1 +feature_min_dist: 15 +parallex_thres: 0.06 +knn_match_ratio: 0.9 # This apply to superpoint feature track & loop clouse detection. +track_from_keyframe: 0 + +#CNN +cnn_use_onnx: 1 +enable_pca_superpoint: 1 +superpoint_pca_dims: 64 +cnn_enable_tensorrt: 0 +enable_pca_netvlad: 1 +netvlad_pca_dims: 1024 + +acc_n: 1.8744963e-01 # accelerometer measurement noise standard deviation. #0.2 0.04 +gyr_n: 5.4259815e-03 # gyroscope measurement noise standard deviation. #0.05 0.004 +acc_w: 6.4808910e-03 # accelerometer bias random work noise standard deviation. #0.002 +gyr_w: 1.0949571e-04 # gyroscope bias random work noise standard deviation. #4.0e-5 +g_norm: 9.805 # gravity magnitude + +#Loop Closure Detection +loop_detection_netvlad_thres: 0.8 +enable_homography_test: 0 +loop_inlier_feature_num: 30 +accept_loop_max_yaw: 10 +accept_loop_max_pos: 1.0 +lazy_broadcast_keyframe: 0 # 0: greedy broadcast; 1: compact mode broadcast +nearby_drone_dist: 3.0 +gravity_check_thres: 0.06 +enable_pcm: 1 +pcm_thres: 3.5 + +#PGO +pgo_solver_time: 1.0 +solver_timer_freq: 0.2 +pgo_mode: 1 +pgo_rho_frame_T: 0.01 +pgo_rho_frame_theta: 2.0 +pgo_eta_k: 1.0 +write_g2o: 1 +g2o_output_path: "output.g2o" +write_pgo_to_file: 1 +debug_save_g2o_only: 0 + +#outputs +output_path: "/home/xuhao/output/" +debug_print_sldwin: 0 +debug_print_states: 0 +enable_perf_output: 0 +print_network_status: 0 +debug_write_margin_matrix: 0 +show_track_id: 0 +write_tracking_image_to_file: 0 \ No newline at end of file diff --git a/config/GRACO/GRACO_single_gnd.yaml b/config/GRACO/GRACO_single_gnd.yaml new file mode 100644 index 00000000..6ee353be --- /dev/null +++ b/config/GRACO/GRACO_single_gnd.yaml @@ -0,0 +1,144 @@ +%YAML:1.0 + +#inputs +imu: 1 +is_fisheye: 0 +imu_topic: "/gnss/imu" +image0_topic: "/camera_left/image_raw_resize" +image1_topic: "/camera_right/image_raw_resize" +is_compressed_images: 1 + +imu_freq: 125 +image_freq: 20 +frame_step: 2 + +#Camera configuration +camera_configuration: 0 #STEREO_PINHOLE = 0, STEREO_FISHEYE = 1, PINHOLE_DEPTH = 2, FOURCORNER_FISHEYE = 3 +num_of_cam: 2 +image_width: 640 +image_height: 440 +# calib_file_path: "GRACO_calib.yaml" +calib_file_path: "GRACO_calib_ground.yaml" + +#estimation +estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. +estimate_td: 0 # online estimate time offset between camera and imu +td: 0.00 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock) +estimation_mode: 0 #0:single; 1:solve all; 2: distributed; 3:server +double_counting_common_feature: 0 +#0:single; 1:solve all; 2: distributed; 3:server + +#optimization parameters +max_solver_time: 0.08 # max solver itration time (ms), to guarantee real time +max_num_iterations: 8 # max solver itrations, to guarantee real time +consensus_max_steps: 1 +timout_wait_sync: 50 +rho_landmark: 1.0 +rho_frame_T: 100 +rho_frame_theta: 100 +relaxation_alpha: 0. +#Note these two param must set to 0 when lazy_broadcast_keyframe=1 +consensus_sync_for_averaging: 0 +consensus_sync_to_start: 0 #Is sync on start of the solving.. +consensus_trigger_time_err_us: 100 + +#depth fusing +depth_far_thres: 3.0 # The max depth in frontend +depth_near_thres: 0.3 +fuse_dep: 0 #if fuse depth measurement +max_depth_to_fuse: 5.0 +min_depth_to_fuse: 0.3 + +#Multi-drone +track_remote_netvlad_thres: 0.8 + +#Initialization +init_method: 0 #0 IMU, 1 PnP +depth_estimate_baseline: 0.03 + +#sliding window +max_sld_win_size: 11 +landmark_estimate_tracks: 4 #when use depth or stereo, 3 is OK. +min_solve_frames: 10 + +#solver +multiple_thread: 1 + +#outlier rejection +thres_outlier : 10.0 +perform_outlier_rejection_num: 100 +tri_max_err: 0.2 + +#Marginalization +enable_marginalization: 1 +margin_sparse_solver: 0 +always_fixed_first_pose: 0 +remove_base_when_margin_remote: 2 +# When set to 2, will use the all relevant measurements of the removing frames to compute the prior, +# and the baseFrame (where!=removeID) will not be removed. This may lead to double counting of this baseFrame measurement: but it's stable. +# When set to 1, will remove the baseframe's measurements of those measurements which is not base on current frame. +# set to 0 those measurements (which on a landmark's baseFrame is not been removed) will be ignore. + +#feature tracker parameters +max_cnt: 250 # max feature number in feature tracking +max_superpoint_cnt: 250 # max feature extraction by superpoint +max_solve_cnt: 250 +check_essential: 1 +enable_lk_optical_flow: 0 #enable lk opticalflow featuretrack to enhance ego-motion estimation. +remote_min_match_num: 40 +enable_superglue_local: 0 +enable_superglue_remote: 0 +ransacReprojThreshold: 10.0 +enable_search_local_aera: 1 +search_local_max_dist: 0.04 +feature_min_dist: 15 +parallex_thres: 0.02 +knn_match_ratio: 0.8 #This apply to superpoint feature track & loop clouse detection. + +#CNN +cnn_use_onnx: 1 +enable_pca_superpoint: 1 +superpoint_pca_dims: 64 +cnn_enable_tensorrt: 0 +enable_pca_netvlad: 1 +netvlad_pca_dims: 1024 + +acc_n: 1.8744963e-01 # accelerometer measurement noise standard deviation. #0.2 0.04 +gyr_n: 5.4259815e-03 # gyroscope measurement noise standard deviation. #0.05 0.004 +acc_w: 6.4808910e-03 # accelerometer bias random work noise standard deviation. #0.002 +gyr_w: 1.0949571e-04 # gyroscope bias random work noise standard deviation. #4.0e-5 +g_norm: 9.805 # gravity magnitude + +#Loop Closure Detection +loop_detection_netvlad_thres: 0.8 +enable_homography_test: 0 +loop_inlier_feature_num: 60 +accept_loop_max_yaw: 10 +accept_loop_max_pos: 1.0 +lazy_broadcast_keyframe: 0 # 0: greedy broadcast; 1: compact mode broadcast +nearby_drone_dist: 3.0 +gravity_check_thres: 0.03 +enable_pcm: 1 +pcm_thres: 3.5 + +#PGO +pgo_solver_time: 1.0 +solver_timer_freq: 0.2 +pgo_mode: 1 +pgo_rho_frame_T: 0.01 +pgo_rho_frame_theta: 2.0 +pgo_eta_k: 1.0 +write_g2o: 1 +g2o_output_path: "output.g2o" +write_pgo_to_file: 1 +debug_save_g2o_only: 0 + +#outputs +output_path: "/root/output/" +debug_print_sldwin: 0 +debug_print_states: 0 +enable_perf_output: 0 +print_network_status: 0 +debug_write_margin_matrix: 0 +show_track_id: 0 +write_tracking_image_to_file: 0 \ No newline at end of file diff --git a/config/MIT_datasets/d435_single.yaml b/config/MIT_datasets/d435_single.yaml new file mode 100644 index 00000000..6d327415 --- /dev/null +++ b/config/MIT_datasets/d435_single.yaml @@ -0,0 +1,157 @@ +%YAML:1.0 + +#inputs +imu: 1 +imu_topic: "/acl_jackal/forward/imu" +image0_topic: "/acl_jackal/forward/infra1/image_rect_raw" +image1_topic: "/acl_jackal/forward/infra2/image_rect_raw" + +is_compressed_images: 1 + +imu_freq: 400 +image_freq: 30 +frame_step: 2 + +#Camera configuration +camera_configuration: 0 #STEREO_PINHOLE = 0, STEREO_FISHEYE = 1, PINHOLE_DEPTH = 2, FOURCORNER_FISHEYE = 3 +num_of_cam: 2 +cam0_calib: "infra.yaml" +cam1_calib: "infra.yaml" +image_width: 640 +image_height: 480 + +body_T_cam0: !!opencv-matrix + rows: 4 + cols: 4 + dt: d + data: [0.9998495663564139, -0.01332358982788149, 0.01110525150501032, -0.03068970324656957, + 0.01316436364820382, 0.9998112331832488, 0.01428977012863299, -0.06789244691763885, + -0.01129354623796312, -0.01414142689722982, 0.9998362245181364, 0.03980004121672052, + 0, 0, 0, 1] +body_T_cam1: !!opencv-matrix + rows: 4 + cols: 4 + dt: d + data: [0.9998326094055786, -0.01317931478898709, 0.01269089559189768, 0.06038933631975229, + 0.01298734678025223, 0.999801762507382, 0.01509186902746622, -0.06753424960055816, + -0.01288728027324361, -0.01492452172853483, 0.999805569427543, 0.03633758435321684, + 0, 0, 0, 1] + +#estimation +estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. +estimate_td: 0 # online estimate time offset between camera and imu +td: 0.00 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock) +estimation_mode: 0 #0:single; 1:solve all; 2: distributed; 3:server +double_counting_common_feature: 0 +not_estimate_first_extrinsic: 1 + +#optimization parameters +max_solver_time: 0.08 # max solver itration time (ms), to guarantee real time +max_num_iterations: 8 # max solver itrations, to guarantee real time +consensus_max_steps: 1 +timout_wait_sync: 100 +rho_landmark: 1.0 +rho_frame_T: 100 +rho_frame_theta: 100 +relaxation_alpha: 0. +consensus_sync_for_averaging: 0 +consensus_sync_to_start: 0 #Is sync on start of the solving.. +consensus_trigger_time_err_us: 100 + +#depth fusing +depth_far_thres: 3.0 # The max depth in frontend +depth_near_thres: 0.3 +fuse_dep: 0 #if fuse depth measurement +max_depth_to_fuse: 5.0 +min_depth_to_fuse: 0.3 + +#Multi-drone +track_remote_netvlad_thres: 0.5 + +#Initialization +init_method: 0 #0 IMU, 1 PnP +depth_estimate_baseline: 0.05 +enable_sfm_initialization: 0 + +#sliding window +max_sld_win_size: 11 +landmark_estimate_tracks: 4 #when use depth or stereo, 3 is OK. +min_solve_frames: 6 + +#outlier rejection +thres_outlier : 10.0 +perform_outlier_rejection_num: 30 +tri_max_err: 10 + +#Marginalization +enable_marginalization: 1 +margin_sparse_solver: 0 +always_fixed_first_pose: 0 +remove_base_when_margin_remote: 2 +# When set to 2, will use the all relevant measurements of the removing frames to compute the prior, +# and the baseFrame (where!=removeID) will not be removed. This may lead to double counting of this baseFrame measurement: but it's stable. +# When set to 1, will remove the baseframe's measurements of those measurements which is not base on current frame. +# set to 0 those measurements (which on a landmark's baseFrame is not been removed) will be ignore. + +#feature tracker parameters +max_cnt: 200 # max feature number in feature tracking +max_superpoint_cnt: 250 # max feature extraction by superpoint +max_solve_cnt: 250 +show_track_id: 0 +check_essential: 0 +enable_lk_optical_flow: 1 #enable lk opticalflow featuretrack to enhance ego-motion estimation. +remote_min_match_num: 40 +ransacReprojThreshold: 10.0 +parallex_thres: 0.02 +knn_match_ratio: 0.7 #This apply to superpoint feature track & loop clouse detection. +feature_min_dist: 20 +sp_track_use_lk: 1 + +#CNN +cnn_use_onnx: 1 +enable_pca_superpoint: 0 +enable_pca_netvlad: 0 +cnn_enable_tensorrt: 0 +cnn_enable_tensorrt_int8: 0 +netvlad_int8_calib_table_name: "mobilenetvlad_calibration.flatbuffers" +superpoint_int8_calib_table_name: "superpoint_calibration.flatbuffers" + +# #Measurement parameters The more accurate parameters you provide, the better performance +acc_n: 0.1 # accelerometer measurement noise standard deviation. +gyr_n: 0.05 # gyroscope measurement noise standard deviation. +acc_w: 0.002 # accelerometer bias random work noise standard deviation. +gyr_w: 0.0004 # gyroscope bias random work noise standard deviation. +g_norm: 9.81 # gravity magnitude + +#Loop Closure Detection +loop_detection_netvlad_thres: 0.8 +enable_homography_test: 0 +accept_loop_max_yaw: 10 +accept_loop_max_pos: 1.0 +loop_inlier_feature_num: 20 +gravity_check_thres: 0.06 +pgo_solver_time: 1.0 +solver_timer_freq: 1.0 +enable_pcm: 1 +pcm_thres: 2.8 + + +#PGO +pgo_solver_time: 1.0 +solver_timer_freq: 0.2 +pgo_mode: 0 +pgo_rho_frame_T: 0.01 +pgo_rho_frame_theta: 2.0 +pgo_eta_k: 1.0 +write_g2o: 1 +g2o_output_path: "output.g2o" +write_pgo_to_file: 1 +debug_save_g2o_only: 0 + + +#outputs +output_path: "/home/xuhao/data/d2slam/mit_campus_hybrid/outputs/" +debug_print_sldwin: 0 +debug_print_states: 0 +enable_perf_output: 0 +debug_write_margin_matrix: 0 diff --git a/config/NTU_VIRAL/NTU_VIRAL_multi.yaml b/config/NTU_VIRAL/NTU_VIRAL_multi.yaml new file mode 100644 index 00000000..abe7bf2d --- /dev/null +++ b/config/NTU_VIRAL/NTU_VIRAL_multi.yaml @@ -0,0 +1,168 @@ +%YAML:1.0 + +#inputs +imu: 1 +is_fisheye: 0 +imu_topic: "/imu/imu" +image0_topic: "/left/image_raw" +image1_topic: "/right/image_raw" +is_compressed_images: 1 + +imu_freq: 385 +image_freq: 10 +frame_step: 1 + +#Camera configuration +camera_configuration: 0 #STEREO_PINHOLE = 0, STEREO_FISHEYE = 1, PINHOLE_DEPTH = 2, FOURCORNER_FISHEYE = 3 +num_of_cam: 2 +cam0_calib: "camera_left.yaml" +cam1_calib: "camera_right.yaml" +image_width: 752 +image_height: 480 + +body_T_cam0: !!opencv-matrix + rows: 4 + cols: 4 + dt: d + data: [ 0.02183084, -0.01312053, 0.99967558, 0.00552943, + 0.99975965, 0.00230088, -0.02180248, -0.12431302, + -0.00201407, 0.99991127, 0.01316761, 0.01614686, + 0.00000000, 0.00000000, 0.00000000, 1.00000000 ] +body_T_cam1: !!opencv-matrix + rows: 4 + cols: 4 + dt: d + data: [0.001844346066628644, -0.01806328724106387, 0.9998351444321376, 0.009085218406426208, + 0.9999385856781032, 0.01095964359935953, -0.00164653704777068, 0.1035287385246496, + -0.01092809496944352, 0.9997767770188613, 0.01808239127209088, 0.002204417022844697, + 0, 0, 0, 1] + +#estimation +estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. +estimate_td: 0 # online estimate time offset between camera and imu +td: -0.02 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock) +estimation_mode: 2 #0:single; 1:solve all; 2: distributed; 3:server +double_counting_common_feature: 0 +not_estimate_first_extrinsic: 1 + +#0:single; 1:solve all; 2: distributed; 3:server + +#optimization parameters +max_solver_time: 0.08 # max solver itration time (ms), to guarantee real time +max_num_iterations: 8 # max solver itrations, to guarantee real time +consensus_max_steps: 1 +timout_wait_sync: 50 +rho_landmark: 1.0 +rho_frame_T: 100 +rho_frame_theta: 100 +relaxation_alpha: 0. +#Note these two param must set to 0 when lazy_broadcast_keyframe=1 +consensus_sync_for_averaging: 0 +consensus_sync_to_start: 0 #Is sync on start of the solving.. +consensus_trigger_time_err_us: 100 + +#depth fusing +depth_far_thres: 3.0 # The max depth in frontend +depth_near_thres: 0.3 +fuse_dep: 0 #if fuse depth measurement +max_depth_to_fuse: 5.0 +min_depth_to_fuse: 0.3 + +#Multi-drone +track_remote_netvlad_thres: 0.6 + +#Initialization +init_method: 0 #0 IMU, 1 PnP +depth_estimate_baseline: 0.05 +add_vel_ba_prior: 0 +enable_sfm_initialization: 0 + +#sliding window +max_sld_win_size: 11 +landmark_estimate_tracks: 4 #when use depth or stereo, 3 is OK. +min_solve_frames: 9 + +#solver +multiple_thread: 1 + +#outlier rejection +thres_outlier : 10.0 +perform_outlier_rejection_num: 10 +tri_max_err: 10000 + +#Marginalization +enable_marginalization: 1 +margin_sparse_solver: 0 +always_fixed_first_pose: 0 +remove_base_when_margin_remote: 2 +# When set to 2, will use the all relevant measurements of the removing frames to compute the prior, +# and the baseFrame (where!=removeID) will not be removed. This may lead to double counting of this baseFrame measurement: but it's stable. +# When set to 1, will remove the baseframe's measurements of those measurements which is not base on current frame. +# set to 0 those measurements (which on a landmark's baseFrame is not been removed) will be ignore. + +#feature tracker parameters +max_cnt: 250 # max feature number in feature tracking +max_superpoint_cnt: 250 # max feature extraction by superpoint +max_solve_cnt: 250 +max_solve_measurements: 10000 +check_essential: 1 +enable_lk_optical_flow: 1 #enable lk opticalflow featuretrack to enhance ego-motion estimation. +remote_min_match_num: 80 +enable_superglue_local: 0 +enable_superglue_remote: 0 +ransacReprojThreshold: 10.0 +enable_search_local_aera: 1 +search_local_max_dist: 0.1 +feature_min_dist: 15 +parallex_thres: 0.04 +knn_match_ratio: 0.9 # This apply to superpoint feature track & loop clouse detection. +track_from_keyframe: 0 +sp_track_use_lk: 1 + +#CNN +cnn_use_onnx: 1 +enable_pca_superpoint: 1 +superpoint_pca_dims: 64 +cnn_enable_tensorrt: 0 +enable_pca_netvlad: 1 +netvlad_pca_dims: 1024 + +acc_n: 6.0e-2 # accelerometer measurement noise standard deviation. +gyr_n: 5.0e-3 # gyroscope measurement noise standard deviation. +acc_w: 8.0e-5 # accelerometer bias random work noise standard deviation. +gyr_w: 3.0e-6 # gyroscope bias random work noise standard deviation. +g_norm: 9.81007 # gravity magnitude + +#Loop Closure Detection +loop_detection_netvlad_thres: 0.6 +enable_homography_test: 0 +loop_inlier_feature_num: 30 +accept_loop_max_yaw: 10 +accept_loop_max_pos: 5.0 +lazy_broadcast_keyframe: 0 # 0: greedy broadcast; 1: compact mode broadcast +nearby_drone_dist: 3.0 +gravity_check_thres: 0.06 +enable_pcm: 1 +pcm_thres: 3.5 + +#PGO +pgo_solver_time: 1.0 +solver_timer_freq: 0.2 +pgo_mode: 1 +pgo_rho_frame_T: 0.01 +pgo_rho_frame_theta: 2.0 +pgo_eta_k: 1.0 +write_g2o: 1 +g2o_output_path: "output.g2o" +write_pgo_to_file: 1 +debug_save_g2o_only: 0 + +#outputs +output_path: "/root/output/" +debug_print_sldwin: 0 +debug_print_states: 0 +enable_perf_output: 0 +print_network_status: 0 +debug_write_margin_matrix: 0 +show_track_id: 0 +write_tracking_image_to_file: 0 \ No newline at end of file diff --git a/config/NTU_VIRAL/NTU_VIRAL_single.yaml b/config/NTU_VIRAL/NTU_VIRAL_single.yaml new file mode 100644 index 00000000..858d0a0f --- /dev/null +++ b/config/NTU_VIRAL/NTU_VIRAL_single.yaml @@ -0,0 +1,168 @@ +%YAML:1.0 + +#inputs +imu: 1 +is_fisheye: 0 +imu_topic: "/imu/imu" +image0_topic: "/left/image_raw" +image1_topic: "/right/image_raw" +is_compressed_images: 1 + +imu_freq: 385 +image_freq: 10 +frame_step: 1 + +#Camera configuration +camera_configuration: 0 #STEREO_PINHOLE = 0, STEREO_FISHEYE = 1, PINHOLE_DEPTH = 2, FOURCORNER_FISHEYE = 3 +num_of_cam: 2 +cam0_calib: "camera_left.yaml" +cam1_calib: "camera_right.yaml" +image_width: 752 +image_height: 480 + +body_T_cam0: !!opencv-matrix + rows: 4 + cols: 4 + dt: d + data: [ 0.02183084, -0.01312053, 0.99967558, 0.00552943, + 0.99975965, 0.00230088, -0.02180248, -0.12431302, + -0.00201407, 0.99991127, 0.01316761, 0.01614686, + 0.00000000, 0.00000000, 0.00000000, 1.00000000 ] +body_T_cam1: !!opencv-matrix + rows: 4 + cols: 4 + dt: d + data: [0.001844346066628644, -0.01806328724106387, 0.9998351444321376, 0.009085218406426208, + 0.9999385856781032, 0.01095964359935953, -0.00164653704777068, 0.1035287385246496, + -0.01092809496944352, 0.9997767770188613, 0.01808239127209088, 0.002204417022844697, + 0, 0, 0, 1] + +#estimation +estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. +estimate_td: 0 # online estimate time offset between camera and imu +td: -0.02 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock) +estimation_mode: 0 #0:single; 1:solve all; 2: distributed; 3:server +double_counting_common_feature: 0 +not_estimate_first_extrinsic: 1 + +#0:single; 1:solve all; 2: distributed; 3:server + +#optimization parameters +max_solver_time: 0.08 # max solver itration time (ms), to guarantee real time +max_num_iterations: 8 # max solver itrations, to guarantee real time +consensus_max_steps: 1 +timout_wait_sync: 50 +rho_landmark: 1.0 +rho_frame_T: 100 +rho_frame_theta: 100 +relaxation_alpha: 0. +#Note these two param must set to 0 when lazy_broadcast_keyframe=1 +consensus_sync_for_averaging: 0 +consensus_sync_to_start: 0 #Is sync on start of the solving.. +consensus_trigger_time_err_us: 100 + +#depth fusing +depth_far_thres: 3.0 # The max depth in frontend +depth_near_thres: 0.3 +fuse_dep: 0 #if fuse depth measurement +max_depth_to_fuse: 5.0 +min_depth_to_fuse: 0.3 + +#Multi-drone +track_remote_netvlad_thres: 0.8 + +#Initialization +init_method: 0 #0 IMU, 1 PnP +depth_estimate_baseline: 0.05 +add_vel_ba_prior: 0 +enable_sfm_initialization: 0 + +#sliding window +max_sld_win_size: 11 +landmark_estimate_tracks: 4 #when use depth or stereo, 3 is OK. +min_solve_frames: 9 + +#solver +multiple_thread: 1 + +#outlier rejection +thres_outlier : 10.0 +perform_outlier_rejection_num: 10 +tri_max_err: 10000 + +#Marginalization +enable_marginalization: 1 +margin_sparse_solver: 0 +always_fixed_first_pose: 0 +remove_base_when_margin_remote: 2 +# When set to 2, will use the all relevant measurements of the removing frames to compute the prior, +# and the baseFrame (where!=removeID) will not be removed. This may lead to double counting of this baseFrame measurement: but it's stable. +# When set to 1, will remove the baseframe's measurements of those measurements which is not base on current frame. +# set to 0 those measurements (which on a landmark's baseFrame is not been removed) will be ignore. + +#feature tracker parameters +max_cnt: 250 # max feature number in feature tracking +max_superpoint_cnt: 250 # max feature extraction by superpoint +max_solve_cnt: 250 +max_solve_measurements: 10000 +check_essential: 1 +enable_lk_optical_flow: 1 #enable lk opticalflow featuretrack to enhance ego-motion estimation. +remote_min_match_num: 40 +enable_superglue_local: 0 +enable_superglue_remote: 0 +ransacReprojThreshold: 10.0 +enable_search_local_aera: 1 +search_local_max_dist: 0.1 +feature_min_dist: 15 +parallex_thres: 0.04 +knn_match_ratio: 0.9 # This apply to superpoint feature track & loop clouse detection. +track_from_keyframe: 0 +sp_track_use_lk: 1 + +#CNN +cnn_use_onnx: 1 +enable_pca_superpoint: 1 +superpoint_pca_dims: 64 +cnn_enable_tensorrt: 0 +enable_pca_netvlad: 1 +netvlad_pca_dims: 1024 + +acc_n: 6.0e-2 # accelerometer measurement noise standard deviation. +gyr_n: 5.0e-3 # gyroscope measurement noise standard deviation. +acc_w: 8.0e-5 # accelerometer bias random work noise standard deviation. +gyr_w: 3.0e-6 # gyroscope bias random work noise standard deviation. +g_norm: 9.81007 # gravity magnitude + +#Loop Closure Detection +loop_detection_netvlad_thres: 0.8 +enable_homography_test: 0 +loop_inlier_feature_num: 30 +accept_loop_max_yaw: 10 +accept_loop_max_pos: 5.0 +lazy_broadcast_keyframe: 0 # 0: greedy broadcast; 1: compact mode broadcast +nearby_drone_dist: 3.0 +gravity_check_thres: 0.06 +enable_pcm: 1 +pcm_thres: 3.5 + +#PGO +pgo_solver_time: 1.0 +solver_timer_freq: 0.2 +pgo_mode: 1 +pgo_rho_frame_T: 0.01 +pgo_rho_frame_theta: 2.0 +pgo_eta_k: 1.0 +write_g2o: 1 +g2o_output_path: "output.g2o" +write_pgo_to_file: 1 +debug_save_g2o_only: 0 + +#outputs +output_path: "/root/output/" +debug_print_sldwin: 0 +debug_print_states: 0 +enable_perf_output: 0 +print_network_status: 0 +debug_write_margin_matrix: 0 +show_track_id: 0 +write_tracking_image_to_file: 0 \ No newline at end of file diff --git a/config/NTU_VIRAL/camera_left.yaml b/config/NTU_VIRAL/camera_left.yaml new file mode 100644 index 00000000..4d9c18b1 --- /dev/null +++ b/config/NTU_VIRAL/camera_left.yaml @@ -0,0 +1,19 @@ +%YAML:1.0 +--- +#camera calibration +model_type: PINHOLE +camera_name: camera +image_width: 752 +image_height: 480 + +# NTU VIRAL datasets +distortion_parameters: + k1: -0.288105327549552 + k2: 0.074578284234601 + p1: 7.784489598138802e-04 + p2: -2.277853975035461e-04 +projection_parameters: + fx: 4.250258563372763e+02 + fy: 4.267976260903337e+02 + cx: 3.860151866550880e+02 + cy: 2.419130336743440e+02 \ No newline at end of file diff --git a/config/NTU_VIRAL/camera_right.yaml b/config/NTU_VIRAL/camera_right.yaml new file mode 100644 index 00000000..28692f8d --- /dev/null +++ b/config/NTU_VIRAL/camera_right.yaml @@ -0,0 +1,19 @@ +%YAML:1.0 +--- +#camera calibration +model_type: PINHOLE +camera_name: camera +image_width: 752 +image_height: 480 + +# NTU VIRAL datasets +distortion_parameters: + k1: -0.300267420221178 + k2: 0.090544063693053 + p1: 3.330220891093334e-05 + p2: 8.989607188457415e-05 +projection_parameters: + fx: 4.313364265799752e+02 + fy: 4.327527965378035e+02 + cx: 3.548956286992647e+02 + cy: 2.325508916495161e+02 \ No newline at end of file diff --git a/config/d2slam.rviz b/config/d2slam.rviz index b9239b3a..241b9210 100644 --- a/config/d2slam.rviz +++ b/config/d2slam.rviz @@ -7,11 +7,12 @@ Panels: - /Global Options1 - /Grid1 - /Axes1 - - /VIOGroup1 - /VIOGroup1/MarginedCloud1/Autocompute Value Bounds1 + - /Remote1 + - /PGO1 - /DenseMapping1/PointCloud21/Autocompute Value Bounds1 Splitter Ratio: 0.5429141521453857 - Tree Height: 413 + Tree Height: 916 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -37,13 +38,14 @@ Panels: Expanded: - /Global Options1 - /VIOGroup1 + - /VIOGroup1/PointCloud1 - /VIOGroup1/MarginedCloud1/Autocompute Value Bounds1 - /VINS-Odom1/Shape1 - /Remote1 - /DenseMapping1/PointCloud21 - /DenseMapping1/PointCloud21/Autocompute Value Bounds1 Splitter Ratio: 0.5 - Tree Height: 413 + Tree Height: 443 Preferences: PromptSaveOnExit: true Toolbars: @@ -124,9 +126,9 @@ Visualization Manager: Position Transformer: XYZ Queue Size: 10 Selectable: true - Size (Pixels): 2 + Size (Pixels): 4 Size (m): 0.03999999910593033 - Style: Boxes + Style: Points Topic: /d2vins/point_cloud Unreliable: false Use Fixed Frame: true @@ -161,13 +163,13 @@ Visualization Manager: Use rainbow: true Value: false - Class: rviz/MarkerArray - Enabled: false + Enabled: true Marker Topic: /d2vins/slding_window Name: SldWin Namespaces: - {} + CameraPoseVisualization: true Queue Size: 100 - Value: false + Value: true - Class: rviz/MarkerArray Enabled: true Marker Topic: /d2vins/camera_visual @@ -354,8 +356,8 @@ Visualization Manager: Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.05000000074505806 + Line Style: Billboards + Line Width: 0.10000000149011612 Name: Path1 Offset: X: 0 @@ -398,7 +400,7 @@ Visualization Manager: Buffer Length: 1 Class: rviz/Path Color: 0; 170; 255 - Enabled: false + Enabled: true Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 @@ -417,12 +419,12 @@ Visualization Manager: Shaft Length: 0.10000000149011612 Topic: /d2vins/path_3 Unreliable: false - Value: false + Value: true - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 85; 255; 255 - Enabled: false + Enabled: true Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 @@ -441,7 +443,7 @@ Visualization Manager: Shaft Length: 0.10000000149011612 Topic: /d2vins/path_4 Unreliable: false - Value: false + Value: true - Alpha: 1 Buffer Length: 1 Class: rviz/Path @@ -891,8 +893,56 @@ Visualization Manager: Topic: /d2pgo/pose_2 Unreliable: false Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /d2pgo/pgo_path_5 + Unreliable: false + Value: true Enabled: true Name: PGO + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /d2pgo/pgo_path_4 + Unreliable: false + Value: true - Class: rviz/Group Displays: - Alpha: 1 @@ -1007,7 +1057,7 @@ Visualization Manager: Views: Current: Class: rviz/XYOrbit - Distance: 2.9188389778137207 + Distance: 352.3196105957031 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -1015,17 +1065,17 @@ Visualization Manager: Value: false Field of View: 0.7853981852531433 Focal Point: - X: -0.5408364534378052 - Y: -1.2800201177597046 - Z: 0.6943288445472717 + X: 54.529605865478516 + Y: -113.03623962402344 + Z: 0.6943408250808716 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 1.0152031183242798 + Pitch: 0.3452037572860718 Target Frame: world - Yaw: 4.636952877044678 + Yaw: 4.141939640045166 Saved: - Class: rviz/XYOrbit Distance: 9.527398109436035 @@ -1050,10 +1100,10 @@ Visualization Manager: Window Geometry: Displays: collapsed: false - Height: 909 + Height: 1022 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 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 + QMainWindow State: 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 Selection: collapsed: false Time: @@ -1062,6 +1112,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1737 - X: 2601 - Y: 273 + Width: 2928 + X: 1376 + Y: 157 diff --git a/config/quadcam/quadcam_depth_gen.yaml b/config/quadcam/quadcam_depth_gen.yaml index 7722e027..4b4c8d8a 100644 --- a/config/quadcam/quadcam_depth_gen.yaml +++ b/config/quadcam/quadcam_depth_gen.yaml @@ -1,7 +1,7 @@ enable_cnn: true cnn_int8: false cnn_fp16: true -cnn_use_tensorrt: true +cnn_use_tensorrt: false # cnn_type: "hitnet" cnn_type: "crestereo" enable_texture: true diff --git a/config/quadcam/quadcam_multi.yaml b/config/quadcam/quadcam_multi.yaml index bb49b050..742c0a56 100644 --- a/config/quadcam/quadcam_multi.yaml +++ b/config/quadcam/quadcam_multi.yaml @@ -32,7 +32,6 @@ estimate_td: 0 # online estimate time offset between camera td: -0.186 # For new datasets with filter estimation_mode: 2 double_counting_common_feature: 0 -min_inv_dep: 0.01 #100 meter away. #optimization parameters max_solver_time: 0.08 # max solver itration time (ms), to guarantee real time diff --git a/config/quadcam/quadcam_multi_rt.yaml b/config/quadcam/quadcam_multi_rt.yaml index c6f1ec2d..0ea7ae6f 100644 --- a/config/quadcam/quadcam_multi_rt.yaml +++ b/config/quadcam/quadcam_multi_rt.yaml @@ -32,7 +32,6 @@ estimate_td: 0 # online estimate time offset between camera td: -0.186 # For new datasets with filter estimation_mode: 2 double_counting_common_feature: 0 -min_inv_dep: 0.01 #100 meter away. #optimization parameters max_solver_time: 0.5 # max solver itration time (ms), to guarantee real time diff --git a/config/quadcam/quadcam_single.yaml b/config/quadcam/quadcam_single.yaml index a237232b..5c7b1431 100644 --- a/config/quadcam/quadcam_single.yaml +++ b/config/quadcam/quadcam_single.yaml @@ -30,7 +30,6 @@ estimate_td: 0 # online estimate time offset between camera td: -0.186 # For new datasets with filter estimation_mode: 0 double_counting_common_feature: 0 -min_inv_dep: 0.01 #100 meter away. #optimization parameters max_solver_time: 0.08 # max solver itration time (ms), to guarantee real time diff --git a/config/quadcam_drone_nxt_tmp/camera_vig_mask_optional/cam_0_vig_mask.png b/config/quadcam_drone_nxt_tmp/camera_vig_mask_optional/cam_0_vig_mask.png new file mode 100644 index 00000000..e12f83d3 Binary files /dev/null and b/config/quadcam_drone_nxt_tmp/camera_vig_mask_optional/cam_0_vig_mask.png differ diff --git a/config/quadcam_drone_nxt_tmp/camera_vig_mask_optional/cam_1_vig_mask.png b/config/quadcam_drone_nxt_tmp/camera_vig_mask_optional/cam_1_vig_mask.png new file mode 100644 index 00000000..64d0d22e Binary files /dev/null and b/config/quadcam_drone_nxt_tmp/camera_vig_mask_optional/cam_1_vig_mask.png differ diff --git a/config/quadcam_drone_nxt_tmp/camera_vig_mask_optional/cam_2_vig_mask.png b/config/quadcam_drone_nxt_tmp/camera_vig_mask_optional/cam_2_vig_mask.png new file mode 100644 index 00000000..4645b323 Binary files /dev/null and b/config/quadcam_drone_nxt_tmp/camera_vig_mask_optional/cam_2_vig_mask.png differ diff --git a/config/quadcam_drone_nxt_tmp/camera_vig_mask_optional/cam_3_vig_mask.png b/config/quadcam_drone_nxt_tmp/camera_vig_mask_optional/cam_3_vig_mask.png new file mode 100644 index 00000000..77d0c5b2 Binary files /dev/null and b/config/quadcam_drone_nxt_tmp/camera_vig_mask_optional/cam_3_vig_mask.png differ diff --git a/config/quadcam_drone_nxt_tmp/fisheye_cams.yaml b/config/quadcam_drone_nxt_tmp/fisheye_cams.yaml new file mode 100755 index 00000000..78c62d20 --- /dev/null +++ b/config/quadcam_drone_nxt_tmp/fisheye_cams.yaml @@ -0,0 +1,195 @@ +cam0: + T_cam_imu: + - - -0.7025841692890046 + - -0.7115830926768211 + - -0.004998727935160485 + - 0.0006875538046129948 + - - -0.01133110800946846 + - 0.018210993559391098 + - -0.999769956392398 + - 0.06280288899684183 + - - 0.7115104293373056 + - -0.7023659031659141 + - -0.020857780662684844 + - -0.10720627808520986 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + cam_overlaps: [] + camera_model: omni + distortion_coeffs: + - -0.21171233219482108 + - 0.4394442689857262 + - 0.001541272532917834 + - -0.0007433853293709857 + distortion_model: radtan + intrinsics: + - 2.0779806782854466 + - 1124.7283061913233 + - 1124.8562955001719 + - 632.7040736742416 + - 327.56651546593287 + resolution: + - 1280 + - 720 + rostopic: CAM_A +cam1: + T_cam_imu: + - - -0.7143937678024522 + - 0.699725731351554 + - 0.005044344313148541 + - 0.004775806338207317 + - - 0.004778082299049191 + - 0.012086664951125162 + - -0.9999155376630086 + - 0.06156095258157772 + - - -0.6997276001806418 + - -0.7143093261430183 + - -0.011977985245760256 + - -0.1015738178235088 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + T_cn_cnm1: + - - 0.0009404731192081406 + - 0.01533420311835991 + - -0.9998819818983816 + - -0.10146345645215102 + - - -0.006958486419914336 + - 0.9998583167080346 + - 0.015327295146830304 + - 0.0008909327534145013 + - - 0.9999753471846465 + - 0.006943250283481162 + - 0.0010470427140399363 + - -0.10089598680976977 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + cam_overlaps: [] + camera_model: omni + distortion_coeffs: + - -0.19999658420274455 + - 0.4681180797506174 + - 0.00045773646975705155 + - 0.0009007846998110019 + distortion_model: radtan + intrinsics: + - 2.103193105300709 + - 1140.8131521173325 + - 1140.970469458345 + - 650.1924953551246 + - 357.6475196274094 + resolution: + - 1280 + - 720 + rostopic: CAM_B +cam2: + T_cam_imu: + - - 0.7013917570433141 + - 0.7127723462367052 + - 0.0022771894773128687 + - -0.01229576112095501 + - - 0.002055138880798113 + - 0.0011725000792903173 + - -0.9999972008199549 + - 0.03671315643205333 + - - -0.7127730210634193 + - 0.7013944736621376 + - -0.00064246427116188 + - -0.0987582372651908 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + T_cn_cnm1: + - - 0.005914667701575005 + - 0.00970340516319522 + - -0.9999354282323567 + - -0.10080778969408415 + - - -0.0018637195266161663 + - 0.9999512894701966 + - 0.009692535087623506 + - 0.001534714038025603 + - - 0.9999807714428881 + - 0.0018062710587227852 + - 0.005932464020654948 + - -0.10174891610997806 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + cam_overlaps: [] + camera_model: omni + distortion_coeffs: + - -0.2172065915345015 + - 0.3857127748422569 + - -0.00023293530548314231 + - 0.0004265861996505244 + distortion_model: radtan + intrinsics: + - 2.0281931420292127 + - 1105.501457313875 + - 1105.83326888899 + - 670.3049308503298 + - 363.92986422446916 + resolution: + - 1280 + - 720 + rostopic: CAM_C +cam3: + T_cam_imu: + - - 0.7132127724745928 + - -0.7008870712757387 + - -0.00921165010311692 + - -0.011961511234085686 + - - -0.007851728742378883 + - 0.005152457214608108 + - -0.999955900297812 + - 0.060931012323222414 + - - 0.7009036249976601 + - 0.7132536473816093 + - -0.0018283768676101708 + - -0.11148231776218376 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + T_cn_cnm1: + - - -0.002869983309342017 + - 0.006623402709936979 + - -0.9999739465267751 + - -0.10252374664672882 + - - -0.003415150594520594 + - 0.9999721683637837 + - 0.006633192612713534 + - 0.0007542059623751768 + - - 0.9999900499216022 + - 0.0034340987700721796 + - -0.0028472835153079967 + - -0.10091019114171863 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + cam_overlaps: [] + camera_model: omni + distortion_coeffs: + - -0.1918092965024199 + - 0.49010329824874016 + - 0.0007743900323585036 + - 4.965097579945239e-05 + distortion_model: radtan + intrinsics: + - 2.118109081741645 + - 1147.7516125011268 + - 1148.335856169043 + - 644.80217848585 + - 342.1810641611838 + resolution: + - 1280 + - 720 + rostopic: CAM_D diff --git a/config/quadcam_drone_nxt_tmp/quadcam_depth.yaml b/config/quadcam_drone_nxt_tmp/quadcam_depth.yaml new file mode 100644 index 00000000..318c00b4 --- /dev/null +++ b/config/quadcam_drone_nxt_tmp/quadcam_depth.yaml @@ -0,0 +1,69 @@ +enable_cnn: true +cnn_int8: false +cnn_fp16: true +cnn_use_tensorrt: true +cnn_type: "hitnet" +# cnn_type: "crestereo" +onnx_path: "/root/swarm_ws/src/D2SLAM/models/hitnet_series/hitnet_1x240x320_model_float16_quant_opt.onnx" +cnn_quant_path: "" +trt_engine_path: "/root/swarm_ws/src/D2SLAM/models/hitnet_series/hitnet_1x240x320_model_float16_quant_opt.trt" +cnn_input_rgb : false +enable_texture: false +width: 320 +height: 240 +pixel_step: 2 +image_step: 3 +max_z: 10 +min_z: 0.1 +cam_calib_file_path: "/root/swarm_ws/src/D2SLAM/config/quadcam_drone_nxt_tmp/fisheye_cams.yaml" +fov: 190 +fps: 10 + +image_topic: "/oak_ffc_4p/assemble_image" +image_type: "raw" + +drone_pose_topic: "/d2vins/imu_propagation" + +use_occ_map: true + +photometric_calib_numbers: 4 +#TODO: refine this process to make vig calibration more easy to read +photometric_calib_path: "/root/swarm_ws/src/D2SLAM/config/quadcam_drone_nxt_tmp/camera_vig_mask_optional" +# photometric_calib_path: "" +# photometric_calib: "mask.png" +photometric_calib: "" +avg_brightness: 0.7 +stereos: + stereo1_0: + cam_idx_l: 0 + cam_idx_r: 1 + idx_l: 1 + idx_r: 0 + stereo_config: "/root/swarm_ws/src/D2SLAM/config/quadcam_drone_nxt_tmp/stereo_calib_0_1_240_320.yaml" + pointcloud_topic: "quaddepth/pointcloud_0" + cam_pos_topic: "quaddepth/cam_pos_0" + stereo2_1: + cam_idx_l: 1 + cam_idx_r: 2 + idx_l: 1 + idx_r: 0 + stereo_config: "/root/swarm_ws/src/D2SLAM/config/quadcam_drone_nxt_tmp/stereo_calib_1_2_240_320.yaml" + pointcloud_topic: "quaddepth/pointcloud_1" + cam_pos_topic: "quaddepth/cam_pos_1" + stereo3_2: + cam_idx_l: 2 + cam_idx_r: 3 + idx_l: 1 + idx_r: 0 + stereo_config: "/root/swarm_ws/src/D2SLAM/config/quadcam_drone_nxt_tmp/stereo_calib_2_3_240_320.yaml" + pointcloud_topic: "quaddepth/pointcloud_2" + cam_pos_topic: "quaddepth/cam_pos_2" + stereo0_3: + cam_idx_l: 3 + cam_idx_r: 0 + idx_l: 1 + idx_r: 0 + stereo_config: "/root/swarm_ws/src/D2SLAM/config/quadcam_drone_nxt_tmp/stereo_calib_3_0_240_320.yaml" + pointcloud_topic: "quaddepth/pointcloud_3" + cam_pos_topic: "quaddepth/cam_pos_3" +camera_configuration: 3 \ No newline at end of file diff --git a/config/quadcam_drone_nxt_tmp/quadcam_single.yaml b/config/quadcam_drone_nxt_tmp/quadcam_single.yaml new file mode 100644 index 00000000..52d437b8 --- /dev/null +++ b/config/quadcam_drone_nxt_tmp/quadcam_single.yaml @@ -0,0 +1,192 @@ +%YAML:1.0 + +#inputs +imu: 1 +imu_topic: "/mavros/imu/data_raw" +# imu_topic: "/dji_sdk_1/dji_sdk/imu" +# image0_topic: "/arducam/image" +image0_topic: "/oak_ffc_4p/assemble_image" + +is_compressed_images: 0 + +imu_freq: 500 +image_freq: 20 +frame_step: 3 + +#Camera configuration +camera_configuration: 3 #STEREO_PINHOLE = 0, STEREO_FISHEYE = 1, PINHOLE_DEPTH = 2, FOURCORNER_FISHEYE = 3 +calib_file_path: "fisheye_cams.yaml" +extrinsic_parameter_type: 0 # 0: cam^T_imu 1: imu^T_cam //NXT use 0, D2SLAM dataset use 1 +image_width: 1280 +image_height: 720 +enable_undistort_image: 1 +undistort_fov: 180 +width_undistort: 320 #600 +height_undistort: 160 #300 +photometric_calib: "" +avg_photometric: 0.7 +sp_track_use_lk: 1 +image_queue_size : 10 + + +#frontend frequency +loop_detection_freq: 1 +lcm_thread_freq: 1 + +#estimation +not_estimate_first_extrinsic: 0 +estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. +estimate_td: 1 # online estimate time offset between camera and imu +td: -0.05 # For new datasets with filter +estimation_mode: 0 +double_counting_common_feature: 0 +min_inv_dep: 0.01 #100 meter away. + +#optimization parameters +max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time 0.04 normal running +max_num_iterations: 8 # max solver itrations, to guarantee real time +consensus_max_steps: 4 +timout_wait_sync: 100 +rho_landmark: 1.0 +rho_frame_T: 100 +rho_frame_theta: 100 +relaxation_alpha: 0. +consensus_sync_for_averaging: 0 +consensus_sync_to_start: 0 #Is sync on start of the solving.. +consensus_trigger_time_err_us: 100 + + +#depth fusing +depth_far_thres: 3.0 # The max depth in frontend +depth_near_thres: 0.3 +fuse_dep: 0 #if fuse depth measurement +max_depth_to_fuse: 5.0 +min_depth_to_fuse: 0.3 + +#Multi-drone +track_remote_netvlad_thres: 0.5 + +#Initialization +init_method: 0 #0 IMU, 1 PnP +depth_estimate_baseline: 0.03 + +#sliding window +max_sld_win_size: 10 +landmark_estimate_tracks: 4 #when use depth or stereo, 3 is OK. +min_solve_frames: 6 + +#solver +multiple_thread: 4 + +#outlier rejection +thres_outlier : 10.0 +perform_outlier_rejection_num: 10000 +tri_max_err: 0.2 + +#Marginalization +enable_marginalization: 1 +margin_sparse_solver: 1 +always_fixed_first_pose: 0 +remove_base_when_margin_remote: 2 +# When set to 2, will use the all relevant measurements of the removing frames to compute the prior, +# and the baseFrame (where!=removeID) will not be removed. This may lead to double counting of this baseFrame measurement: but it's stable. +# When set to 1, will remove the baseframe's measurements of those measurements which is not base on current frame. +# set to 0 those measurements (which on a landmark's baseFrame is not been removed) will be ignore. + +#feature tracker parameters +max_cnt: 50 # max feature number in feature tracking +max_superpoint_cnt: 50 # max feature extraction by superpoint //oringal super 150 +max_solve_cnt: 200 +check_essential: 0 +enable_lk_optical_flow: 1 #enable lk opticalflow featuretrack to enhance ego-motion estimation. + +# dynamic_superpoint: 1 # dynmiac modify the threshold of superpoint +# dynamic_superpoint_thres: 0.7 # next time extract 0.7*best_score points +# dynamic_superpoint_min_thres: 0.1 # at least extract dynamic_superpoint_min_thres*max_cnt points, if can not meet, degree frame +1 +# dynamic_superpoint_degree_frames: 10 # if 10 frames do not have enough features, then dynamic modify the threshold to current_threadhold * degree_percent +# dynamic_superpoint_degree_percent: 0.5 # next time extract 0.1*best_score points + +use_gpu_feature_tracking: 0 # 1 use gpu, 0 use cpu +use_gpu_good_feature_extraction: 0 # 1 use gpu, 0 use cpu + +remote_min_match_num: 20 +enable_superglue_local: 0 +enable_superglue_remote: 0 +ransacReprojThreshold: 10.0 +enable_search_local_aera: 1 +search_local_max_dist: 0.05 +parallex_thres: 0.006 #original 0.012 +knn_match_ratio: 0.8 #This apply to superpoint feature track & loop clouse detection. + +#CNN +cnn_engine: 0 # 0 use onnx; 1 use tensorrt raw engine +cnn_use_onnx: 1 + +nn_engine_type: 1 # 0 use onnx; 1 use tensorrt raw engine +superpoint_trt_path: "/root/swarm_ws/src/D2SLAM/models/superpoint_v1_dyn_size_onnx_200_300.trt" +moblieNetVlad_trt_path: "/root/swarm_ws/src/D2SLAM/models/mobilenetvlad_dyn_size_onnx_200_300.trt" + +#Superpoint +superpoint_config: + onnx_path: "/root/swarm_ws/src/D2SLAM/models/superpoint_series/superpoint_v1_sim_int32.onnx" + trt_engine_path: "/root/swarm_ws/src/D2SLAM/models/superpoint_series/superpoint_v1_sim_int32.trt" + enable_fp16: 1 + input_width: 320 + input_height: 160 + input_tensor_names: + - "input" + output_tensor_names: + - "scores" # keypoint output + - "descriptors" # descriptor output + # these two parameters sequence must follow the given output sequence. + threshold: 0.15 + nms_radius: 20 + enable_pca: 1 + pca_mean_path: "/root/swarm_ws/src/D2SLAM/models/superpoint_series/mean_.csv" + pca_comp_path: "/root/swarm_ws/src/D2SLAM/models/superpoint_series/components_.csv" + superpoint_pca_dims: 64 + + + + +#xuhao quadcam +acc_n: 0.1 # accelerometer measurement noise standard deviation. #0.2 0.04 +gyr_n: 0.05 # gyroscope measurement noise standard deviation. #0.05 0.004 +acc_w: 0.002 # accelerometer bias random work noise standard deviation. #0.002 +gyr_w: 0.0004 # gyroscope bias random work noise standard deviation. #4.0e-5 +g_norm: 9.805 # gravity magnitude + +# X +# acc_n: 0.0018445838609014217 # accelerometer measurement noise standard deviation. #0.2 0.04 +# gyr_n: 0.0001606421844332533 # gyroscope measurement noise standard deviation. #0.05 0.004 +# acc_w: 9.637244490647607e-05 # accelerometer bias random work noise standard deviation. #0.002 +# gyr_w: 6.091456992295194e-06 # gyroscope bias random work noise standard deviation. #4.0e-5 +# g_norm: 9.805 # gravity magnitude + +#Loop Closure Detection +loop_detection_netvlad_thres: 0.8 +enable_homography_test: 1 +accept_loop_max_yaw: 10 +accept_loop_max_pos: 1.0 +loop_inlier_feature_num: 50 +gravity_check_thres: 0.03 + +#PGO +pgo_solver_time: 0.5 +pgo_mode: 0 +write_g2o: 0 +g2o_output_path: "output.g2o" +pgo_solver_time: 1.0 +solver_timer_freq: 1.0 +enable_pcm: 1 +pcm_thres: 2.8 + +#outputs +output_path: "/root/output/" +debug_print_sldwin: 0 +debug_print_states: 0 +enable_perf_output: 0 +enbale_detailed_output: 0 # output feature extraction time, feature tracking time, feature matching time +enbale_speed_ouptut: 1 # output Frontend use time, backedn use time +debug_write_margin_matrix: 0 +show_track_id: 0 \ No newline at end of file diff --git a/config/quadcam_drone_nxt_tmp/stereo_calib_0_1_240_320.yaml b/config/quadcam_drone_nxt_tmp/stereo_calib_0_1_240_320.yaml new file mode 100755 index 00000000..0d16f82f --- /dev/null +++ b/config/quadcam_drone_nxt_tmp/stereo_calib_0_1_240_320.yaml @@ -0,0 +1,21 @@ +cam0: + cam_overlaps: [1] + camera_model: pinhole + distortion_coeffs: [0.0010044905592098258, -0.0009291093880756094, 0.0010600518842032187, -0.0002989831433127006] + distortion_model: radtan + intrinsics: [134.27259542052585, 134.20880922638906, 159.90439595299992, 120.72456205516177] + resolution: [320, 240] + rostopic: /cam_0_1/compressed +cam1: + T_cn_cnm1: + - [0.9999105571385035, 0.013355655948125272, 0.0007100543334215128, -0.14300993326557723] + - [-0.013359968004622623, 0.9998898752053185, 0.0064613246940297, -0.001902787851313964] + - [-0.0006236809092512477, -0.0064702330778360845, 0.9999788733298496, -0.00479275919269893] + - [0.0, 0.0, 0.0, 1.0] + cam_overlaps: [0] + camera_model: pinhole + distortion_coeffs: [-0.0018856337989545443, -0.0010698565116146776, 0.003260837411637211, 0.0046281512329318176] + distortion_model: radtan + intrinsics: [132.9146156010627, 133.81938724700305, 161.3876733214489, 121.61867817770249] + resolution: [320, 240] + rostopic: /cam_1_0/compressed diff --git a/config/quadcam_drone_nxt_tmp/stereo_calib_1_2_240_320.yaml b/config/quadcam_drone_nxt_tmp/stereo_calib_1_2_240_320.yaml new file mode 100755 index 00000000..cc013c5d --- /dev/null +++ b/config/quadcam_drone_nxt_tmp/stereo_calib_1_2_240_320.yaml @@ -0,0 +1,21 @@ +cam0: + cam_overlaps: [1] + camera_model: pinhole + distortion_coeffs: [0.0019542539478918674, -9.355711747726592e-05, 1.154215637877751e-05, 0.006797887500515746] + distortion_model: radtan + intrinsics: [134.52045558312344, 133.39188339891385, 162.41885659215507, 119.626688495879] + resolution: [320, 240] + rostopic: /cam_1_1/compressed +cam1: + T_cn_cnm1: + - [0.9999709664150888, 0.007021736627484321, 0.002959990135168101, -0.13952037596411238] + - [-0.007020392890547193, 0.9999752490351698, -0.0004641128149688724, -0.0033637822158709397] + - [-0.002963175750508531, 0.00044331904640902833, 0.9999955115187704, -0.0018206824835311523] + - [0.0, 0.0, 0.0, 1.0] + cam_overlaps: [0] + camera_model: pinhole + distortion_coeffs: [-0.002712663901606837, -0.000252918982184175, 0.0010986933639378167, 0.0006106766515577222] + distortion_model: radtan + intrinsics: [133.3927909133752, 133.4028567515827, 160.24163180407845, 120.58500862733793] + resolution: [320, 240] + rostopic: /cam_2_0/compressed diff --git a/config/quadcam_drone_nxt_tmp/stereo_calib_2_3_240_320.yaml b/config/quadcam_drone_nxt_tmp/stereo_calib_2_3_240_320.yaml new file mode 100755 index 00000000..956e4dcf --- /dev/null +++ b/config/quadcam_drone_nxt_tmp/stereo_calib_2_3_240_320.yaml @@ -0,0 +1,21 @@ +cam0: + cam_overlaps: [1] + camera_model: pinhole + distortion_coeffs: [-0.0008479597353304228, -0.0007848243525118119, 0.00045974167369074306, -0.00012844786834216817] + distortion_model: radtan + intrinsics: [133.46517544226054, 133.43476474020883, 159.7854620010016, 120.19249011199906] + resolution: [320, 240] + rostopic: /cam_2_1/compressed +cam1: + T_cn_cnm1: + - [0.9999639952740408, 0.007001466183352844, -0.004794541360175011, -0.14310493333504393] + - [-0.0069907056220526705, 0.9999730166605102, 0.002257429019448259, 0.0014242402282056753] + - [0.004810217300378795, -0.00222383051409331, 0.9999859580950928, 0.0017913783006894971] + - [0.0, 0.0, 0.0, 1.0] + cam_overlaps: [0] + camera_model: pinhole + distortion_coeffs: [-0.0002818938899710855, 0.00027288932548441146, 4.352662750665867e-05, -0.001065676012543514] + distortion_model: radtan + intrinsics: [133.91458060005104, 133.9298590556437, 159.53662251667342, 119.9308597477711] + resolution: [320, 240] + rostopic: /cam_3_0/compressed diff --git a/config/quadcam_drone_nxt_tmp/stereo_calib_3_0_240_320.yaml b/config/quadcam_drone_nxt_tmp/stereo_calib_3_0_240_320.yaml new file mode 100755 index 00000000..b6453d0e --- /dev/null +++ b/config/quadcam_drone_nxt_tmp/stereo_calib_3_0_240_320.yaml @@ -0,0 +1,21 @@ +cam0: + cam_overlaps: [1] + camera_model: pinhole + distortion_coeffs: [-0.0004758881305727118, -0.00033395787982904476, -0.000345158131252896, -0.00018116480498871267] + distortion_model: radtan + intrinsics: [134.04399646183631, 134.02783378213667, 159.95794651384972, 119.73104765087855] + resolution: [320, 240] + rostopic: /cam_3_1/compressed +cam1: + T_cn_cnm1: + - [0.9999175601933362, 0.012835174207506028, 0.00036210504893680306, -0.1439003012879887] + - [-0.012833728330503266, 0.9999106244479907, -0.003746803059380803, 0.0017934832369789619] + - [-0.0004101635555865386, 0.003741847015835826, 0.9999929151482851, 0.0010262859572919445] + - [0.0, 0.0, 0.0, 1.0] + cam_overlaps: [0] + camera_model: pinhole + distortion_coeffs: [-0.00037122713496759576, -0.00013606750385260837, -0.0002597458004832687, -4.8864941940957164e-05] + distortion_model: radtan + intrinsics: [133.89470350496566, 133.9785001865875, 160.02805311502328, 119.7218520585046] + resolution: [320, 240] + rostopic: /cam_0_0/compressed diff --git a/config/realsense_d435/d435_multi.yaml b/config/realsense_d435/d435_multi.yaml index bc77899b..d5a4eaf7 100644 --- a/config/realsense_d435/d435_multi.yaml +++ b/config/realsense_d435/d435_multi.yaml @@ -47,7 +47,6 @@ estimate_td: 0 # online estimate time offset between camera td: 0.001 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock) estimation_mode: 2 #0:single; 1:solve all; 2: distributed; 3:server double_counting_common_feature: 1 -min_inv_dep: 0.01 #100 meter away. #optimization parameters diff --git a/config/realsense_d435/d435_multi_rt.yaml b/config/realsense_d435/d435_multi_rt.yaml index bab37f3c..f2fea777 100644 --- a/config/realsense_d435/d435_multi_rt.yaml +++ b/config/realsense_d435/d435_multi_rt.yaml @@ -45,7 +45,6 @@ estimate_td: 0 # online estimate time offset between camera td: 0.001 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock) estimation_mode: 2 #0:single; 1:solve all; 2: distributed; 3:server double_counting_common_feature: 0 -min_inv_dep: 0.01 #100 meter away. min_measurements_per_keyframe: 20 #optimization parameters diff --git a/config/realsense_d435/d435_single.yaml b/config/realsense_d435/d435_single.yaml index 25ec8e4a..96cd5cb2 100644 --- a/config/realsense_d435/d435_single.yaml +++ b/config/realsense_d435/d435_single.yaml @@ -48,7 +48,6 @@ estimate_td: 0 # online estimate time offset between camera td: 0.001 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock) estimation_mode: 0 #0:single; 1:solve all; 2: distributed; 3:server double_counting_common_feature: 1 -min_inv_dep: 0.01 #100 meter away. #optimization parameters diff --git a/config/tum/tum_multi.yaml b/config/tum/tum_multi.yaml index f1a685d6..7919d641 100644 --- a/config/tum/tum_multi.yaml +++ b/config/tum/tum_multi.yaml @@ -42,7 +42,6 @@ estimate_td: 0 # online estimate time offset between camera td: 0.00 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock) estimation_mode: 2 #0:single; 1:solve all; 2: distributed; 3:server double_counting_common_feature: 0 -min_inv_dep: 0.05 #10 meter away. #0:single; 1:solve all; 2: distributed; 3:server #optimization parameters diff --git a/config/tum/tum_single.yaml b/config/tum/tum_single.yaml index e86410c4..77e69c1b 100644 --- a/config/tum/tum_single.yaml +++ b/config/tum/tum_single.yaml @@ -43,7 +43,6 @@ estimate_td: 0 # online estimate time offset between camera td: 0.001 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock) estimation_mode: 0 #0:single; 1:solve all; 2: distributed; 3:server double_counting_common_feature: 1 -min_inv_dep: 0.1 #10 meter away. #optimization parameters max_solver_time: 0.08 # max solver itration time (ms), to guarantee real time @@ -70,7 +69,8 @@ track_remote_netvlad_thres: 0.5 #Initialization init_method: 0 #0 IMU, 1 PnP -depth_estimate_baseline: 0.05 +depth_estimate_baseline: -0.05 +enable_sfm_initialization: 0 #sliding window max_sld_win_size: 11 @@ -106,7 +106,8 @@ enable_lk_optical_flow: 1 #enable lk opticalflow featuretrack to enhance ego-mot remote_min_match_num: 40 ransacReprojThreshold: 10.0 parallex_thres: 0.022 -knn_match_ratio: 0.8 #This apply to superpoint feature track & loop clouse detection. +knn_match_ratio: 0.9 #This apply to superpoint feature track & loop clouse detection. +track_from_keyframe: 0 #CNN cnn_use_onnx: 1 @@ -129,7 +130,7 @@ loop_detection_netvlad_thres: 0.8 enable_homography_test: 0 accept_loop_max_yaw: 10 accept_loop_max_pos: 1.0 -loop_inlier_feature_num: 50 +loop_inlier_feature_num: 40 gravity_check_thres: 0.03 pgo_solver_time: 1.0 solver_timer_freq: 1.0 @@ -138,10 +139,19 @@ pcm_thres: 2.8 #PGO -pgo_solver_time: 0.5 +pgo_solver_time: 1.0 +solver_timer_freq: 0.2 +pgo_mode: 1 +pgo_rho_frame_T: 0.01 +pgo_rho_frame_theta: 2.0 +pgo_eta_k: 1.0 +write_g2o: 1 +g2o_output_path: "output.g2o" +write_pgo_to_file: 1 +debug_save_g2o_only: 0 #outputs -output_path: "/root/output/" +output_path: "/home/xuhao/output/" debug_print_sldwin: 0 debug_print_states: 0 enable_perf_output: 0 diff --git a/d2comm/src/d2comm.cpp b/d2comm/src/d2comm.cpp index 3c94fec8..6d161a54 100644 --- a/d2comm/src/d2comm.cpp +++ b/d2comm/src/d2comm.cpp @@ -1,45 +1,47 @@ #include "d2comm.h" namespace D2Comm { -void D2Comm::init(ros::NodeHandle & nh) { - std::string lcm_uri; - nh.param("lcm_uri", lcm_uri, "udpm://224.0.0.251:7667?ttl=1"); - nh.param("self_id", self_id, 0); - printf("[D2Comm] Try to initialize LCM URI: %s\n", lcm_uri.c_str()); - lcm = new lcm::LCM(lcm_uri); - if (!lcm->good()) { - ROS_ERROR("D2Comm: Failed to initialize LCM."); - return; +void D2Comm::init(ros::NodeHandle& nh) { + std::string lcm_uri; + nh.param("lcm_uri", lcm_uri, "udpm://224.0.0.251:7667?ttl=1"); + nh.param("self_id", self_id, 0); + printf("[D2Comm] Try to initialize LCM URI: %s\n", lcm_uri.c_str()); + lcm = new lcm::LCM(lcm_uri); + if (!lcm->good()) { + ROS_ERROR("D2Comm: Failed to initialize LCM."); + return; + } + lcm->subscribe("PGO_Sync_Data", &D2Comm::PGODataLCMCallback, this); + pgo_data_pub = nh.advertise("/d2pgo/pgo_data", 1); + pgo_data_sub = nh.subscribe("/d2pgo/pgo_data", 1, &D2Comm::PGODataRosCallback, + this, ros::TransportHints().tcpNoDelay()); + th = std::thread([&] { + ROS_INFO("[D2Comm] Starting d2comm lcm thread."); + while (0 == this->lcmHandle()) { } - lcm->subscribe("PGO_Sync_Data", &D2Comm::PGODataLCMCallback, this); - pgo_data_pub = nh.advertise("/d2pgo/pgo_data", 1); - pgo_data_sub = nh.subscribe("/d2pgo/pgo_data", 1, &D2Comm::PGODataRosCallback, this, ros::TransportHints().tcpNoDelay()); - th = std::thread([&] { - ROS_INFO("[D2Comm] Starting d2comm lcm thread."); - while(0 == this->lcmHandle()) { - } - }); - ROS_INFO("[D2Comm] Drone %d ready.", self_id); + }); + ROS_INFO("[D2Comm] Drone %d ready.", self_id); } void D2Comm::PGODataLCMCallback(const lcm::ReceiveBuffer* rbuf, - const std::string& chan, - const DistributedPGOData_t * msg) { - if (msg->drone_id == self_id) { - return; - } - printf("[D2Comm] Received PGO_Sync_Data from drone %d\n", msg->drone_id); - D2Common::DPGOData data(*msg); - pgo_data_pub.publish(data.toROS()); + const std::string& chan, + const DistributedPGOData_t* msg) { + if (msg->drone_id == self_id) { + return; + } + printf("[D2Comm] Received PGO_Sync_Data from drone %d\n", msg->drone_id); + D2Common::DPGOData data(*msg); + pgo_data_pub.publish(data.toROS()); } -void D2Comm::PGODataRosCallback(const swarm_msgs::DPGOData & ros_data) { - if (ros_data.drone_id != self_id) { - return; - } - D2Common::DPGOData data(ros_data); - auto lcm_data = data.toLCM(); - printf("[D2Comm] Broadcast PGO data of drone %d, lcm %d bytes.\n", ros_data.drone_id, lcm_data.getEncodedSize()); - fflush(stdout); - lcm->publish("PGO_Sync_Data", &lcm_data); +void D2Comm::PGODataRosCallback(const swarm_msgs::DPGOData& ros_data) { + if (ros_data.drone_id != self_id) { + return; + } + D2Common::DPGOData data(ros_data); + auto lcm_data = data.toLCM(); + printf("[D2Comm] Broadcast PGO data of drone %d, lcm %d bytes.\n", + ros_data.drone_id, lcm_data.getEncodedSize()); + fflush(stdout); + lcm->publish("PGO_Sync_Data", &lcm_data); } -} \ No newline at end of file +} // namespace D2Comm \ No newline at end of file diff --git a/d2comm/src/d2comm_node.cpp b/d2comm/src/d2comm_node.cpp index 1ad4e4d4..0f0bc2a5 100644 --- a/d2comm/src/d2comm_node.cpp +++ b/d2comm/src/d2comm_node.cpp @@ -1,13 +1,12 @@ #include "d2comm.h" -int main(int argc, char **argv) -{ - printf("D2Comm starting....\n"); - ros::init(argc, argv, "d2comm"); - ros::NodeHandle n("~"); - D2Comm::D2Comm d2comm; - d2comm.init(n); - ros::MultiThreadedSpinner spinner(4); - spinner.spin(); - return 0; +int main(int argc, char **argv) { + printf("D2Comm starting....\n"); + ros::init(argc, argv, "d2comm"); + ros::NodeHandle n("~"); + D2Comm::D2Comm d2comm; + d2comm.init(n); + ros::MultiThreadedSpinner spinner(4); + spinner.spin(); + return 0; } \ No newline at end of file diff --git a/d2common/include/d2common/d2basetypes.h b/d2common/include/d2common/d2basetypes.h index c53fc119..9124edaf 100644 --- a/d2common/include/d2common/d2basetypes.h +++ b/d2common/include/d2common/d2basetypes.h @@ -27,6 +27,13 @@ enum PGO_MODE { PGO_MODE_DISTRIBUTED_AROCK }; +enum ESTIMATION_MODE { + SINGLE_DRONE_MODE, //Not accept remote frame + SOLVE_ALL_MODE, //Each drone solve all the information + DISTRIBUTED_CAMERA_CONSENUS, //Distributed camera consensus + SERVER_MODE //In this mode receive all remote and solve them +}; + enum CameraConfig{ STEREO_PINHOLE = 0, STEREO_FISHEYE = 1, diff --git a/d2common/include/d2common/d2frontend_types.h b/d2common/include/d2common/d2frontend_types.h index 2aefb901..cbf92d51 100644 --- a/d2common/include/d2common/d2frontend_types.h +++ b/d2common/include/d2common/d2frontend_types.h @@ -335,6 +335,12 @@ struct VisualImageDesc { } } + void clearLandmarks() { + landmarks.clear(); + landmark_descriptor.clear(); + landmark_scores.clear(); + } + }; struct VisualImageDescArray { @@ -356,6 +362,7 @@ struct VisualImageDescArray { double cur_td = 0; Swarm::Pose motion_prediction; bool send_to_backend = true; //If send to backend + bool is_stereo = false; void sync_landmark_ids() { for (auto & image : images) { diff --git a/d2common/include/d2common/d2landmarks.h b/d2common/include/d2common/d2landmarks.h index 6e51c9c6..05da7f3c 100644 --- a/d2common/include/d2common/d2landmarks.h +++ b/d2common/include/d2common/d2landmarks.h @@ -261,6 +261,25 @@ struct LandmarkPerId { } return cam_set.size() > 1; } + + LandmarkPerFrame at(FrameIdType frame_id) const { + for (auto & it: track) { + if (it.frame_id == frame_id) { + return it; + } + } + printf("LandmarkPerId::at: Error, cannot find frame_id %d\n", frame_id); + return LandmarkPerFrame(); + } + + bool HasFrame(FrameIdType frame_id) const { + for (auto & it: track) { + if (it.frame_id == frame_id) { + return true; + } + } + return false; + } }; } \ No newline at end of file diff --git a/d2common/include/d2common/d2vinsframe.h b/d2common/include/d2common/d2vinsframe.h index f87116dd..cd0be8d2 100644 --- a/d2common/include/d2common/d2vinsframe.h +++ b/d2common/include/d2common/d2vinsframe.h @@ -12,7 +12,7 @@ struct VINSFrame: public D2BaseFrame { Vector3d Ba; // bias of acc Vector3d Bg; //bias of gyro FrameIdType prev_frame_id = -1; - IntegrationBase * pre_integrations = nullptr; + IntegrationBase * pre_integrations = nullptr; // From prev to this int imu_buf_index = 0; VINSFrame():Ba(0., 0., 0.), Bg(0., 0., 0.) {} diff --git a/d2common/include/d2common/solver/BaseParamResInfo.hpp b/d2common/include/d2common/solver/BaseParamResInfo.hpp index 0224bcd2..f3f60142 100644 --- a/d2common/include/d2common/solver/BaseParamResInfo.hpp +++ b/d2common/include/d2common/solver/BaseParamResInfo.hpp @@ -43,11 +43,11 @@ enum ResidualType { LandmarkTwoFrameTwoCamResidual, // 3 LandmarkTwoDroneTwoCamResidual, // 4 LandmarkOneFrameTwoCamResidual, // 5 - PriorResidual, // 7 - DepthResidual, // 8 - RelPoseResidual, //9 - RelRotResidual, //10 - GravityPriorResidual //11 + PriorResidual, // 6 + DepthResidual, // 7 + RelPoseResidual, //8 + RelRotResidual, //9 + GravityPriorResidual //10 }; class ResidualInfo { diff --git a/d2common/src/d2imu.cpp b/d2common/src/d2imu.cpp index 849e7400..5b94ad8d 100644 --- a/d2common/src/d2imu.cpp +++ b/d2common/src/d2imu.cpp @@ -1,165 +1,167 @@ #include #include #include +#include + namespace D2Common { Vector3d IMUData::Gravity = Vector3d(0., 0., 9.805); -Eigen::Matrix IntegrationBase::noise = Eigen::Matrix::Zero(); +Eigen::Matrix IntegrationBase::noise = + Eigen::Matrix::Zero(); size_t IMUBuffer::searchClosest(double t) const { - const Guard lock(buf_lock); - if (buf.size() == 0) { - printf("IMUBuffer::searchClosest: empty buffer\n"); - return 0; - } - if (buf.size() == 1) { - return 0; - } - return searchClosest(t, 0, buf.size()); + const Guard lock(buf_lock); + if (buf.size() == 0) { + SPDLOG_WARN("IMUBuffer::searchClosest: empty buffer!"); + return 0; + } + if (buf.size() == 1) { + return 0; + } + return searchClosest(t, 0, buf.size()); } size_t IMUBuffer::searchClosest(double t, int i0, int i1) const { - const double eps = 5e-4; - const Guard lock(buf_lock); - // printf("IMUBuffer::searchClosest: t=%f, i0=%d, i1=%d\n", t, i0, i1); - if (i1 - i0 == 1) { - return i0; - } - int i = (i0 + i1) / 2; - if (i > buf.size()) { - return i0; - } - if (buf[i].t > t - eps) { - return searchClosest(t, i0, i); - } else { - return searchClosest(t, i, i1); - } + const double eps = 5e-4; + const Guard lock(buf_lock); + // printf("IMUBuffer::searchClosest: t=%f, i0=%d, i1=%d\n", t, i0, i1); + if (i1 - i0 == 1) { + return i0; + } + int i = (i0 + i1) / 2; + if (i > buf.size()) { + return i0; + } + if (buf[i].t > t - eps) { + return searchClosest(t, i0, i); + } else { + return searchClosest(t, i, i1); + } } IMUBuffer IMUBuffer::slice(int i0, int i1) const { - const Guard lock(buf_lock); - IMUBuffer ret; - if (i0 > buf.size()) { - return ret; - } - if (i1 + 1 > buf.size()) { - ret.buf = std::vector(buf.begin() + i0, buf.end()); - } else { - ret.buf = std::vector(buf.begin() + i0, buf.begin() + i1 + 1); - } - ret.t_last = buf.back().t; + const Guard lock(buf_lock); + IMUBuffer ret; + if (i0 > buf.size()) { return ret; + } + if (i1 + 1 > buf.size()) { + ret.buf = std::vector(buf.begin() + i0, buf.end()); + } else { + ret.buf = std::vector(buf.begin() + i0, buf.begin() + i1 + 1); + } + ret.t_last = buf.back().t; + return ret; } -void IMUBuffer::add(const IMUData & data) { - const Guard lock(buf_lock); - buf.emplace_back(data); - t_last = data.t; +void IMUBuffer::add(const IMUData& data) { + const Guard lock(buf_lock); + buf.emplace_back(data); + t_last = data.t; } Vector3d IMUBuffer::mean_acc() const { - const Guard lock(buf_lock); - Vector3d acc_sum(0, 0, 0); - for (auto & data : buf) { - acc_sum += data.acc; - } - return acc_sum/size(); + const Guard lock(buf_lock); + Vector3d acc_sum(0, 0, 0); + for (auto& data : buf) { + acc_sum += data.acc; + } + return acc_sum / size(); } Vector3d IMUBuffer::mean_gyro() const { - const Guard lock(buf_lock); - Vector3d gyro_sum(0, 0, 0); - for (auto & data : buf) { - gyro_sum += data.gyro; - } - return gyro_sum/size(); + const Guard lock(buf_lock); + Vector3d gyro_sum(0, 0, 0); + for (auto& data : buf) { + gyro_sum += data.gyro; + } + return gyro_sum / size(); } size_t IMUBuffer::size() const { - const Guard lock(buf_lock); - return buf.size(); + const Guard lock(buf_lock); + return buf.size(); } -bool IMUBuffer::available(double t) const { - return t_last > t; -} +bool IMUBuffer::available(double t) const { return t_last > t; } IMUBuffer IMUBuffer::pop(double t) { - const Guard lock(buf_lock); - if (buf.size() == 0){ - return IMUBuffer(); - } - auto i0 = searchClosest(t); - IMUBuffer ret; - if (i0 > 0) { - ret.buf = std::vector(buf.begin(), buf.begin() + i0); - ret.t_last = ret.buf.back().t; - buf.erase(buf.begin(), buf.begin() + i0); - } - return ret; + const Guard lock(buf_lock); + if (buf.size() == 0) { + return IMUBuffer(); + } + auto i0 = searchClosest(t); + IMUBuffer ret; + if (i0 > 0) { + ret.buf = std::vector(buf.begin(), buf.begin() + i0); + ret.t_last = ret.buf.back().t; + buf.erase(buf.begin(), buf.begin() + i0); + } + return ret; } IMUBuffer IMUBuffer::tail(double t) const { - const Guard lock(buf_lock); - if (buf.size() == 0){ - return IMUBuffer(); - } - auto i0 = searchClosest(t); - IMUBuffer ret; - ret.buf = std::vector(buf.begin() + i0, buf.end()); - ret.t_last = buf.back().t; - return ret; + const Guard lock(buf_lock); + if (buf.size() == 0) { + return IMUBuffer(); + } + auto i0 = searchClosest(t); + IMUBuffer ret; + ret.buf = std::vector(buf.begin() + i0, buf.end()); + ret.t_last = buf.back().t; + return ret; } std::pair IMUBuffer::periodIMU(double t0, double t1) const { - const Guard lock(buf_lock); - if (buf.size() == 0){ - return std::make_pair(IMUBuffer(), 0); - } - auto i0 = searchClosest(t0); - auto i1 = searchClosest(t1); - return std::make_pair(slice(i0 + 1, i1 + 1), i1 + 1); + const Guard lock(buf_lock); + if (buf.size() == 0) { + return std::make_pair(IMUBuffer(), 0); + } + auto i0 = searchClosest(t0); + auto i1 = searchClosest(t1); + return std::make_pair(slice(i0 + 1, i1 + 1), i1 + 1); } std::pair IMUBuffer::periodIMU(int i0, double t1) const { - const Guard lock(buf_lock); - if (buf.size() == 0){ - return std::make_pair(IMUBuffer(), 0); - } - auto i1 = searchClosest(t1, i0 + 1, buf.size());; - return std::make_pair(slice(i0 + 1, i1 + 1), i1 + 1); + const Guard lock(buf_lock); + if (buf.size() == 0) { + return std::make_pair(IMUBuffer(), 0); + } + auto i1 = searchClosest(t1, i0 + 1, buf.size()); + ; + return std::make_pair(slice(i0 + 1, i1 + 1), i1 + 1); } -Swarm::Odometry IMUBuffer::propagation(const VINSFrame & baseframe) const { - return propagation(baseframe.odom, baseframe.Ba, baseframe.Bg); +Swarm::Odometry IMUBuffer::propagation(const VINSFrame& baseframe) const { + return propagation(baseframe.odom, baseframe.Ba, baseframe.Bg); } -Swarm::Odometry IMUBuffer::propagation(const Swarm::Odometry & prev_odom, const Vector3d & Ba, const Vector3d & Bg) const { - const Guard lock(buf_lock); - if(buf.size() == 0) { - return prev_odom; - } - Vector3d acc_last = buf[0].acc; - Vector3d gyro_last = buf[0].gyro; - - Swarm::Odometry odom = prev_odom; - IMUData imu_last = buf[0]; - for (auto & imu: buf) { - imu.propagation(odom, Ba, Bg, imu_last); - imu_last = imu; - } - return odom; +Swarm::Odometry IMUBuffer::propagation(const Swarm::Odometry& prev_odom, + const Vector3d& Ba, + const Vector3d& Bg) const { + const Guard lock(buf_lock); + if (buf.size() == 0) { + return prev_odom; + } + Swarm::Odometry odom = prev_odom; + IMUData imu_last = buf[0]; + for (auto& imu : buf) { + imu.propagation(odom, Ba, Bg, imu_last); + imu_last = imu; + } + return odom; } -void IMUData::propagation(Swarm::Odometry & odom, const Vector3d & Ba, const Vector3d & Bg, const IMUData & imu_last) const { - Vector3d un_acc_0 = odom.att() * (imu_last.acc - Ba) - Gravity; - Vector3d un_gyr = 0.5 * (imu_last.gyro + this->gyro) - Bg; - odom.att() = odom.att() * Utility::deltaQ(un_gyr * this->dt); - odom.att().normalize(); - Vector3d un_acc_1 = odom.att() * (this->acc - Ba) - Gravity; - Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1); - odom.pos() += this->dt * odom.vel() + 0.5 * this->dt * this->dt * un_acc; - odom.vel() += this->dt * un_acc; - odom.stamp = this->t; +void IMUData::propagation(Swarm::Odometry& odom, const Vector3d& Ba, + const Vector3d& Bg, const IMUData& imu_last) const { + Vector3d un_acc_0 = odom.att() * (imu_last.acc - Ba) - Gravity; + Vector3d un_gyr = 0.5 * (imu_last.gyro + this->gyro) - Bg; + odom.att() = odom.att() * Utility::deltaQ(un_gyr * this->dt); + odom.att().normalize(); + Vector3d un_acc_1 = odom.att() * (this->acc - Ba) - Gravity; + Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1); + odom.pos() += this->dt * odom.vel() + 0.5 * this->dt * this->dt * un_acc; + odom.vel() += this->dt * un_acc; + odom.stamp = this->t; } -} \ No newline at end of file +} // namespace D2Common \ No newline at end of file diff --git a/d2common/src/d2pgo_types.cpp b/d2common/src/d2pgo_types.cpp index 3450a53a..b825f6b7 100644 --- a/d2common/src/d2pgo_types.cpp +++ b/d2common/src/d2pgo_types.cpp @@ -1,92 +1,95 @@ #include -#include #include +#include + namespace D2Common { -DPGOData::DPGOData(const swarm_msgs::DPGOData & msg) { - stamp = msg.header.stamp.toSec(); - drone_id = msg.drone_id; - target_id = msg.target_id; - reference_frame_id = msg.reference_frame_id; - type = static_cast(msg.type); - for (size_t i = 0; i < msg.frame_ids.size(); i ++ ) { - frame_poses[msg.frame_ids[i]] = Swarm::Pose(msg.frame_poses[i]); - frame_duals[msg.frame_ids[i]] = Map(msg.frame_duals[i].data.data(), - msg.frame_duals[i].data.size()); - } - solver_token = msg.solver_token; - iteration_count = msg.iteration_count; +DPGOData::DPGOData(const swarm_msgs::DPGOData& msg) { + stamp = msg.header.stamp.toSec(); + drone_id = msg.drone_id; + target_id = msg.target_id; + reference_frame_id = msg.reference_frame_id; + type = static_cast(msg.type); + for (size_t i = 0; i < msg.frame_ids.size(); i++) { + frame_poses[msg.frame_ids[i]] = Swarm::Pose(msg.frame_poses[i]); + frame_duals[msg.frame_ids[i]] = Map( + msg.frame_duals[i].data.data(), msg.frame_duals[i].data.size()); + } + solver_token = msg.solver_token; + iteration_count = msg.iteration_count; } -DPGOData::DPGOData(const DistributedPGOData_t & msg) { - stamp = toROSTime(msg.timestamp).toSec(); - drone_id = msg.drone_id; - target_id = msg.target_id; - type = static_cast(msg.type); - reference_frame_id = msg.reference_frame_id; - for (size_t i = 0; i < msg.frame_ids.size(); i ++ ) { - frame_poses[msg.frame_ids[i]] = Swarm::Pose(msg.frame_poses[i]); - Map dual(msg.frame_duals[i].data.data(),msg.frame_duals[i].data.size()); - frame_duals[msg.frame_ids[i]] = dual.template cast(); - } - solver_token = msg.solver_token; - iteration_count = msg.iteration_count; +DPGOData::DPGOData(const DistributedPGOData_t& msg) { + stamp = toROSTime(msg.timestamp).toSec(); + drone_id = msg.drone_id; + target_id = msg.target_id; + type = static_cast(msg.type); + reference_frame_id = msg.reference_frame_id; + for (size_t i = 0; i < msg.frame_ids.size(); i++) { + frame_poses[msg.frame_ids[i]] = Swarm::Pose(msg.frame_poses[i]); + Map dual(msg.frame_duals[i].data.data(), + msg.frame_duals[i].data.size()); + frame_duals[msg.frame_ids[i]] = dual.template cast(); + } + solver_token = msg.solver_token; + iteration_count = msg.iteration_count; } swarm_msgs::DPGOData DPGOData::toROS() const { - swarm_msgs::DPGOData msg; - msg.header.stamp = ros::Time(stamp); - msg.drone_id = drone_id; - msg.target_id = target_id; - msg.reference_frame_id = reference_frame_id; - msg.type = type; - for (auto it: frame_poses) { - auto i = it.first; - auto pose = it.second; - msg.frame_poses.emplace_back(pose.toROS()); - msg.frame_ids.emplace_back(i); - VectorXd dual = frame_duals.at(i); - //Convert VectorXd of frame_duals to Float64MultiArray - std_msgs::Float64MultiArray dual_array; - dual_array.layout.data_offset = 0; - std_msgs::MultiArrayDimension dim0; - dual_array.layout.dim.emplace_back(dim0); - dim0.label = "length"; - dim0.size = dual.size(); - dim0.stride = dual.size(); - dual_array.data = std::vector(dual.data(), dual.data()+dual.size()); - msg.frame_duals.emplace_back(dual_array); - } - msg.solver_token = solver_token; - msg.iteration_count = iteration_count; - return msg; + swarm_msgs::DPGOData msg; + msg.header.stamp = ros::Time(stamp); + msg.drone_id = drone_id; + msg.target_id = target_id; + msg.reference_frame_id = reference_frame_id; + msg.type = type; + for (auto it : frame_poses) { + auto i = it.first; + auto pose = it.second; + msg.frame_poses.emplace_back(pose.toROS()); + msg.frame_ids.emplace_back(i); + VectorXd dual = frame_duals.at(i); + // Convert VectorXd of frame_duals to Float64MultiArray + std_msgs::Float64MultiArray dual_array; + dual_array.layout.data_offset = 0; + std_msgs::MultiArrayDimension dim0; + dual_array.layout.dim.emplace_back(dim0); + dim0.label = "length"; + dim0.size = dual.size(); + dim0.stride = dual.size(); + dual_array.data = + std::vector(dual.data(), dual.data() + dual.size()); + msg.frame_duals.emplace_back(dual_array); + } + msg.solver_token = solver_token; + msg.iteration_count = iteration_count; + return msg; } DistributedPGOData_t DPGOData::toLCM() const { - DistributedPGOData_t msg; - msg.timestamp = toLCMTime(ros::Time(stamp)); - msg.drone_id = drone_id; - msg.target_id = target_id; - msg.reference_frame_id = reference_frame_id; - msg.type = type; - for (auto it: frame_poses) { - auto i = it.first; - auto pose = it.second; - msg.frame_poses.emplace_back(pose.toLCM()); - msg.frame_ids.emplace_back(i); - VectorXd dual = frame_duals.at(i); - Vector_t dual_vec; - dual_vec.size = dual.size(); - dual_vec.data.resize(dual.size()); - Map _dual(dual_vec.data.data(), dual_vec.data.size()); - _dual = dual.template cast(); - msg.frame_duals.emplace_back(dual_vec); - } - msg.frame_num = frame_poses.size(); - msg.frame_poses_num = frame_poses.size(); - msg.frame_dual_num = frame_duals.size(); - msg.solver_token = solver_token; - msg.iteration_count = iteration_count; - return msg; + DistributedPGOData_t msg; + msg.timestamp = toLCMTime(ros::Time(stamp)); + msg.drone_id = drone_id; + msg.target_id = target_id; + msg.reference_frame_id = reference_frame_id; + msg.type = type; + for (auto it : frame_poses) { + auto i = it.first; + auto pose = it.second; + msg.frame_poses.emplace_back(pose.toLCM()); + msg.frame_ids.emplace_back(i); + VectorXd dual = frame_duals.at(i); + Vector_t dual_vec; + dual_vec.size = dual.size(); + dual_vec.data.resize(dual.size()); + Map _dual(dual_vec.data.data(), dual_vec.data.size()); + _dual = dual.template cast(); + msg.frame_duals.emplace_back(dual_vec); + } + msg.frame_num = frame_poses.size(); + msg.frame_poses_num = frame_poses.size(); + msg.frame_dual_num = frame_duals.size(); + msg.solver_token = solver_token; + msg.iteration_count = iteration_count; + return msg; } -}; \ No newline at end of file +}; // namespace D2Common \ No newline at end of file diff --git a/d2common/src/d2vinsframe.cpp b/d2common/src/d2vinsframe.cpp index f5100cd7..1726a7e0 100644 --- a/d2common/src/d2vinsframe.cpp +++ b/d2common/src/d2vinsframe.cpp @@ -3,114 +3,136 @@ namespace D2Common { double t0 = 0; -VINSFrame::VINSFrame(const VisualImageDescArray & frame, const IMUBuffer & buf, const VINSFrame & prev_frame): - D2BaseFrame(frame.stamp, frame.frame_id, frame.drone_id, frame.reference_frame_id, frame.is_keyframe, frame.pose_drone), - Ba(prev_frame.Ba), Bg(prev_frame.Bg), - prev_frame_id(prev_frame.frame_id){ - pre_integrations = new IntegrationBase(buf, Ba, Bg); - if (t0 == 0) { - t0 = stamp; - } +VINSFrame::VINSFrame(const VisualImageDescArray& frame, const IMUBuffer& buf, + const VINSFrame& prev_frame) + : D2BaseFrame(frame.stamp, frame.frame_id, frame.drone_id, + frame.reference_frame_id, frame.is_keyframe, + frame.pose_drone), + Ba(prev_frame.Ba), + Bg(prev_frame.Bg), + prev_frame_id(prev_frame.frame_id) { + pre_integrations = new IntegrationBase(buf, Ba, Bg); + if (t0 == 0) { + t0 = stamp; + } } -VINSFrame::VINSFrame(const VisualImageDescArray & frame, const std::pair & buf, const VINSFrame & prev_frame): - D2BaseFrame(frame.stamp, frame.frame_id, frame.drone_id, frame.reference_frame_id, frame.is_keyframe, frame.pose_drone), - Ba(prev_frame.Ba), Bg(prev_frame.Bg), - prev_frame_id(prev_frame.frame_id), - imu_buf_index(buf.second) { - pre_integrations = new IntegrationBase(buf.first, Ba, Bg); - if (t0 == 0) { - t0 = stamp; - } +VINSFrame::VINSFrame(const VisualImageDescArray& frame, + const std::pair& buf, + const VINSFrame& prev_frame) + : D2BaseFrame(frame.stamp, frame.frame_id, frame.drone_id, + frame.reference_frame_id, frame.is_keyframe, + frame.pose_drone), + Ba(prev_frame.Ba), + Bg(prev_frame.Bg), + prev_frame_id(prev_frame.frame_id), + imu_buf_index(buf.second) { + pre_integrations = new IntegrationBase(buf.first, Ba, Bg); + if (t0 == 0) { + t0 = stamp; + } } -VINSFrame::VINSFrame(const VisualImageDescArray & frame, const Vector3d & _Ba, const Vector3d & _Bg): - D2BaseFrame(frame.stamp, frame.frame_id, frame.drone_id, frame.reference_frame_id, frame.is_keyframe, frame.pose_drone), - Ba(_Ba), Bg(_Bg) { - if (t0 == 0) { - t0 = stamp; - } +VINSFrame::VINSFrame(const VisualImageDescArray& frame, const Vector3d& _Ba, + const Vector3d& _Bg) + : D2BaseFrame(frame.stamp, frame.frame_id, frame.drone_id, + frame.reference_frame_id, frame.is_keyframe, + frame.pose_drone), + Ba(_Ba), + Bg(_Bg) { + if (t0 == 0) { + t0 = stamp; + } } -VINSFrame::VINSFrame(const VisualImageDescArray & frame): - D2BaseFrame(frame.stamp, frame.frame_id, frame.drone_id, frame.reference_frame_id, frame.is_keyframe, frame.pose_drone), - Ba(0, 0., 0.), Bg(0., 0., 0.) { - if (t0 == 0) { - t0 = stamp; - } +VINSFrame::VINSFrame(const VisualImageDescArray& frame) + : D2BaseFrame(frame.stamp, frame.frame_id, frame.drone_id, + frame.reference_frame_id, frame.is_keyframe, + frame.pose_drone), + Ba(0, 0., 0.), + Bg(0., 0., 0.) { + if (t0 == 0) { + t0 = stamp; + } } std::string VINSFrame::toStr() { - char buf[1024] = {0}; - char buf_imu[1024] = {0}; - if (pre_integrations != nullptr) { - sprintf(buf_imu, "imu_size %ld sumdt %.1fms dP %3.2f %.2f %3.2f dQ %3.2f %3.2f %3.2f %3.2f dV %3.2f %3.2f %3.2f", - pre_integrations->acc_buf.size(), pre_integrations->sum_dt*1000, - pre_integrations->delta_p.x(), pre_integrations->delta_p.y(), pre_integrations->delta_p.z(), - pre_integrations->delta_q.w(), pre_integrations->delta_q.x(), pre_integrations->delta_q.y(), pre_integrations->delta_q.z(), - pre_integrations->delta_v.x(), pre_integrations->delta_v.y(), pre_integrations->delta_v.z()); - } - sprintf(buf, "VINSFrame %ld@%d stamp: %.3fs Odom: %s\nBa %.4f %.4f %.4f Bg %.4f %.4f %.4f pre_integrations: %s\n", - frame_id, drone_id, stamp-t0, odom.toStr().c_str(), - Ba(0), Ba(1), Ba(2), Bg(0), Bg(1), Bg(2), buf_imu); - return std::string(buf); + char buf[1024] = {0}; + char buf_imu[1024] = {0}; + if (pre_integrations != nullptr) { + sprintf(buf_imu, + "imu_size %ld sumdt %.1fms dP %3.2f %.2f %3.2f dQ %3.2f %3.2f " + "%3.2f %3.2f dV %3.2f %3.2f %3.2f", + pre_integrations->acc_buf.size(), pre_integrations->sum_dt * 1000, + pre_integrations->delta_p.x(), pre_integrations->delta_p.y(), + pre_integrations->delta_p.z(), pre_integrations->delta_q.w(), + pre_integrations->delta_q.x(), pre_integrations->delta_q.y(), + pre_integrations->delta_q.z(), pre_integrations->delta_v.x(), + pre_integrations->delta_v.y(), pre_integrations->delta_v.z()); + } + sprintf(buf, + "VINSFrame %ld@%d stamp: %.3fs Odom: %s\nBa %.4f %.4f %.4f Bg %.4f " + "%.4f %.4f pre_integrations: %s\n", + frame_id, drone_id, stamp - t0, odom.toStr().c_str(), Ba(0), Ba(1), + Ba(2), Bg(0), Bg(1), Bg(2), buf_imu); + return std::string(buf); } swarm_msgs::VIOFrame VINSFrame::toROS() { - swarm_msgs::VIOFrame msg; - msg.header.stamp = ros::Time(stamp); - msg.header.frame_id = "world"; - msg.frame_id = frame_id; - msg.drone_id = drone_id; - msg.is_keyframe = is_keyframe; - msg.reference_frame_id = reference_frame_id; - msg.odom = odom.toRos(); - return msg; + swarm_msgs::VIOFrame msg; + msg.header.stamp = ros::Time(stamp); + msg.header.frame_id = "world"; + msg.frame_id = frame_id; + msg.drone_id = drone_id; + msg.is_keyframe = is_keyframe; + msg.reference_frame_id = reference_frame_id; + msg.odom = odom.toRos(); + return msg; } -swarm_msgs::VIOFrame VINSFrame::toROS(const std::vector & exts) { - swarm_msgs::VIOFrame msg; - msg.header.stamp = ros::Time(stamp); - msg.header.frame_id = "world"; - msg.frame_id = frame_id; - msg.drone_id = drone_id; - msg.is_keyframe = is_keyframe; - msg.reference_frame_id = reference_frame_id; - msg.odom = odom.toRos(); - for (int i = 0; i < exts.size(); i++) { - msg.extrinsics.emplace_back(exts[i].toROS()); - } - return msg; +swarm_msgs::VIOFrame VINSFrame::toROS(const std::vector& exts) { + swarm_msgs::VIOFrame msg; + msg.header.stamp = ros::Time(stamp); + msg.header.frame_id = "world"; + msg.frame_id = frame_id; + msg.drone_id = drone_id; + msg.is_keyframe = is_keyframe; + msg.reference_frame_id = reference_frame_id; + msg.odom = odom.toRos(); + for (int i = 0; i < exts.size(); i++) { + msg.extrinsics.emplace_back(exts[i].toROS()); + } + return msg; } -void VINSFrame::toVector(state_type * _pose, state_type * _spd_bias) const { - odom.pose().to_vector(_pose); - _spd_bias[0] = odom.vel().x(); - _spd_bias[1] = odom.vel().y(); - _spd_bias[2] = odom.vel().z(); +void VINSFrame::toVector(state_type* _pose, state_type* _spd_bias) const { + odom.pose().to_vector(_pose); + _spd_bias[0] = odom.vel().x(); + _spd_bias[1] = odom.vel().y(); + _spd_bias[2] = odom.vel().z(); - _spd_bias[3] = Ba.x(); - _spd_bias[4] = Ba.y(); - _spd_bias[5] = Ba.z(); - - _spd_bias[6] = Bg.x(); - _spd_bias[7] = Bg.y(); - _spd_bias[8] = Bg.z(); + _spd_bias[3] = Ba.x(); + _spd_bias[4] = Ba.y(); + _spd_bias[5] = Ba.z(); + + _spd_bias[6] = Bg.x(); + _spd_bias[7] = Bg.y(); + _spd_bias[8] = Bg.z(); } -void VINSFrame::fromVector(state_type * _pose, state_type * _spd_bias) { - odom.pose().from_vector(_pose); - - odom.vel().x() = _spd_bias[0]; - odom.vel().y() = _spd_bias[1]; - odom.vel().z() = _spd_bias[2]; - - Ba.x() = _spd_bias[3]; - Ba.y() = _spd_bias[4]; - Ba.z() = _spd_bias[5]; +void VINSFrame::fromVector(state_type* _pose, state_type* _spd_bias) { + odom.pose().from_vector(_pose); + + odom.vel().x() = _spd_bias[0]; + odom.vel().y() = _spd_bias[1]; + odom.vel().z() = _spd_bias[2]; + + Ba.x() = _spd_bias[3]; + Ba.y() = _spd_bias[4]; + Ba.z() = _spd_bias[5]; - Bg.x() = _spd_bias[6]; - Bg.y() = _spd_bias[7]; - Bg.z() = _spd_bias[8]; + Bg.x() = _spd_bias[6]; + Bg.y() = _spd_bias[7]; + Bg.z() = _spd_bias[8]; } -} \ No newline at end of file +} // namespace D2Common \ No newline at end of file diff --git a/d2common/src/solver/ARock.cpp b/d2common/src/solver/ARock.cpp index dbbf8431..95be12f8 100644 --- a/d2common/src/solver/ARock.cpp +++ b/d2common/src/solver/ARock.cpp @@ -1,295 +1,329 @@ -#include +#include #include #include -#include + +#include namespace D2Common { void ARockBase::reset() { - dual_states_local.clear(); - dual_states_remote.clear(); - all_estimating_params.clear(); + dual_states_local.clear(); + dual_states_remote.clear(); + all_estimating_params.clear(); } -void ARockBase::addParam(const ParamInfo & param_info) { - if (all_estimating_params.find(param_info.pointer) != all_estimating_params.end()) { - return; - } - all_estimating_params[param_info.pointer] = param_info; +void ARockBase::addParam(const ParamInfo& param_info) { + if (all_estimating_params.find(param_info.pointer) != + all_estimating_params.end()) { + return; + } + all_estimating_params[param_info.pointer] = param_info; } void ARockBase::updateDualStates() { - for (auto & param_pair : dual_states_local) { - auto remote_drone_id = param_pair.first; - auto & duals = param_pair.second; - for (auto & it: duals) { - auto * state_pointer = it.first; - auto param_info = all_estimating_params.at(state_pointer); - auto & dual_state_local = it.second; - //Now we need to average the remote and the local dual state. - if (IsSE3(param_info.type)) { - //Use pose average. - Swarm::Pose dual_pose_local(dual_state_local); - Swarm::Pose dual_pose_remote(dual_states_remote.at(remote_drone_id).at(state_pointer)); - std::vector poses{dual_pose_remote, dual_pose_local}; - Swarm::Pose avg_pose = Swarm::Pose::averagePoses(poses); - Swarm::Pose cur_est_pose = Swarm::Pose(state_pointer); - Vector6d pose_err = Swarm::Pose::DeltaPose(cur_est_pose, avg_pose).tangentSpace(); - Vector6d delta_state = pose_err*config.eta_k; - //Retraction the delta state to pose - // printf("[ARockSolver%d] Pose %d delta_state: ", self_id, param_info.id); - // std::cout << delta_state.transpose() << std::endl; - Swarm::Pose dual_pose_local_new = dual_pose_local * - Swarm::Pose::fromTangentSpace(-delta_state); - dual_pose_local_new.to_vector(dual_state_local.data()); - } else if (IsPose4D(param_info.type)) { - VectorXd dual_state_remote = dual_states_remote.at(remote_drone_id).at(state_pointer); - VectorXd avg_state = (dual_state_local + dual_state_remote)/2; - Map cur_est_state(state_pointer, param_info.size); - VectorXd delta = (avg_state - cur_est_state)*config.eta_k; - dual_state_local = dual_state_local - delta; - if (dual_state_local(3) > M_PI || dual_state_local(3) < -M_PI) { - ROS_WARN("Note: [ARockSolver] Dual state %ld has angle %f\n", param_info.id, dual_state_local(3)); - dual_state_local(3) = Utility::NormalizeAngle(dual_state_local(3)); - ROS_WARN("Normed angle: %f", dual_state_local(3)); - } - } else { - // printf("[ARockSolver@%d] type %d frame_id %d\n", self_id, param_info.type, param_info.id); - //Is a vector. - VectorXd dual_state_remote = dual_states_remote.at(remote_drone_id).at(state_pointer); - // printf("\nFrame %d \n", param_info.id); - // std::cout << "Dual state remote:\n" << dual_state_remote.transpose() << std::endl; - // std::cout << "Dual state local :\n" << dual_state_local.transpose() << std::endl; - VectorXd avg_state = (dual_state_local + dual_state_remote)/2; - // std::cout << "avg_state: \n" << avg_state.transpose() << std::endl; - Map cur_est_state(state_pointer, param_info.size); - VectorXd delta = (avg_state - cur_est_state)*config.eta_k; - // std::cout << "cur_est_state: \n" << cur_est_state.transpose() << std::endl; - // std::cout << "delta: \n" << delta.transpose() << std::endl; - dual_state_local -= delta; - } + for (auto& param_pair : dual_states_local) { + auto remote_drone_id = param_pair.first; + auto& duals = param_pair.second; + for (auto& it : duals) { + auto* state_pointer = it.first; + auto param_info = all_estimating_params.at(state_pointer); + auto& dual_state_local = it.second; + // Now we need to average the remote and the local dual state. + if (IsSE3(param_info.type)) { + // Use pose average. + Swarm::Pose dual_pose_local(dual_state_local); + Swarm::Pose dual_pose_remote( + dual_states_remote.at(remote_drone_id).at(state_pointer)); + std::vector poses{dual_pose_remote, dual_pose_local}; + Swarm::Pose avg_pose = Swarm::Pose::averagePoses(poses); + Swarm::Pose cur_est_pose = Swarm::Pose(state_pointer); + Vector6d pose_err = + Swarm::Pose::DeltaPose(cur_est_pose, avg_pose).tangentSpace(); + Vector6d delta_state = pose_err * config.eta_k; + // Retraction the delta state to pose + // printf("[ARockSolver%d] Pose %d delta_state: ", self_id, + // param_info.id); std::cout << delta_state.transpose() << std::endl; + Swarm::Pose dual_pose_local_new = + dual_pose_local * Swarm::Pose::fromTangentSpace(-delta_state); + dual_pose_local_new.to_vector(dual_state_local.data()); + } else if (IsPose4D(param_info.type)) { + VectorXd dual_state_remote = + dual_states_remote.at(remote_drone_id).at(state_pointer); + VectorXd avg_state = (dual_state_local + dual_state_remote) / 2; + Map cur_est_state(state_pointer, param_info.size); + VectorXd delta = (avg_state - cur_est_state) * config.eta_k; + dual_state_local = dual_state_local - delta; + if (dual_state_local(3) > M_PI || dual_state_local(3) < -M_PI) { + ROS_WARN("Note: [ARockSolver] Dual state %ld has angle %f\n", + param_info.id, dual_state_local(3)); + dual_state_local(3) = Utility::NormalizeAngle(dual_state_local(3)); + ROS_WARN("Normed angle: %f", dual_state_local(3)); } + } else { + // printf("[ARockSolver@%d] type %d frame_id %d\n", self_id, + // param_info.type, param_info.id); + // Is a vector. + VectorXd dual_state_remote = + dual_states_remote.at(remote_drone_id).at(state_pointer); + // printf("\nFrame %d \n", param_info.id); + // std::cout << "Dual state remote:\n" << dual_state_remote.transpose() + // << std::endl; std::cout << "Dual state local :\n" << + // dual_state_local.transpose() << std::endl; + VectorXd avg_state = (dual_state_local + dual_state_remote) / 2; + // std::cout << "avg_state: \n" << avg_state.transpose() << std::endl; + Map cur_est_state(state_pointer, param_info.size); + VectorXd delta = (avg_state - cur_est_state) * config.eta_k; + // std::cout << "cur_est_state: \n" << cur_est_state.transpose() << + // std::endl; std::cout << "delta: \n" << delta.transpose() << + // std::endl; + dual_state_local -= delta; + } } + } } -bool ARockBase::isRemoteParam(const ParamInfo & param_info) { - if (param_info.type == ParamsType::POSE || param_info.type == ParamsType::POSE_4D || - param_info.type == ParamsType::ROTMAT || param_info.type == ParamsType::POSE_PERTURB_6D) { - auto frame = state->getFramebyId(param_info.id); - if (frame->drone_id != self_id) { - return true; - } +bool ARockBase::isRemoteParam(const ParamInfo& param_info) { + if (param_info.type == ParamsType::POSE || + param_info.type == ParamsType::POSE_4D || + param_info.type == ParamsType::ROTMAT || + param_info.type == ParamsType::POSE_PERTURB_6D) { + auto frame = state->getFramebyId(param_info.id); + if (frame->drone_id != self_id) { + return true; } - return false; + } + return false; } -int ARockBase::solverId(const ParamInfo & param_info) { - if (param_info.type == ParamsType::POSE || param_info.type == ParamsType::POSE_4D || param_info.type == ParamsType::ROTMAT - || param_info.type == ParamsType::POSE_PERTURB_6D) { - auto frame = state->getFramebyId(param_info.id); - return frame->drone_id; - } - return -1; +int ARockBase::solverId(const ParamInfo& param_info) { + if (param_info.type == ParamsType::POSE || + param_info.type == ParamsType::POSE_4D || + param_info.type == ParamsType::ROTMAT || + param_info.type == ParamsType::POSE_PERTURB_6D) { + auto frame = state->getFramebyId(param_info.id); + return frame->drone_id; + } + return -1; } bool ARockBase::hasDualState(state_type* param, int drone_id) { - if (dual_states_remote.find(drone_id) != dual_states_remote.end()) { - if (dual_states_remote[drone_id].find(param) != dual_states_remote[drone_id].end()) { - return true; - } + if (dual_states_remote.find(drone_id) != dual_states_remote.end()) { + if (dual_states_remote[drone_id].find(param) != + dual_states_remote[drone_id].end()) { + return true; } - return false; + } + return false; } -void ARockBase::createDualState(const ParamInfo & param_info, int drone_id, bool init_to_zero) { - if (dual_states_remote.find(drone_id) == dual_states_remote.end()) { - dual_states_remote[drone_id] = std::map(); - dual_states_local[drone_id] = std::map(); - } - if (init_to_zero) { - dual_states_remote[drone_id][param_info.pointer] = VectorXd::Zero(param_info.size); - dual_states_local[drone_id][param_info.pointer] = VectorXd::Zero(param_info.size); - } else { - dual_states_remote[drone_id][param_info.pointer] = Map(param_info.pointer, param_info.size); - dual_states_local[drone_id][param_info.pointer] = Map(param_info.pointer, param_info.size); - } - updated = true; +void ARockBase::createDualState(const ParamInfo& param_info, int drone_id, + bool init_to_zero) { + if (dual_states_remote.find(drone_id) == dual_states_remote.end()) { + dual_states_remote[drone_id] = std::map(); + dual_states_local[drone_id] = std::map(); + } + if (init_to_zero) { + dual_states_remote[drone_id][param_info.pointer] = + VectorXd::Zero(param_info.size); + dual_states_local[drone_id][param_info.pointer] = + VectorXd::Zero(param_info.size); + } else { + dual_states_remote[drone_id][param_info.pointer] = + Map(param_info.pointer, param_info.size); + dual_states_local[drone_id][param_info.pointer] = + Map(param_info.pointer, param_info.size); + } + updated = true; } SolverReport ARockBase::solve_arock() { - // ROS_INFO("ARockBase::solve"); - SolverReport report; - Utility::TicToc tic; - int iter_cnt = 0; - int total_cnt = 0; - while (iter_cnt < config.max_steps) { - //If sync mode. - receiveAll(); - if (!updated) { - if (config.verbose) - printf("[ARock@%d] No new data, skip this step: %d total_cnt %d.\n", self_id, iter_cnt, total_cnt); - usleep(config.skip_iteration_usec); - total_cnt ++; - if (total_cnt > config.max_wait_steps + config.max_steps) { - if (config.verbose) - printf("Exit because exceed max_wait_steps: %d\n", total_cnt); - break; - } else { - continue; - } - } - prepareSolverInIter(total_cnt == config.max_steps - 1); - scanAndCreateDualStates(); - setDualStateFactors(); - auto _report = solveLocalStep(); - updateDualStates(); - broadcastData(); - clearSolver(iter_cnt == config.max_steps - 1); - report.compose(_report); - float changes = (_report.initial_cost-_report.final_cost)/_report.initial_cost; - if (iter_cnt == 0) { - report.initial_cost = _report.initial_cost; - } + // ROS_INFO("ARockBase::solve"); + SolverReport report; + Utility::TicToc tic; + int iter_cnt = 0; + int total_cnt = 0; + while (iter_cnt < config.max_steps) { + // If sync mode. + receiveAll(); + if (!updated) { + if (config.verbose) + printf("[ARock@%d] No new data, skip this step: %d total_cnt %d.\n", + self_id, iter_cnt, total_cnt); + usleep(config.skip_iteration_usec); + total_cnt++; + if (total_cnt > config.max_wait_steps + config.max_steps) { if (config.verbose) - printf("[ARock@%d] substeps: %d/%d total_iterations: %d initial_cost: %.2e final_cost: %.2e changes: %02.2f%% state_changes: %02.2f%% time: %.2fms steps: %d\n", - self_id, iter_cnt, config.max_steps, report.total_iterations, report.initial_cost, report.final_cost, changes*100, _report.state_changes, - _report.total_time * 1000, report.total_iterations); - iter_cnt ++; - total_cnt ++; + printf("Exit because exceed max_wait_steps: %d\n", total_cnt); + break; + } else { + continue; + } } - report.total_time = tic.toc()/1000; - return report; + prepareSolverInIter(total_cnt == config.max_steps - 1); + scanAndCreateDualStates(); + setDualStateFactors(); + auto _report = solveLocalStep(); + updateDualStates(); + broadcastData(); + clearSolver(iter_cnt == config.max_steps - 1); + report.compose(_report); + float changes = + (_report.initial_cost - _report.final_cost) / _report.initial_cost; + if (iter_cnt == 0) { + report.initial_cost = _report.initial_cost; + } + if (config.verbose) + printf( + "[ARock@%d] substeps: %d/%d total_iterations: %d initial_cost: %.2e " + "final_cost: %.2e changes: %02.2f%% state_changes: %02.2f%% time: " + "%.2fms steps: %d\n", + self_id, iter_cnt, config.max_steps, report.total_iterations, + report.initial_cost, report.final_cost, changes * 100, + _report.state_changes, _report.total_time * 1000, + report.total_iterations); + iter_cnt++; + total_cnt++; + } + report.total_time = tic.toc() / 1000; + return report; } void ARockSolver::scanAndCreateDualStates() { - for (auto res: residuals) { - auto param_infos = res->paramsList(SolverWrapper::state); - for (auto param_info: param_infos) { - if (isRemoteParam(param_info)) { - auto drone_id = solverId(param_info); - if (drone_id!=self_id) { - if (!hasDualState(param_info.pointer, drone_id)) { - // createDualState(param_info, drone_id, true); - createDualState(param_info, drone_id, config.dual_state_init_to_zero); - } - } - } + for (auto res : residuals) { + auto param_infos = res->paramsList(SolverWrapper::state); + for (auto param_info : param_infos) { + if (isRemoteParam(param_info)) { + auto drone_id = solverId(param_info); + if (drone_id != self_id) { + if (!hasDualState(param_info.pointer, drone_id)) { + // createDualState(param_info, drone_id, true); + createDualState(param_info, drone_id, + config.dual_state_init_to_zero); + } } + } } + } } void ARockSolver::reset() { - SolverWrapper::reset(); - ARockBase::reset(); + SolverWrapper::reset(); + ARockBase::reset(); } -void ARockSolver::addResidual(ResidualInfo*residual_info) { - for (auto param: residual_info->paramsList(SolverWrapper::state)) { - addParam(param); - } - SolverWrapper::addResidual(residual_info); - updated = true; +void ARockSolver::addResidual(ResidualInfo* residual_info) { + for (auto param : residual_info->paramsList(SolverWrapper::state)) { + addParam(param); + } + SolverWrapper::addResidual(residual_info); + updated = true; } -void ARockSolver::resetResiduals() { - residuals.clear(); -} +void ARockSolver::resetResiduals() { residuals.clear(); } void ARockSolver::prepareSolverInIter(bool final_iter) { - ceres::Problem::Options problem_options; - if (!final_iter) { - problem_options.cost_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; - problem_options.loss_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; - } - problem = new ceres::Problem(problem_options); - for (auto residual_info : residuals) { - problem->AddResidualBlock(residual_info->cost_function, residual_info->loss_function, - residual_info->paramsPointerList(SolverWrapper::state)); - } - setStateProperties(); + ceres::Problem::Options problem_options; + if (!final_iter) { + problem_options.cost_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; + problem_options.loss_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; + } + problem = new ceres::Problem(problem_options); + for (auto residual_info : residuals) { + problem->AddResidualBlock( + residual_info->cost_function, residual_info->loss_function, + residual_info->paramsPointerList(SolverWrapper::state)); + } + setStateProperties(); } -SolverReport ARockSolver::solve() { - return ARockBase::solve_arock(); -} +SolverReport ARockSolver::solve() { return ARockBase::solve_arock(); } void ARockSolver::clearSolver(bool final_substep) { - if (problem != nullptr) { - delete problem; - } - problem = nullptr; - // if (!final_substep) { - // //We need to clear dual factors manually. - // for (auto factor : dual_factors) { - // delete factor; - // } - // dual_factors.clear(); - // } + if (problem != nullptr) { + delete problem; + } + problem = nullptr; + // if (!final_substep) { + // //We need to clear dual factors manually. + // for (auto factor : dual_factors) { + // delete factor; + // } + // dual_factors.clear(); + // } } void ARockSolver::setDualStateFactors() { - for (auto & param_pair : dual_states_remote) { - for (auto it : param_pair.second) { - auto state_pointer = it.first; - auto param_info = all_estimating_params.at(state_pointer); - auto dual_state = it.second; - if (IsSE3(param_info.type)) { - //Is SE(3) pose. - Swarm::Pose pose_dual(dual_state); - auto factor = new ConsenusPoseFactor(pose_dual.pos(), pose_dual.att(), - Vector3d::Zero(), Vector3d::Zero(), rho_T, rho_theta); - // printf("[ARockSolver] ConsenusPoseFactor param %ld, drone_id %d pose_dual %s pose_cur %s\n", - // param_info.id, param_pair.first, pose_dual.toStr().c_str(), Swarm::Pose(state_pointer).toStr().c_str()); - problem->AddResidualBlock(factor, nullptr, state_pointer); - dual_factors.push_back(factor); - } else if (IsPose4D(param_info.type)) { - Swarm::Pose pose_dual(dual_state); - // printf("[ARockSolver] ConsenusPoseFactor4D param %ld, drone_id %d pose_dual %s pose_cur %s\n", - // param_info.id, param_pair.first, pose_dual.toStr().c_str(), Swarm::Pose(state_pointer, true).toStr().c_str()); - auto factor = ConsenusPoseFactor4D::Create(pose_dual, rho_T, rho_theta, true); - problem->AddResidualBlock(factor, nullptr, state_pointer); - dual_factors.push_back(factor); - } else if (param_info.type == D2Common::POSE_PERTURB_6D) { - MatrixXd A(param_info.size, param_info.size); - A.setIdentity(); - A.block<3, 3>(0, 0) *= sqrt(rho_T); - A.block<3, 3>(3, 3) *= sqrt(rho_theta); - auto factor = new ceres::NormalPrior(A, dual_state); - problem->AddResidualBlock(factor, nullptr, state_pointer); - dual_factors.push_back(factor); - // if (self_id == 0) { - // printf("[ARockSolver] ConsenusPosePerturbFactor param %ld, drone_id %d A:\n", - // param_info.id, param_pair.first); - // std::cout << A << std::endl << "dual_state: " << dual_state.transpose() << std::endl; - // } - } else { - //Is euclidean. - MatrixXd A(param_info.size, param_info.size); - A.setIdentity(); - if (param_info.type == LANDMARK) { - A *= rho_landmark; - } else { - //Not implement yet - } - auto factor = new ceres::NormalPrior(A, dual_state); - problem->AddResidualBlock(factor, nullptr, state_pointer); - dual_factors.push_back(factor); - } + for (auto& param_pair : dual_states_remote) { + for (auto it : param_pair.second) { + auto state_pointer = it.first; + auto param_info = all_estimating_params.at(state_pointer); + auto dual_state = it.second; + if (IsSE3(param_info.type)) { + // Is SE(3) pose. + Swarm::Pose pose_dual(dual_state); + auto factor = new ConsenusPoseFactor(pose_dual.pos(), pose_dual.att(), + Vector3d::Zero(), Vector3d::Zero(), + rho_T, rho_theta); + // printf("[ARockSolver] ConsenusPoseFactor param %ld, drone_id %d + // pose_dual %s pose_cur %s\n", + // param_info.id, param_pair.first, pose_dual.toStr().c_str(), + // Swarm::Pose(state_pointer).toStr().c_str()); + problem->AddResidualBlock(factor, nullptr, state_pointer); + dual_factors.push_back(factor); + } else if (IsPose4D(param_info.type)) { + Swarm::Pose pose_dual(dual_state); + // printf("[ARockSolver] ConsenusPoseFactor4D param %ld, drone_id %d + // pose_dual %s pose_cur %s\n", + // param_info.id, param_pair.first, pose_dual.toStr().c_str(), + // Swarm::Pose(state_pointer, true).toStr().c_str()); + auto factor = + ConsenusPoseFactor4D::Create(pose_dual, rho_T, rho_theta, true); + problem->AddResidualBlock(factor, nullptr, state_pointer); + dual_factors.push_back(factor); + } else if (param_info.type == D2Common::POSE_PERTURB_6D) { + MatrixXd A(param_info.size, param_info.size); + A.setIdentity(); + A.block<3, 3>(0, 0) *= sqrt(rho_T); + A.block<3, 3>(3, 3) *= sqrt(rho_theta); + auto factor = new ceres::NormalPrior(A, dual_state); + problem->AddResidualBlock(factor, nullptr, state_pointer); + dual_factors.push_back(factor); + // if (self_id == 0) { + // printf("[ARockSolver] ConsenusPosePerturbFactor param %ld, + // drone_id %d A:\n", + // param_info.id, param_pair.first); + // std::cout << A << std::endl << "dual_state: " << + // dual_state.transpose() << std::endl; + // } + } else { + // Is euclidean. + MatrixXd A(param_info.size, param_info.size); + A.setIdentity(); + if (param_info.type == LANDMARK) { + A *= rho_landmark; + } else { + // Not implement yet } + auto factor = new ceres::NormalPrior(A, dual_state); + problem->AddResidualBlock(factor, nullptr, state_pointer); + dual_factors.push_back(factor); + } } + } } SolverReport ARockSolver::solveLocalStep() { - ceres::Solver::Summary summary; - ceres::Solve(config.ceres_options, problem, &summary); - updated = false; - SolverReport report; - report.total_iterations = summary.num_successful_steps + summary.num_unsuccessful_steps; - report.total_time = summary.total_time_in_seconds; - report.initial_cost = summary.initial_cost; - report.final_cost = summary.final_cost; - report.summary = summary; - return report; + ceres::Solver::Summary summary; + ceres::Solve(config.ceres_options, problem, &summary); + updated = false; + SolverReport report; + report.total_iterations = + summary.num_successful_steps + summary.num_unsuccessful_steps; + report.total_time = summary.total_time_in_seconds; + report.initial_cost = summary.initial_cost; + report.final_cost = summary.final_cost; + report.summary = summary; + return report; } -}; +}; // namespace D2Common diff --git a/d2common/src/solver/BaseParamResInfo.cpp b/d2common/src/solver/BaseParamResInfo.cpp index 8f826e85..3ca1124d 100644 --- a/d2common/src/solver/BaseParamResInfo.cpp +++ b/d2common/src/solver/BaseParamResInfo.cpp @@ -1,97 +1,100 @@ #include namespace D2Common { -ParamInfo createFramePose(D2State * state, FrameIdType id, bool is_perturb) { - ParamInfo info; - if (is_perturb) { - info.pointer = state->getPerturbState(id); - info.size = POSE_EFF_SIZE; - info.type = POSE_PERTURB_6D; - } else { - info.type = POSE; - info.pointer = state->getPoseState(id); - info.size = POSE_SIZE; - } - info.index = -1; - info.eff_size = POSE_EFF_SIZE; - info.id = id; - info.data_copied = Map(info.pointer, info.size); - return info; +ParamInfo createFramePose(D2State *state, FrameIdType id, bool is_perturb) { + ParamInfo info; + if (is_perturb) { + info.pointer = state->getPerturbState(id); + info.size = POSE_EFF_SIZE; + info.type = POSE_PERTURB_6D; + } else { + info.type = POSE; + info.pointer = state->getPoseState(id); + info.size = POSE_SIZE; + } + info.index = -1; + info.eff_size = POSE_EFF_SIZE; + info.id = id; + info.data_copied = Map(info.pointer, info.size); + return info; } -ParamInfo createFrameRotMat(D2State * state, FrameIdType id) { - ParamInfo info; - info.pointer = state->getRotState(id); - info.index = -1; - info.size = ROTMAT_SIZE; - info.eff_size = ROTMAT_SIZE; - info.type = ROTMAT; - info.id = id; - info.data_copied = Map(info.pointer, info.size); - return info; +ParamInfo createFrameRotMat(D2State *state, FrameIdType id) { + ParamInfo info; + info.pointer = state->getRotState(id); + info.index = -1; + info.size = ROTMAT_SIZE; + info.eff_size = ROTMAT_SIZE; + info.type = ROTMAT; + info.id = id; + info.data_copied = Map(info.pointer, info.size); + return info; } -ParamInfo createFramePose4D(D2State * state, FrameIdType id) { - ParamInfo info; - info.pointer = state->getPoseState(id); - info.index = -1; - info.size = POSE4D_SIZE; - info.eff_size = POSE4D_SIZE; - info.type = POSE_4D; - info.id = id; - info.data_copied = Map(info.pointer, info.size); - return info; +ParamInfo createFramePose4D(D2State *state, FrameIdType id) { + ParamInfo info; + info.pointer = state->getPoseState(id); + info.index = -1; + info.size = POSE4D_SIZE; + info.eff_size = POSE4D_SIZE; + info.type = POSE_4D; + info.id = id; + info.data_copied = Map(info.pointer, info.size); + return info; } -void ResidualInfo::Evaluate(const std::vector & param_infos, bool use_copied) { - std::vector params; - if (use_copied) { - for (auto & info : param_infos) { - params.push_back(info.data_copied.data()); - } - } else { - for (auto info : param_infos) { - params.push_back(info.pointer); - } +void ResidualInfo::Evaluate(const std::vector ¶m_infos, + bool use_copied) { + std::vector params; + if (use_copied) { + for (auto &info : param_infos) { + params.push_back(info.data_copied.data()); + } + } else { + for (auto info : param_infos) { + params.push_back(info.pointer); } + } - //This function is from VINS. - residuals.resize(cost_function->num_residuals()); - std::vector blk_sizes = cost_function->parameter_block_sizes(); - std::vector raw_jacobians(blk_sizes.size()); - jacobians.resize(blk_sizes.size()); - for (int i = 0; i < static_cast(blk_sizes.size()); i++) { - jacobians[i].resize(cost_function->num_residuals(), blk_sizes[i]); - jacobians[i].setZero(); - raw_jacobians[i] = jacobians[i].data(); + // This function is from VINS. + residuals.resize(cost_function->num_residuals()); + std::vector blk_sizes = cost_function->parameter_block_sizes(); + std::vector raw_jacobians(blk_sizes.size()); + jacobians.resize(blk_sizes.size()); + for (int i = 0; i < static_cast(blk_sizes.size()); i++) { + jacobians[i].resize(cost_function->num_residuals(), blk_sizes[i]); + jacobians[i].setZero(); + raw_jacobians[i] = jacobians[i].data(); + } + cost_function->Evaluate(params.data(), residuals.data(), + raw_jacobians.data()); + if (loss_function) { + double residual_scaling_, alpha_sq_norm_; + double sq_norm, rho[3]; + sq_norm = residuals.squaredNorm(); + loss_function->Evaluate(sq_norm, rho); + double sqrt_rho1_ = sqrt(rho[1]); + if ((sq_norm == 0.0) || (rho[2] <= 0.0)) { + residual_scaling_ = sqrt_rho1_; + alpha_sq_norm_ = 0.0; + } else { + const double D = 1.0 + 2.0 * sq_norm * rho[2] / rho[1]; + const double alpha = 1.0 - sqrt(D); + residual_scaling_ = sqrt_rho1_ / (1 - alpha); + alpha_sq_norm_ = alpha / sq_norm; } - cost_function->Evaluate(params.data(), residuals.data(), raw_jacobians.data()); - if (loss_function) - { - double residual_scaling_, alpha_sq_norm_; - double sq_norm, rho[3]; - sq_norm = residuals.squaredNorm(); - loss_function->Evaluate(sq_norm, rho); - double sqrt_rho1_ = sqrt(rho[1]); - if ((sq_norm == 0.0) || (rho[2] <= 0.0)) { - residual_scaling_ = sqrt_rho1_; - alpha_sq_norm_ = 0.0; - } else { - const double D = 1.0 + 2.0 * sq_norm * rho[2] / rho[1]; - const double alpha = 1.0 - sqrt(D); - residual_scaling_ = sqrt_rho1_ / (1 - alpha); - alpha_sq_norm_ = alpha / sq_norm; - } - for (int i = 0; i < static_cast(params.size()); i++) { - jacobians[i] = sqrt_rho1_ * (jacobians[i] - alpha_sq_norm_ * residuals * (residuals.transpose() * jacobians[i])); - } - residuals *= residual_scaling_; + for (int i = 0; i < static_cast(params.size()); i++) { + jacobians[i] = sqrt_rho1_ * (jacobians[i] - + alpha_sq_norm_ * residuals * + (residuals.transpose() * jacobians[i])); } + residuals *= residual_scaling_; + } } -void ResidualInfo::Evaluate(D2State * state) { - auto param_infos = paramsList(state); - Evaluate(param_infos); +void ResidualInfo::Evaluate(D2State *state) { + auto param_infos = paramsList(state); + Evaluate(param_infos); } -} \ No newline at end of file +} // namespace D2Common \ No newline at end of file diff --git a/d2common/src/solver/BaseSolverWrapper.cpp b/d2common/src/solver/BaseSolverWrapper.cpp index 864a493e..820c3b97 100644 --- a/d2common/src/solver/BaseSolverWrapper.cpp +++ b/d2common/src/solver/BaseSolverWrapper.cpp @@ -1,27 +1,27 @@ -#include #include +#include namespace D2Common { -void CeresSolver::addResidual(ResidualInfo*residual_info) { - auto pointers = residual_info->paramsPointerList(state); - // printf("Add residual info %d", residual_info->residual_type); - problem->AddResidualBlock(residual_info->cost_function, - residual_info->loss_function, - pointers); - SolverWrapper::addResidual(residual_info); +void CeresSolver::addResidual(ResidualInfo* residual_info) { + auto pointers = residual_info->paramsPointerList(state); + // printf("Add residual info %d", residual_info->residual_type); + problem->AddResidualBlock(residual_info->cost_function, + residual_info->loss_function, pointers); + SolverWrapper::addResidual(residual_info); } SolverReport CeresSolver::solve() { - ceres::Solver::Summary summary; - ceres::Solve(options, problem, &summary); - SolverReport report; - report.total_iterations = summary.num_successful_steps + summary.num_unsuccessful_steps; - report.total_time = summary.total_time_in_seconds; - report.initial_cost = summary.initial_cost; - report.final_cost = summary.final_cost; - report.summary = summary; - // std::cout << summary.FullReport() << std::endl; - return report; + ceres::Solver::Summary summary; + ceres::Solve(options, problem, &summary); + SolverReport report; + report.total_iterations = + summary.num_successful_steps + summary.num_unsuccessful_steps; + report.total_time = summary.total_time_in_seconds; + report.initial_cost = summary.initial_cost; + report.final_cost = summary.final_cost; + report.summary = summary; + // std::cout << summary.FullReport() << std::endl; + return report; } -} \ No newline at end of file +} // namespace D2Common \ No newline at end of file diff --git a/d2common/src/solver/ConsensusSolver.cpp b/d2common/src/solver/ConsensusSolver.cpp index a9f8a9c2..6f438d8c 100644 --- a/d2common/src/solver/ConsensusSolver.cpp +++ b/d2common/src/solver/ConsensusSolver.cpp @@ -1,216 +1,237 @@ -#include #include #include + #include +#include namespace D2Common { -void ConsensusSolver::addResidual(ResidualInfo*residual_info) { - for (auto param: residual_info->paramsList(state)) { - addParam(param); - } - SolverWrapper::addResidual(residual_info); +void ConsensusSolver::addResidual(ResidualInfo* residual_info) { + for (auto param : residual_info->paramsList(state)) { + addParam(param); + } + SolverWrapper::addResidual(residual_info); } void ConsensusSolver::reset() { - SolverWrapper::reset(); - consenus_params.clear(); - all_estimating_params.clear(); - active_params.clear(); - residuals.clear(); - if (config.sync_for_averaging) { - remote_params.clear(); - } + SolverWrapper::reset(); + consenus_params.clear(); + all_estimating_params.clear(); + active_params.clear(); + residuals.clear(); + if (config.sync_for_averaging) { + remote_params.clear(); + } } -void ConsensusSolver::addParam(const ParamInfo & param_info) { - active_params.insert(param_info.pointer); - if (all_estimating_params.find(param_info.pointer) != all_estimating_params.end()) { - return; - } - all_estimating_params[param_info.pointer] = param_info; - consenus_params[param_info.pointer] = ConsenusParamState::create(param_info); - // printf("add param type %d %d: ", param_info.type, param_info.id); - // std::cout << consenus_params[param_info.pointer].param_global.transpose() << std::endl; +void ConsensusSolver::addParam(const ParamInfo& param_info) { + active_params.insert(param_info.pointer); + if (all_estimating_params.find(param_info.pointer) != + all_estimating_params.end()) { + return; + } + all_estimating_params[param_info.pointer] = param_info; + consenus_params[param_info.pointer] = ConsenusParamState::create(param_info); + // printf("add param type %d %d: ", param_info.type, param_info.id); + // std::cout << consenus_params[param_info.pointer].param_global.transpose() + // << std::endl; } SolverReport ConsensusSolver::solve() { - SolverReport report; - Utility::TicToc tic; - iteration_count = 0; - for (int i = 0; i < config.max_steps; i++) { - syncData(); - if (problem != nullptr) { - delete problem; - } - ceres::Problem::Options problem_options; - if (i != config.max_steps - 1) { - problem_options.cost_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; - problem_options.loss_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; - problem_options.local_parameterization_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; - problem_options.manifold_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; - } - problem = new ceres::Problem(problem_options); - for (auto residual_info : residuals) { - problem->AddResidualBlock(residual_info->cost_function, residual_info->loss_function, - residual_info->paramsPointerList(state)); - } - // removeDeactivatedParams(); - updateTilde(); - setStateProperties(); - ceres::Solver::Summary summary; - summary = solveLocalStep(); - report.total_iterations += summary.num_successful_steps + summary.num_unsuccessful_steps; - report.final_cost = summary.final_cost; - iteration_count++; + SolverReport report; + Utility::TicToc tic; + iteration_count = 0; + for (int i = 0; i < config.max_steps; i++) { + syncData(); + if (problem != nullptr) { + delete problem; + } + ceres::Problem::Options problem_options; + if (i != config.max_steps - 1) { + problem_options.cost_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; + problem_options.loss_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; + problem_options.local_parameterization_ownership = + ceres::DO_NOT_TAKE_OWNERSHIP; + problem_options.manifold_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; } - report.total_time = tic.toc()/1000; - broadcastData(); - return report; + problem = new ceres::Problem(problem_options); + for (auto residual_info : residuals) { + problem->AddResidualBlock(residual_info->cost_function, + residual_info->loss_function, + residual_info->paramsPointerList(state)); + } + // removeDeactivatedParams(); + updateTilde(); + setStateProperties(); + ceres::Solver::Summary summary; + summary = solveLocalStep(); + report.total_iterations += + summary.num_successful_steps + summary.num_unsuccessful_steps; + report.final_cost = summary.final_cost; + iteration_count++; + } + report.total_time = tic.toc() / 1000; + broadcastData(); + return report; } void ConsensusSolver::syncData() { - broadcastData(); - if (config.sync_for_averaging) { - Utility::TicToc tic_sync; - waitForSync(); - // printf("ConsensusSolver wait for sync time: %.1fms step %d/%d\n", tic_sync.toc(), i, config.max_steps); - } else { - receiveAll(); - } - updateGlobal(); + broadcastData(); + if (config.sync_for_averaging) { + Utility::TicToc tic_sync; + waitForSync(); + // printf("ConsensusSolver wait for sync time: %.1fms step %d/%d\n", + // tic_sync.toc(), i, config.max_steps); + } else { + receiveAll(); + } + updateGlobal(); } void ConsensusSolver::removeDeactivatedParams() { - for (auto it = consenus_params.begin(); it != consenus_params.end();) { - if (active_params.find(it->first) == active_params.end()) { - it = consenus_params.erase(it); - } else { - it++; - } + for (auto it = consenus_params.begin(); it != consenus_params.end();) { + if (active_params.find(it->first) == active_params.end()) { + it = consenus_params.erase(it); + } else { + it++; } - for (auto it = all_estimating_params.begin(); it != all_estimating_params.end();) { - if (active_params.find(it->first) == active_params.end()) { - it = all_estimating_params.erase(it); - } else { - it++; - } + } + for (auto it = all_estimating_params.begin(); + it != all_estimating_params.end();) { + if (active_params.find(it->first) == active_params.end()) { + it = all_estimating_params.erase(it); + } else { + it++; } + } } void ConsensusSolver::updateTilde() { - for (auto it: all_estimating_params) { - auto pointer = it.first; - auto paraminfo = it.second; - auto & consenus_param = consenus_params[pointer]; - if (consenus_param.local_only) { - //Add normal prior factor - //Assmue is a vector. - Eigen::Map prior_ref(paraminfo.pointer, paraminfo.size); - MatrixXd A(paraminfo.size, paraminfo.size); - A.setIdentity(); - if (paraminfo.type == LANDMARK) { - A *= rho_landmark; - } else { - //Not implement yet - } - auto factor = new ceres::NormalPrior(A, prior_ref); - problem->AddResidualBlock(factor, nullptr, pointer); + for (auto it : all_estimating_params) { + auto pointer = it.first; + auto paraminfo = it.second; + auto& consenus_param = consenus_params[pointer]; + if (consenus_param.local_only) { + // Add normal prior factor + // Assmue is a vector. + Eigen::Map prior_ref(paraminfo.pointer, paraminfo.size); + MatrixXd A(paraminfo.size, paraminfo.size); + A.setIdentity(); + if (paraminfo.type == LANDMARK) { + A *= rho_landmark; + } else { + // Not implement yet + } + auto factor = new ceres::NormalPrior(A, prior_ref); + problem->AddResidualBlock(factor, nullptr, pointer); + } else { + if (IsSE3(paraminfo.type)) { + // Is SE(3) pose. + Swarm::Pose pose_global(consenus_param.param_global.data()); + Swarm::Pose pose_local(pointer); + Swarm::Pose pose_err = Swarm::Pose::DeltaPose(pose_global, pose_local); + auto& tilde = consenus_param.param_tilde; + tilde += (1 + config.relaxation_alpha) * pose_err.tangentSpace(); + // printf("update tilde %d:\n", paraminfo.id); + // printf("[updateTilde%d] frame %d pose_local %s pose_global %s tilde + // :", self_id, + // paraminfo.id, pose_local.toStr().c_str(), + // pose_global.toStr().c_str()); + // std::cout << "tilde" << tilde.transpose() << std::endl << std::endl; + auto factor = new ConsenusPoseFactor( + pose_global.pos(), pose_global.att(), tilde.segment<3>(0), + tilde.segment<3>(3), rho_T, rho_theta); + problem->AddResidualBlock(factor, nullptr, pointer); + } else { + // Is euclidean. + printf("[updateTilde] unknow param type %d id %d", paraminfo.type, + paraminfo.id); + VectorXd x_global = consenus_param.param_global; + Eigen::Map x_local(pointer, consenus_param.global_size); + auto& tilde = consenus_param.param_tilde; + tilde += x_local - x_global; + MatrixXd A(paraminfo.size, paraminfo.size); + A.setIdentity(); + if (paraminfo.type == LANDMARK) { + A *= rho_landmark; } else { - if (IsSE3(paraminfo.type)) { - //Is SE(3) pose. - Swarm::Pose pose_global(consenus_param.param_global.data()); - Swarm::Pose pose_local(pointer); - Swarm::Pose pose_err = Swarm::Pose::DeltaPose(pose_global, pose_local); - auto & tilde = consenus_param.param_tilde; - tilde += (1+config.relaxation_alpha) * pose_err.tangentSpace(); - // printf("update tilde %d:\n", paraminfo.id); - // printf("[updateTilde%d] frame %d pose_local %s pose_global %s tilde :", self_id, - // paraminfo.id, pose_local.toStr().c_str(), pose_global.toStr().c_str()); - // std::cout << "tilde" << tilde.transpose() << std::endl << std::endl; - auto factor = new ConsenusPoseFactor(pose_global.pos(), pose_global.att(), - tilde.segment<3>(0), tilde.segment<3>(3), rho_T, rho_theta); - problem->AddResidualBlock(factor, nullptr, pointer); - } else { - //Is euclidean. - printf("[updateTilde] unknow param type %d id %d", paraminfo.type, paraminfo.id); - VectorXd x_global = consenus_param.param_global; - Eigen::Map x_local(pointer, consenus_param.global_size); - auto & tilde = consenus_param.param_tilde; - tilde += x_local - x_global; - MatrixXd A(paraminfo.size, paraminfo.size); - A.setIdentity(); - if (paraminfo.type == LANDMARK) { - A *= rho_landmark; - } else { - //Not implement yet - } - auto factor = new ceres::NormalPrior(A, x_global - tilde); - problem->AddResidualBlock(factor, nullptr, pointer); - } + // Not implement yet } + auto factor = new ceres::NormalPrior(A, x_global - tilde); + problem->AddResidualBlock(factor, nullptr, pointer); + } } + } } void ConsensusSolver::updateGlobal() { - //Assmue all drone's information has been received. - for (auto it : all_estimating_params) { - auto pointer = it.first; - auto paraminfo = it.second; - if (consenus_params[pointer].local_only) { - continue; + // Assmue all drone's information has been received. + for (auto it : all_estimating_params) { + auto pointer = it.first; + auto paraminfo = it.second; + if (consenus_params[pointer].local_only) { + continue; + } + if (IsSE3(paraminfo.type)) { + // Average SE(3) pose. + std::vector poses; + for (auto drone_id : state->availableDrones()) { + if (remote_params.at(pointer).find(drone_id) == + remote_params.at(pointer).end()) { + // printf("\033[0;31mError: remote_params %d type %d of drone %d not + // found in remote_params.\033[0m\n", + // paraminfo.id, paraminfo.type, drone_id); + continue; } - if (IsSE3(paraminfo.type)) { - //Average SE(3) pose. - std::vector poses; - for (auto drone_id: state->availableDrones()) { - if (remote_params.at(pointer).find(drone_id) == remote_params.at(pointer).end()) { - // printf("\033[0;31mError: remote_params %d type %d of drone %d not found in remote_params.\033[0m\n", - // paraminfo.id, paraminfo.type, drone_id); - continue; - } - Swarm::Pose pose_(remote_params.at(pointer).at(drone_id).data()); - poses.emplace_back(pose_); - } - if (poses.size() == 0) { - printf("\033[0;31m[ConsensusSolver::updateGlobal] Error: no remote pose found for %d type %d.\033[0m\n", - paraminfo.id, paraminfo.type); - } - auto pose_global = Swarm::Pose::averagePoses(poses); - pose_global.to_vector(consenus_params[pointer].param_global.data()); - } else { - //Average vectors - VectorXd x_global_sum(paraminfo.eff_size); - x_global_sum.setZero(); - int drone_count = 0; - for (auto drone_id: state->availableDrones()) { - if (remote_params.find(pointer) == remote_params.end()) { - // printf("\033[0;31mError: remote_params %d of type %d not found in remote_params.\033[0m\n", - // paraminfo.id, paraminfo.type); - continue; - } - if (remote_params.at(pointer).find(drone_id) == remote_params.at(pointer).end()) { - printf("\033[0;31mError: remote_params %d type %d of drone %d not found in remote_params.\033[0m\n", - paraminfo.id, paraminfo.type, drone_id); - continue; - } - x_global_sum += remote_params.at(pointer).at(drone_id); - drone_count += 1; - } - if (drone_count == 0) { - printf("\033[0;31mError: no drone found for %d of type %d.\033[0m\n", paraminfo.id, paraminfo.type); - continue; - } - consenus_params.at(pointer).param_global = x_global_sum / drone_count; + Swarm::Pose pose_(remote_params.at(pointer).at(drone_id).data()); + poses.emplace_back(pose_); + } + if (poses.size() == 0) { + printf( + "\033[0;31m[ConsensusSolver::updateGlobal] Error: no remote pose " + "found for %d type %d.\033[0m\n", + paraminfo.id, paraminfo.type); + } + auto pose_global = Swarm::Pose::averagePoses(poses); + pose_global.to_vector(consenus_params[pointer].param_global.data()); + } else { + // Average vectors + VectorXd x_global_sum(paraminfo.eff_size); + x_global_sum.setZero(); + int drone_count = 0; + for (auto drone_id : state->availableDrones()) { + if (remote_params.find(pointer) == remote_params.end()) { + // printf("\033[0;31mError: remote_params %d of type %d not found in + // remote_params.\033[0m\n", + // paraminfo.id, paraminfo.type); + continue; } + if (remote_params.at(pointer).find(drone_id) == + remote_params.at(pointer).end()) { + printf( + "\033[0;31mError: remote_params %d type %d of drone %d not found " + "in remote_params.\033[0m\n", + paraminfo.id, paraminfo.type, drone_id); + continue; + } + x_global_sum += remote_params.at(pointer).at(drone_id); + drone_count += 1; + } + if (drone_count == 0) { + printf("\033[0;31mError: no drone found for %d of type %d.\033[0m\n", + paraminfo.id, paraminfo.type); + continue; + } + consenus_params.at(pointer).param_global = x_global_sum / drone_count; } - remote_params.clear(); + } + remote_params.clear(); } ceres::Solver::Summary ConsensusSolver::solveLocalStep() { - ceres::Solver::Summary summary; - ceres::Solve(config.ceres_options, problem, &summary); - // std::cout << summary.FullReport() << std::endl; - return summary; + ceres::Solver::Summary summary; + ceres::Solve(config.ceres_options, problem, &summary); + // std::cout << summary.FullReport() << std::endl; + return summary; } -} \ No newline at end of file +} // namespace D2Common \ No newline at end of file diff --git a/d2common/src/solver/consenus_factor.cpp b/d2common/src/solver/consenus_factor.cpp index 012c9a55..2c24b11d 100644 --- a/d2common/src/solver/consenus_factor.cpp +++ b/d2common/src/solver/consenus_factor.cpp @@ -1,44 +1,53 @@ #include + #include namespace D2Common { -ConsenusPoseFactor::ConsenusPoseFactor(Eigen::Vector3d _t_ref, Eigen::Quaterniond _q_ref, - Eigen::Vector3d _t_tilde, Eigen::Vector3d _theta_tilde, double rho_T, double rho_theta): - t_ref(_t_ref), q_ref(_q_ref), t_tilde(_t_tilde), theta_tilde(_theta_tilde) -{ - q_sqrt_info = Eigen::Matrix3d::Identity() * rho_T; - T_sqrt_info = Eigen::Matrix3d::Identity() * rho_theta; +ConsenusPoseFactor::ConsenusPoseFactor(Eigen::Vector3d _t_ref, + Eigen::Quaterniond _q_ref, + Eigen::Vector3d _t_tilde, + Eigen::Vector3d _theta_tilde, + double rho_T, double rho_theta) + : t_ref(_t_ref), + q_ref(_q_ref), + t_tilde(_t_tilde), + theta_tilde(_theta_tilde) { + q_sqrt_info = Eigen::Matrix3d::Identity() * rho_T; + T_sqrt_info = Eigen::Matrix3d::Identity() * rho_theta; } -bool ConsenusPoseFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const { - Eigen::Map T_local(parameters[0]); - Eigen::Map q_local(parameters[0] + 3); - Eigen::Map T_err(residuals); - Eigen::Map theta_err(residuals + 3); - - Matrix3d R_ref_inv = q_ref.toRotationMatrix().transpose(); +bool ConsenusPoseFactor::Evaluate(double const *const *parameters, + double *residuals, double **jacobians) const { + Eigen::Map T_local(parameters[0]); + Eigen::Map q_local(parameters[0] + 3); + Eigen::Map T_err(residuals); + Eigen::Map theta_err(residuals + 3); + + Matrix3d R_ref_inv = q_ref.toRotationMatrix().transpose(); - Eigen::Quaterniond q_err = q_ref.inverse() * q_local; - theta_err = 2.0 * q_err.vec(); - theta_err = q_sqrt_info * (theta_err + theta_tilde); - T_err = T_sqrt_info*(R_ref_inv*(T_local - t_ref) + t_tilde); - if (T_err.hasNaN()) { - std::cout << "q_ref" << q_ref.coeffs().transpose() << std::endl; - std::cout << "t_ref" << t_ref.transpose() << std::endl; - std::cout << "T_local" << T_local.transpose() << std::endl; - std::cout << "t_tilde" << t_tilde.transpose() << std::endl; - std::cout << "T_err" << T_err.transpose() << std::endl; - std::cout << "T_sqrt_info\n" << T_sqrt_info << std::endl; - std::cout << "R_ref_inv\n" << R_ref_inv << std::endl; - } - if (jacobians) { - //Fill in jacobians... - Eigen::Map> jacobian_pose_local(jacobians[0]); - jacobian_pose_local.setZero(); - jacobian_pose_local.block<3, 3>(0, 0) = T_sqrt_info * R_ref_inv; - jacobian_pose_local.block<3, 3>(3, 3) = q_sqrt_info * Utility::Qleft(q_err).bottomRightCorner<3, 3>(); - } - return true; + Eigen::Quaterniond q_err = q_ref.inverse() * q_local; + theta_err = 2.0 * q_err.vec(); + theta_err = q_sqrt_info * (theta_err + theta_tilde); + T_err = T_sqrt_info * (R_ref_inv * (T_local - t_ref) + t_tilde); + if (T_err.hasNaN()) { + std::cout << "q_ref" << q_ref.coeffs().transpose() << std::endl; + std::cout << "t_ref" << t_ref.transpose() << std::endl; + std::cout << "T_local" << T_local.transpose() << std::endl; + std::cout << "t_tilde" << t_tilde.transpose() << std::endl; + std::cout << "T_err" << T_err.transpose() << std::endl; + std::cout << "T_sqrt_info\n" << T_sqrt_info << std::endl; + std::cout << "R_ref_inv\n" << R_ref_inv << std::endl; + } + if (jacobians) { + // Fill in jacobians... + Eigen::Map> + jacobian_pose_local(jacobians[0]); + jacobian_pose_local.setZero(); + jacobian_pose_local.block<3, 3>(0, 0) = T_sqrt_info * R_ref_inv; + jacobian_pose_local.block<3, 3>(3, 3) = + q_sqrt_info * Utility::Qleft(q_err).bottomRightCorner<3, 3>(); + } + return true; } -} \ No newline at end of file +} // namespace D2Common \ No newline at end of file diff --git a/d2common/src/solver/pose_local_parameterization.cpp b/d2common/src/solver/pose_local_parameterization.cpp index 5cb77d98..c3f01969 100644 --- a/d2common/src/solver/pose_local_parameterization.cpp +++ b/d2common/src/solver/pose_local_parameterization.cpp @@ -1,37 +1,39 @@ /******************************************************* - * Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology - * + * Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science + *and Technology + * * This file is part of VINS. - * + * * Licensed under the GNU General Public License v3.0; * you may not use this file except in compliance with the License. *******************************************************/ #include namespace D2Common { -bool PoseLocalParameterization::Plus(const double *x, const double *delta, double *x_plus_delta) const -{ - Eigen::Map _p(x); - Eigen::Map _q(x + 3); +bool PoseLocalParameterization::Plus(const double *x, const double *delta, + double *x_plus_delta) const { + Eigen::Map _p(x); + Eigen::Map _q(x + 3); - Eigen::Map dp(delta); + Eigen::Map dp(delta); - Eigen::Quaterniond dq = Utility::deltaQ(Eigen::Map(delta + 3)); + Eigen::Quaterniond dq = + Utility::deltaQ(Eigen::Map(delta + 3)); - Eigen::Map p(x_plus_delta); - Eigen::Map q(x_plus_delta + 3); + Eigen::Map p(x_plus_delta); + Eigen::Map q(x_plus_delta + 3); - p = _p + dp; - q = (_q * dq).normalized(); + p = _p + dp; + q = (_q * dq).normalized(); - return true; + return true; } -bool PoseLocalParameterization::ComputeJacobian(const double *x, double *jacobian) const -{ - Eigen::Map> j(jacobian); - j.topRows<6>().setIdentity(); - j.bottomRows<1>().setZero(); +bool PoseLocalParameterization::ComputeJacobian(const double *x, + double *jacobian) const { + Eigen::Map> j(jacobian); + j.topRows<6>().setIdentity(); + j.bottomRows<1>().setZero(); - return true; + return true; } -} \ No newline at end of file +} // namespace D2Common \ No newline at end of file diff --git a/d2common/src/test.cpp b/d2common/src/test.cpp index 8c8aebde..351cd03a 100644 --- a/d2common/src/test.cpp +++ b/d2common/src/test.cpp @@ -3,18 +3,14 @@ using namespace D2Common; void testQuaternionAveraging() { - std::vector qs { - Eigen::Quaterniond(1.0, -0.2, 0.0, 0.0), - Eigen::Quaterniond(1.0, 0.4, 0.0, 0.0), - Eigen::Quaterniond(-1.0, 0.0, 0.0, 0.0) - }; - for (auto q: qs) { - q.normalize(); - } - auto q = Utility::averageQuaterions(qs); - std::cout << "q.w() " << q.w() << " xyz " << q.vec().transpose() << std::endl; + std::vector qs{Eigen::Quaterniond(1.0, -0.2, 0.0, 0.0), + Eigen::Quaterniond(1.0, 0.4, 0.0, 0.0), + Eigen::Quaterniond(-1.0, 0.0, 0.0, 0.0)}; + for (auto q : qs) { + q.normalize(); + } + auto q = Utility::averageQuaterions(qs); + std::cout << "q.w() " << q.w() << " xyz " << q.vec().transpose() << std::endl; } -int main() { - testQuaternionAveraging(); -} \ No newline at end of file +int main() { testQuaternionAveraging(); } \ No newline at end of file diff --git a/d2frontend/CMakeLists.txt b/d2frontend/CMakeLists.txt index 73f72f97..4f21ed29 100644 --- a/d2frontend/CMakeLists.txt +++ b/d2frontend/CMakeLists.txt @@ -25,27 +25,32 @@ find_package(catkin REQUIRED COMPONENTS ) SET("OpenCV_DIR" "/usr/local/share/OpenCV/") +SET("TENSORRT_UTILS_INC" "../tensorrt_utils/include/") + find_package(OpenCV REQUIRED) find_package(Eigen3 REQUIRED) find_package(lcm REQUIRED) find_package(yaml-cpp REQUIRED) find_package(opengv REQUIRED) +find_package(CUDA REQUIRED) set(Torch_DIR "$ENV{HOME}/source/libtorch/share/cmake/Torch" CACHE STRING "Path of libtorch") set(TORCH_INSTALL_PREFIX "$ENV{HOME}/source/libtorch" CACHE STRING "Path of libtorch") +set(TORCH_INCLUDE_DIRS "/root/libtorch/include") +set(TORCH_LIBRARIES "/root/libtorch/lib") #Find torch Optional -find_package(Torch) -if (NOT Torch_FOUND) - message("PyTorch not found") - set(TORCH_INCLUDE_DIRS "$ENV{HOME}/source/libtorch/" CACHE STRING "Path of torch install path") - set(TORCH_INCLUDE_DIRS - ${TORCH_INSTALL_PREFIX}/include - ${TORCH_INSTALL_PREFIX}/include/torch/csrc/api/include) - set(TORCH_LIBRARIES - ${TORCH_INSTALL_PREFIX}/lib/libtorch.so - ${TORCH_INSTALL_PREFIX}/lib/libc10.so - ${TORCH_INSTALL_PREFIX}/lib/libtorch_cpu.so) -endif() +find_package(Torch REQUIRED) +# if (NOT Torch_FOUND) +# message("PyTorch not found") +# set(TORCH_INCLUDE_DIRS "$ENV{HOME}/source/libtorch/" CACHE STRING "Path of torch install path") +# set(TORCH_INCLUDE_DIRS +# ${TORCH_INSTALL_PREFIX}/include +# ${TORCH_INSTALL_PREFIX}/include/torch/csrc/api/include) +# set(TORCH_LIBRARIES +# ${TORCH_INSTALL_PREFIX}/lib/libtorch.so +# ${TORCH_INSTALL_PREFIX}/lib/libc10.so +# ${TORCH_INSTALL_PREFIX}/lib/libtorch_cpu.so) +# endif() find_package(Boost REQUIRED COMPONENTS program_options) include_directories(${TORCH_INCLUDE_DIRS}) @@ -65,6 +70,7 @@ catkin_package( ## Specify additional locations of header files ## Your package locations should be listed before other locations + include_directories( /usr/local/include/ include @@ -86,6 +92,8 @@ add_library(libd2frontend src/d2landmark_manager.cpp ) +add_subdirectory(../tensorrt_utils tensorrt_utils) + add_library(${PROJECT_NAME}_nodelet src/d2frontend_nodelet.cpp ) @@ -108,7 +116,8 @@ set_property(TARGET libd2frontend PROPERTY CXX_STANDARD 17) add_library(loop_cnn src/CNN/superpoint_common.cpp - src/CNN/superpoint_onnx.cpp + # src/CNN/superpoint_onnx.cpp + src/CNN/superpoint.cpp src/CNN/superglue_onnx.cpp src/loop_utils.cpp src/d2frontend_params.cpp @@ -117,24 +126,38 @@ set_property(TARGET loop_cnn PROPERTY CXX_STANDARD 17) #Use tensorrt and onnx -target_link_libraries(loop_cnn opencv_dnn +target_link_libraries(loop_cnn + opencv_dnn onnxruntime - ${TORCH_LIBRARIES} opengv -) - -add_executable(loop_tensorrt_test - tests/loop_tensorrt_test.cpp -) - -target_link_libraries(loop_tensorrt_test - loop_cnn - dw - ${YAML_CPP_LIBRARIES} - ${TORCH_LIBRARIES} - ${OpenCV_LIBRARIES} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES}) + nvinfer + nvinfer_plugin + nvonnxparser + tensorrt_utils +) + +target_include_directories(loop_cnn PUBLIC + /usr/include + /usr/local/include + ${CMAKE_CURRENT_SOURCE_DIR}/include + # ${ONNXRUNTIME_INC_DIR} + ${CUDA_INCLUDE_DIRS} + ${TENSORRT_UTILS_INC} + ${Boost_LIBRARIES} +) + +# add_executable(loop_tensorrt_test +# tests/loop_tensorrt_test.cpp +# ) + +# target_link_libraries(loop_tensorrt_test +# loop_cnn +# dw +# ${YAML_CPP_LIBRARIES} +# ${TORCH_LIBRARIES} +# ${OpenCV_LIBRARIES} +# ${catkin_LIBRARIES} +# ${Boost_LIBRARIES}) add_executable(camera_undistort_test tests/camera_undistort_test.cpp @@ -203,3 +226,4 @@ target_link_libraries(${PROJECT_NAME}_spy dw libd2frontend ) + diff --git a/d2frontend/include/d2frontend/CNN/superpoint.h b/d2frontend/include/d2frontend/CNN/superpoint.h new file mode 100644 index 00000000..92ea5786 --- /dev/null +++ b/d2frontend/include/d2frontend/CNN/superpoint.h @@ -0,0 +1,89 @@ +#ifndef SUPER_POINT_H_ +#define SUPER_POINT_H_ + +#include +#include +#include +#include +#include +#include +#include "d2frontend/utils.h" + +#include "tensorrt_utils/buffers.h" + +namespace D2FrontEnd { +class SuperPoint { +public: + struct SuperPointConfig { + int32_t max_keypoints = 100; + int32_t remove_borders = 1; + int32_t dla_core; + int32_t fp_16; + int32_t input_width; + int32_t input_height; + int32_t superpoint_pca_dims = -1; + float keypoint_threshold = 0.015; + std::vector input_tensor_names; + std::vector output_tensor_names; + std::string onnx_path; + std::string engine_path; + std::string pca_mean_path; + std::string pca_comp_path; + bool enable_pca; + }; + explicit SuperPoint(const SuperPointConfig &super_point_config); + bool build(); + // bool infer(const cv::Mat &image, Eigen::Matrix &features); + bool infer(const cv::Mat &image, + std::vector &keypoints, + std::vector &descriptors, + std::vector & scores); + + bool infer(const cv::Mat & input, std::vector & keypoints, + std::vector & local_descriptors, std::vector & scores); //a middle level for D2SLAM + + void saveEngine(); + bool deserializeEngine(); + void visualization(const std::string &image_name, const cv::Mat &image,std::vector &keypoints); +private: + SuperPointConfig super_point_config_; + nvinfer1::Dims input_dims_{}; + nvinfer1::Dims semi_dims_{}; + nvinfer1::Dims desc_dims_{}; + std::shared_ptr engine_; + std::shared_ptr context_; + std::vector> keypoints_; + std::vector> descriptors_; + Eigen::MatrixXf pca_comp_T_; + Eigen::RowVectorXf pca_mean_; + + bool constructNetwork(tensorrt_common::TensorRTUniquePtr &builder, + tensorrt_common::TensorRTUniquePtr &network, + tensorrt_common::TensorRTUniquePtr &config, + tensorrt_common::TensorRTUniquePtr &parser) const; + + bool processInput(const tensorrt_buffer::BufferManager &buffers, const cv::Mat &image); + + // bool processOutput(const tensorrt_buffer::BufferManager &buffers, Eigen::Matrix &features); + + bool processOutput(const tensorrt_buffer::BufferManager &buffers, std::vector &keypoints, + std::vector &descriptors, std::vector &scores); + + void removeBorders( std::vector &keypoints, std::vector &scores, int border, int height, + int width); + + std::vector sortIndexes(std::vector &data); + + void topKeypoints( std::vector &keypoints, std::vector &scores, int k); + + void findHighScoreIndex(std::vector &scores, std::vector &keypoints, int h, int w, + float threshold); + + void sampleDescriptors( std::vector &keypoints, float *des_map, + std::vector &descriptors, int dim, int h, int w, int s = 8); +}; + + +} // namespace D2FrontEnd + +#endif \ No newline at end of file diff --git a/d2frontend/include/d2frontend/d2featuretracker.h b/d2frontend/include/d2frontend/d2featuretracker.h index bad27693..b313c0dc 100644 --- a/d2frontend/include/d2frontend/d2featuretracker.h +++ b/d2frontend/include/d2frontend/d2featuretracker.h @@ -5,6 +5,7 @@ #include #include #include +#include using namespace Eigen; @@ -29,6 +30,7 @@ struct D2FTConfig { double ransacReprojThreshold = 10; double max_pts_velocity_time=0.3; int remote_min_match_num = 30; + int min_stereo_points = 10; bool double_counting_common_feature = false; bool enable_superglue_local = false; bool enable_superglue_remote = false; @@ -37,11 +39,21 @@ struct D2FTConfig { bool enable_motion_prediction_local = false; bool enable_search_local_aera_remote = false; //Enable motion prediction searching for remote drones. double search_local_max_dist = 0.04; //To multiply with width + double search_local_max_dist_lr = 0.2; //To multiply with width double knn_match_ratio = 0.8; std::string output_folder = "/root/output/"; std::string superglue_model_path; - double landmark_distance_assumption = 2.0; // For uninitialized landmark, assume it is 3m away + double landmark_distance_assumption = 100.0; // For uninitialized landmark, assume it is 100 meter away int frame_step = 2; + bool track_from_keyframe = true; + bool lr_match_use_lk = true; + bool lk_lk_use_pred = true; + bool sp_track_use_lk = true; + + //frontend thread frequency. + float stereo_frame_thread_rate = 20.0; + float loop_detection_thread_rate = 1.0; + float lcm_thread_rate = 1.0; }; struct TrackReport { @@ -66,13 +78,6 @@ struct TrackReport { } }; -struct LKImageInfo { - FrameIdType frame_id; - std::vector lk_pts; - std::vector lk_ids; - cv::Mat image; - std::vector pyr; -}; class SuperGlueOnnx; @@ -100,8 +105,9 @@ class D2FeatureTracker { int keyframe_count = 0; int frame_count = 0; bool inited = false; - std::map prev_lk_info; //frame.camera_index->image - std::pair createLKLandmark(const VisualImageDesc & frame, cv::Point2f pt, LandmarkIdType landmark_id = -1); + std::map> keyframe_lk_infos; //frame.camera_index->image + std::pair createLKLandmark(const VisualImageDesc & frame, + cv::Point2f pt, LandmarkIdType landmark_id = -1, LandmarkType type=LandmarkType::FlowLandmark); std::recursive_mutex track_lock; std::recursive_mutex keyframe_lock; std::recursive_mutex lmanager_lock; @@ -110,8 +116,9 @@ class D2FeatureTracker { std::map> landmark_predictions_matched_viz; TrackReport trackLK(VisualImageDesc & frame); - TrackReport track(const VisualImageDesc & left_frame, VisualImageDesc & right_frame, bool enable_lk=true, TrackLRType type=WHOLE_IMG_MATCH); - TrackReport trackLK(const VisualImageDesc & frame, VisualImageDesc & right_frame, TrackLRType type=WHOLE_IMG_MATCH); + TrackReport track(const VisualImageDesc & left_frame, VisualImageDesc & right_frame, + bool enable_lk=true, TrackLRType type=WHOLE_IMG_MATCH, bool use_lk_for_left_right_track = true); + TrackReport trackLK(const VisualImageDesc & frame, VisualImageDesc & right_frame, TrackLRType type=WHOLE_IMG_MATCH, bool use_lk_for_left_right_track = true); TrackReport track(VisualImageDesc & frame, const Swarm::Pose & motion_prediction=Swarm::Pose()); TrackReport trackRemote(VisualImageDesc & frame, const VisualImageDesc & prev_frame, bool use_motion_predict=false, const Swarm::Pose & motion_prediction=Swarm::Pose()); @@ -119,7 +126,8 @@ class D2FeatureTracker { void processFrame(VisualImageDescArray & frames, bool is_keyframe); bool isKeyframe(const TrackReport & reports); Vector3d extractPointVelocity(const LandmarkPerFrame & lpf) const; - std::pair getPreviousLandmarkFrame(const LandmarkPerFrame & lpf) const; + std::pair getPreviousLandmarkFrame(const LandmarkPerFrame & lpf, FrameIdType keyframe_id=-1) const; + const VisualImageDescArray& getLatestKeyframe() const; void draw(const VisualImageDesc & frame, bool is_keyframe, const TrackReport & report) const; void draw(const VisualImageDesc & lframe, VisualImageDesc & rframe, bool is_keyframe, const TrackReport & report) const; @@ -133,6 +141,8 @@ class D2FeatureTracker { SuperGlueOnnx * superglue = nullptr; bool matchLocalFeatures(const VisualImageDesc & img_desc_a, const VisualImageDesc & img_desc_b, std::vector & ids_down_to_up, const MatchLocalFeatureParams & param); + std::map predictLandmarksWithExtrinsic(int camera_index, + std::vector pts_ids, std::vector pts_3d_norm, const Swarm::Pose & cam_pose_a, const Swarm::Pose & cam_pose_b) const; std::vector predictLandmarks(const VisualImageDesc & img_desc_a, const Swarm::Pose & cam_pose_a, const Swarm::Pose & cam_pose_b, bool use_extrinsic=false) const; public: diff --git a/d2frontend/include/d2frontend/d2frontend.h b/d2frontend/include/d2frontend/d2frontend.h index 1e624f76..e2950ae9 100644 --- a/d2frontend/include/d2frontend/d2frontend.h +++ b/d2frontend/include/d2frontend/d2frontend.h @@ -17,6 +17,7 @@ #include #include #include +#include using namespace std::chrono; using namespace swarm_msgs; @@ -29,7 +30,10 @@ class D2FeatureTracker; class LoopDetector; class D2Frontend { typedef image_transport::SubscriberFilter ImageSubscriber; + protected: + using ApproSync = message_filters::sync_policies::ApproximateTime; + LoopDetector * loop_detector = nullptr; LoopCam * loop_cam = nullptr; LoopNet * loop_net = nullptr; @@ -42,7 +46,7 @@ class D2Frontend { std::queue loop_queue; std::mutex loop_lock; image_transport::ImageTransport * it_; - + virtual void Init(ros::NodeHandle & nh); virtual void backendFrameCallback(const VisualImageDescArray & viokf) {}; void onLoopConnection (LoopEdge & loop_con, bool is_local = false); @@ -66,7 +70,6 @@ class D2Frontend { virtual void processRemoteImage(VisualImageDescArray & frame_desc, bool succ_track); void processStereoframe(const StereoFrame & stereoframe); - void loopDetectionThread(); void addToLoopQueue(const VisualImageDescArray & viokf); @@ -77,18 +80,64 @@ class D2Frontend { ros::Publisher keyframe_pub; ImageSubscriber * image_sub_l, *image_sub_r; - message_filters::TimeSynchronizer * sync; + message_filters::Synchronizer * sync; image_transport::Subscriber image_sub_single; std::thread th, th_loop_det; bool received_image = false; ros::Timer timer, loop_timer; + + //data buffe + std::queue> stereo_frame_q_; + + std::mutex stereo_frame_buffer_lock_; + int32_t visual_frame_size_ = 2; + + //process stereo frame thread + std::thread stereo_frame_thread_; + bool stereo_frame_thread_running_ = false; + std::unique_ptr stereo_frame_thread_rate_ptr_; + void processStereoFrameThread(); + + //Loop detection thread + std::thread loop_detection_thread_; + bool loop_detection_thread_running_ = false; + std::unique_ptr loop_detection_thread_rate_ptr_; + void loopDetectionThread(); + + //lcm thread + std::thread lcm_thread_; + bool lcm_thread_running_ = false; + std::unique_ptr lcm_thread_rate_ptr_; + void lcmThread(); + + void startThread(){ + stereo_frame_thread_running_ = true; + stereo_frame_thread_ = std::thread(&D2Frontend::processStereoFrameThread, this); + loop_detection_thread_running_ = true; + loop_detection_thread_ = std::thread(&D2Frontend::loopDetectionThread, this); + lcm_thread_running_ = true; + lcm_thread_ = std::thread(&D2Frontend::lcmThread, this); + } + + void stopThreads(){ + stereo_frame_thread_running_ = false; + stereo_frame_thread_.join(); + loop_detection_thread_running_ = false; + loop_detection_thread_.join(); + lcm_thread_running_ = false; + lcm_thread_.join(); + }; + + + public: D2Frontend (); virtual Swarm::Pose getMotionPredict(double stamp) const {return Swarm::Pose();}; + void stopFrontend(){ + stopThreads(); + }; -protected: - virtual void Init(ros::NodeHandle & nh); }; } \ No newline at end of file diff --git a/d2frontend/include/d2frontend/d2frontend_params.h b/d2frontend/include/d2frontend/d2frontend_params.h index c79985cf..d435dc34 100644 --- a/d2frontend/include/d2frontend/d2frontend_params.h +++ b/d2frontend/include/d2frontend/d2frontend_params.h @@ -8,6 +8,8 @@ #include #include #include +#include + #define ACCEPT_LOOP_YAW (30) //ACCEPT MAX Yaw @@ -39,6 +41,7 @@ class FisheyeUndist; namespace D2FrontEnd { using D2Common::CameraConfig; +using D2Common::ESTIMATION_MODE; enum TrackLRType { WHOLE_IMG_MATCH = 0, @@ -57,6 +60,7 @@ struct D2FrontendParams { std::string OUTPUT_PATH; int width; int height; + int image_queue_size; //this size is critical for the realtime performance double recv_msg_duration = 0.5; double feature_min_dist = 20; int total_feature_num = 150; @@ -76,6 +80,7 @@ struct D2FrontendParams { int min_receive_images = 2; D2Common::PGO_MODE pgo_mode; + ESTIMATION_MODE estimation_mode; //Debug params bool send_img; @@ -112,6 +117,7 @@ struct D2FrontendParams { std::vector camera_seq; bool show_raw_image = false; + //Configs of submodules LoopCamConfig * loopcamconfig; LoopDetectorConfig * loopdetectorconfig; @@ -119,9 +125,12 @@ struct D2FrontendParams { D2FrontendParams(ros::NodeHandle &); D2FrontendParams() {} - void readCameraCalibrationfromFile(const std::string & path); + void readCameraCalibrationfromFile(const std::string & path, int32_t extrinsic_parameter_type = 1); void generateCameraModels(cv::FileStorage & fsSettings, std::string config_path); void readCameraConfigs(cv::FileStorage & fsSettings, std::string config_path); + static std::pair readCameraConfig( + const std::string& camera_name, const YAML::Node& config, int32_t extrinsic_parameter_type = 1); + }; extern D2FrontendParams * params; diff --git a/d2frontend/include/d2frontend/d2landmark_manager.h b/d2frontend/include/d2frontend/d2landmark_manager.h index 135f144c..2d977757 100644 --- a/d2frontend/include/d2frontend/d2landmark_manager.h +++ b/d2frontend/include/d2frontend/d2landmark_manager.h @@ -12,11 +12,17 @@ class LandmarkManager { int count = 0; typedef std::lock_guard Guard; mutable std::recursive_mutex state_lock; + public: int total_lm_per_frame_num = 0; virtual int addLandmark(const LandmarkPerFrame & lm); virtual void updateLandmark(const LandmarkPerFrame & lm); LandmarkPerId & at(LandmarkIdType i) { + if (landmark_db.find(i) == landmark_db.end()) { + // Throw error with i + std::cout << "Landmark id " << i << " not found!" << std::endl; + throw std::runtime_error("Landmark id not found!"); + } return landmark_db.at(i); } const LandmarkPerId & at(LandmarkIdType i) const { @@ -43,6 +49,8 @@ class LandmarkManager { std::vector getInitializedLandmarks(int min_tracks) const; FrameIdType getLandmarkBaseFrame(LandmarkIdType landmark_id) const; bool hasLandmark(LandmarkIdType landmark_id) const; + std::vector findCommonLandmarkIds(FrameIdType frame_id1, FrameIdType frame_id2) const; + std::vector> findCommonLandmarkPerFrames(FrameIdType frame_id1, FrameIdType frame_id2) const; }; } \ No newline at end of file diff --git a/d2frontend/include/d2frontend/loop_cam.h b/d2frontend/include/d2frontend/loop_cam.h index c442a94c..1383695b 100644 --- a/d2frontend/include/d2frontend/loop_cam.h +++ b/d2frontend/include/d2frontend/loop_cam.h @@ -6,12 +6,14 @@ #include "d2frontend_params.h" #include "CNN/onnx_generic.h" #include "CNN/mobilenetvlad_onnx.h" -#include "CNN/superpoint_onnx.h" #include #include #include #include "d2common/d2frontend_types.h" #include +#include + +#include "CNN/superpoint.h" //#include @@ -56,6 +58,8 @@ struct LoopCamConfig bool enable_undistort_image; //Undistort image before feature detection std::string netvlad_int8_calib_table_name; std::string superpoint_int8_calib_table_name; + + SuperPoint::SuperPointConfig superpoint_config; }; class LoopCam { @@ -70,7 +74,9 @@ class LoopCam { std::fstream fsp; std::vector undistortors; MobileNetVLADONNX * netvlad_onnx = nullptr; - SuperPointONNX * superpoint_onnx = nullptr; + // SuperPointONNX * superpoint_onnx = nullptr; + std::unique_ptr superpoint_ptr = nullptr; + public: // LoopDetector * loop_detector = nullptr; LoopCam(LoopCamConfig config, ros::NodeHandle & nh); diff --git a/d2frontend/include/d2frontend/utils.h b/d2frontend/include/d2frontend/utils.h index cc848f66..bb4fa51e 100644 --- a/d2frontend/include/d2frontend/utils.h +++ b/d2frontend/include/d2frontend/utils.h @@ -1,25 +1,40 @@ #pragma once #include -#include -#include -#include #include +#include #include #include +#include +#include +#include namespace D2FrontEnd { using D2Common::LandmarkIdType; +using LandmarkType = D2Common::LandmarkType; + +template struct LKImageInfo { + std::vector lk_pts; + std::vector lk_pts_3d_norm; + std::vector lk_ids; + std::vector lk_local_index; + std::vector lk_types; + std::vector pyr; +}; + +using LKImageInfoCPU = LKImageInfo; +using LKImageInfoGPU = LKImageInfo; cv_bridge::CvImagePtr getImageFromMsg(const sensor_msgs::Image &img_msg); -cv_bridge::CvImagePtr getImageFromMsg(const sensor_msgs::ImageConstPtr &img_msg); -cv::Mat getImageFromMsg(const sensor_msgs::CompressedImageConstPtr &img_msg, int flag); +cv_bridge::CvImagePtr +getImageFromMsg(const sensor_msgs::ImageConstPtr &img_msg); +cv::Mat getImageFromMsg(const sensor_msgs::CompressedImageConstPtr &img_msg, + int flag); Eigen::MatrixXf load_csv_mat_eigen(std::string csv); Eigen::VectorXf load_csv_vec_eigen(std::string csv); -template -inline void reduceVector(std::vector &v, std::vector status) -{ +template +inline void reduceVector(std::vector &v, std::vector status) { int j = 0; for (int i = 0; i < int(v.size()); i++) if (status[i]) @@ -27,38 +42,56 @@ inline void reduceVector(std::vector &v, std::vector status) v.resize(j); } - Swarm::Pose AffineRestoCamPose(Eigen::Matrix4d affine); -void PnPInitialFromCamPose(const Swarm::Pose &p, cv::Mat & rvec, cv::Mat & tvec); +void PnPInitialFromCamPose(const Swarm::Pose &p, cv::Mat &rvec, cv::Mat &tvec); Swarm::Pose PnPRestoCamPose(cv::Mat rvec, cv::Mat tvec); cv::Vec3b extractColor(const cv::Mat &img, cv::Point2f p); cv::Point2f rotate_pt_norm2d(cv::Point2f pt, Eigen::Quaterniond q); +void detectPoints(const cv::Mat &img, std::vector &n_pts, + const std::vector &cur_pts, int require_pts, + bool enable_cuda = true, bool use_fast = false, + int fast_rows = 3, int fast_cols = 4); -void detectPoints(const cv::Mat & img, std::vector & n_pts, std::vector & cur_pts, int require_pts, - bool enable_cuda=true, bool use_fast=false, int fast_rows=3, int fast_cols=4); +std::vector buildImagePyramid(const cv::cuda::GpuMat &prevImg, + int maxLevel_ = 3); -std::vector buildImagePyramid(const cv::cuda::GpuMat& prevImg, int maxLevel_=3); +std::vector opticalflowTrack(const cv::Mat &cur_img, + const cv::Mat &prev_img, + std::vector &prev_pts, + std::vector &ids, + TrackLRType type = WHOLE_IMG_MATCH, + bool enable_cuda = true); -std::vector opticalflowTrack(const cv::Mat & cur_img, const cv::Mat & prev_img, std::vector & prev_pts, - std::vector & ids, TrackLRType type=WHOLE_IMG_MATCH, bool enable_cuda=true); +LKImageInfoGPU opticalflowTrackPyr( + const cv::Mat &cur_img, const LKImageInfoGPU& prev_lk, + TrackLRType type = WHOLE_IMG_MATCH); -std::vector opticalflowTrackPyr(const cv::Mat & cur_img, std::vector & prev_pyr, - std::vector & prev_pts, std::vector & ids, TrackLRType type=WHOLE_IMG_MATCH, bool update_pyr=true); +std::vector +matchKNN(const cv::Mat &desc_a, const cv::Mat &desc_b, + double knn_match_ratio = 0.8, + const std::vector pts_a = std::vector(), + const std::vector pts_b = std::vector(), + double search_local_dist = -1); -std::vector matchKNN(const cv::Mat & desc_a, const cv::Mat & desc_b, double knn_match_ratio=0.8, - const std::vector pts_a=std::vector(), - const std::vector pts_b=std::vector(), - double search_local_dist = -1); - -int computeRelativePosePnP(const std::vector lm_positions_a, const std::vector lm_3d_norm_b, - Swarm::Pose extrinsic_b, Swarm::Pose drone_pose_a, Swarm::Pose drone_pose_b, Swarm::Pose & DP_b_to_a, - std::vector &inliers, bool is_4dof, bool verify_gravity=true); -Swarm::Pose computePosePnPnonCentral(const std::vector & lm_positions_a, const std::vector & lm_3d_norm_b, - const std::vector & cam_extrinsics, const std::vector & camera_indices, std::vector &inliers); -int computeRelativePosePnPnonCentral(const std::vector & lm_positions_a, const std::vector & lm_3d_norm_b, - const std::vector & cam_extrinsics, const std::vector & camera_indices, - Swarm::Pose drone_pose_a, Swarm::Pose drone_pose_b, Swarm::Pose & DP_b_to_a, - std::vector &inliers, bool is_4dof, bool verify_gravity=true); -} \ No newline at end of file +int computeRelativePosePnP(const std::vector lm_positions_a, + const std::vector lm_3d_norm_b, + Swarm::Pose extrinsic_b, Swarm::Pose drone_pose_a, + Swarm::Pose drone_pose_b, Swarm::Pose &DP_b_to_a, + std::vector &inliers, bool is_4dof, + bool verify_gravity = true); +Swarm::Pose +computePosePnPnonCentral(const std::vector &lm_positions_a, + const std::vector &lm_3d_norm_b, + const std::vector &cam_extrinsics, + const std::vector &camera_indices, + std::vector &inliers); +int computeRelativePosePnPnonCentral( + const std::vector &lm_positions_a, + const std::vector &lm_3d_norm_b, + const std::vector &cam_extrinsics, + const std::vector &camera_indices, Swarm::Pose drone_pose_a, + Swarm::Pose drone_pose_b, Swarm::Pose &DP_b_to_a, std::vector &inliers, + bool is_4dof, bool verify_gravity = true); +} // namespace D2FrontEnd \ No newline at end of file diff --git a/d2frontend/src/CNN/superglue_onnx.cpp b/d2frontend/src/CNN/superglue_onnx.cpp index e032cf05..94dcf88c 100644 --- a/d2frontend/src/CNN/superglue_onnx.cpp +++ b/d2frontend/src/CNN/superglue_onnx.cpp @@ -2,95 +2,103 @@ #include namespace D2FrontEnd { -std::vector flatten(const std::vector & vec) { - std::vector res; - for (const auto & p : vec) { - res.push_back(p.x); - res.push_back(p.y); - } - return res; +std::vector flatten(const std::vector& vec) { + std::vector res; + for (const auto& p : vec) { + res.push_back(p.x); + res.push_back(p.y); + } + return res; } -SuperGlueOnnx::SuperGlueOnnx(const std::string & engine_path): - env(Ort::Env(ORT_LOGGING_LEVEL_WARNING, "SuperGlueOnnx")) { - init(engine_path); +SuperGlueOnnx::SuperGlueOnnx(const std::string& engine_path) + : env(Ort::Env(ORT_LOGGING_LEVEL_WARNING, "SuperGlueOnnx")) { + init(engine_path); } -void SuperGlueOnnx::init(const std::string & engine_path) { - Ort::SessionOptions session_options; - session_options.SetIntraOpNumThreads(1); - session_options.SetGraphOptimizationLevel(GraphOptimizationLevel::ORT_ENABLE_EXTENDED); - OrtCUDAProviderOptions options; - options.device_id = 0; - options.arena_extend_strategy = 0; - options.gpu_mem_limit = 1 * 1024 * 1024 * 1024; - options.cudnn_conv_algo_search = OrtCudnnConvAlgoSearch::OrtCudnnConvAlgoSearchExhaustive; - options.do_copy_in_default_stream = 1; - session_options.AppendExecutionProvider_CUDA(options); - printf("[SuperGlueOnnx] Loading superglue from %s...\n", engine_path.c_str()); - session_ = new Ort::Session(env, engine_path.c_str(), session_options); - memory_info = Ort::MemoryInfo::CreateCpu(OrtDeviceAllocator, OrtMemTypeCPU); +void SuperGlueOnnx::init(const std::string& engine_path) { + Ort::SessionOptions session_options; + session_options.SetIntraOpNumThreads(1); + session_options.SetGraphOptimizationLevel( + GraphOptimizationLevel::ORT_ENABLE_EXTENDED); + OrtCUDAProviderOptions options; + options.device_id = 0; + options.arena_extend_strategy = 0; + options.gpu_mem_limit = 1 * 1024 * 1024 * 1024; + options.cudnn_conv_algo_search = + OrtCudnnConvAlgoSearch::OrtCudnnConvAlgoSearchExhaustive; + options.do_copy_in_default_stream = 1; + session_options.AppendExecutionProvider_CUDA(options); + printf("[SuperGlueOnnx] Loading superglue from %s...\n", engine_path.c_str()); + session_ = new Ort::Session(env, engine_path.c_str(), session_options); + memory_info = Ort::MemoryInfo::CreateCpu(OrtDeviceAllocator, OrtMemTypeCPU); } -void SuperGlueOnnx::resetTensors() { -} +void SuperGlueOnnx::resetTensors() {} -std::vector SuperGlueOnnx::inference(const std::vector kpts0, - const std::vector kpts1, - const std::vector & desc0, const std::vector & desc1, - const std::vector & scores0, const std::vector & scores1) { - std::vector matches; - if (kpts0.size() == 0 || kpts1.size() == 0) { - return matches; - } - int num_kpts0 = kpts0.size(); - int num_kpts1 = kpts1.size(); - if(memory_info!=nullptr) { - resetTensors(); - } - std::array input_desc_dim0{1, num_kpts0, dim_desc}; - std::array input_kps_dim0{1, num_kpts0, 2}; - std::array input_scores_dim0{1, num_kpts0}; - std::array input_desc_dim1{1, num_kpts1, dim_desc}; - std::array input_kps_dim1{1, num_kpts1, 2}; - std::array input_scores_dim1{1, num_kpts1}; - std::vector inputs; - std::vector _kpts0 = flatten(kpts0); - std::vector _kpts1 = flatten(kpts1); - // printf("num_kpts0 %ld num_kpts1 %ld\n", num_kpts0, num_kpts1); - // printf("desc0 data size %ld\n", desc0.size()); - // printf("desc1 data size %ld\n", desc1.size()); - // printf("scores0 data size %ld\n", scores0.size()); - // printf("scores1 data size %ld\n", scores1.size()); - // printf("kpts0 data size %ld\n", _kpts0.size()); - // printf("kpts1 data size %ld\n", _kpts1.size()); - // std::cout << std::endl; - inputs.emplace_back(Ort::Value::CreateTensor(memory_info, const_cast(desc0.data()), - dim_desc*num_kpts0, input_desc_dim0.data(), input_desc_dim0.size())); - inputs.emplace_back(Ort::Value::CreateTensor(memory_info, _kpts0.data(), - 2*num_kpts0, input_kps_dim0.data(), input_kps_dim0.size())); - inputs.emplace_back(Ort::Value::CreateTensor(memory_info, const_cast(scores0.data()), - num_kpts0, input_scores_dim0.data(), input_scores_dim0.size())); - inputs.emplace_back(Ort::Value::CreateTensor(memory_info, const_cast(desc1.data()), - dim_desc*num_kpts1, input_desc_dim1.data(), input_desc_dim1.size())); - inputs.emplace_back(Ort::Value::CreateTensor(memory_info, _kpts1.data(), - 2*num_kpts1, input_kps_dim1.data(), input_kps_dim1.size())); - inputs.emplace_back(Ort::Value::CreateTensor(memory_info, const_cast(scores1.data()), - num_kpts1, input_scores_dim1.data(), input_scores_dim1.size())); - auto outputs = session_->Run(Ort::RunOptions{nullptr}, input_names, inputs.data(), 6, output_names, 4); - for (int i = 0; i < num_kpts0; i++) { - auto id_j = outputs[0].GetTensorData()[i]; - if (id_j < 0) { - continue; - } - auto score = outputs[2].GetTensorData()[i]; - cv::DMatch match; - match.queryIdx = i; - match.trainIdx = id_j; - match.distance = 1.0-score; - matches.emplace_back(match); - // printf("i->id_j %d->%d\n", i, id_j); - // printf("[SuperGlueOnnx] Match %d -> %d, score %f\n", i, id_j, score); - } +std::vector SuperGlueOnnx::inference( + const std::vector kpts0, const std::vector kpts1, + const std::vector& desc0, const std::vector& desc1, + const std::vector& scores0, const std::vector& scores1) { + std::vector matches; + if (kpts0.size() == 0 || kpts1.size() == 0) { return matches; + } + int num_kpts0 = kpts0.size(); + int num_kpts1 = kpts1.size(); + if (memory_info != nullptr) { + resetTensors(); + } + std::array input_desc_dim0{1, num_kpts0, dim_desc}; + std::array input_kps_dim0{1, num_kpts0, 2}; + std::array input_scores_dim0{1, num_kpts0}; + std::array input_desc_dim1{1, num_kpts1, dim_desc}; + std::array input_kps_dim1{1, num_kpts1, 2}; + std::array input_scores_dim1{1, num_kpts1}; + std::vector inputs; + std::vector _kpts0 = flatten(kpts0); + std::vector _kpts1 = flatten(kpts1); + // printf("num_kpts0 %ld num_kpts1 %ld\n", num_kpts0, num_kpts1); + // printf("desc0 data size %ld\n", desc0.size()); + // printf("desc1 data size %ld\n", desc1.size()); + // printf("scores0 data size %ld\n", scores0.size()); + // printf("scores1 data size %ld\n", scores1.size()); + // printf("kpts0 data size %ld\n", _kpts0.size()); + // printf("kpts1 data size %ld\n", _kpts1.size()); + // std::cout << std::endl; + inputs.emplace_back(Ort::Value::CreateTensor( + memory_info, const_cast(desc0.data()), dim_desc * num_kpts0, + input_desc_dim0.data(), input_desc_dim0.size())); + inputs.emplace_back(Ort::Value::CreateTensor( + memory_info, _kpts0.data(), 2 * num_kpts0, input_kps_dim0.data(), + input_kps_dim0.size())); + inputs.emplace_back(Ort::Value::CreateTensor( + memory_info, const_cast(scores0.data()), num_kpts0, + input_scores_dim0.data(), input_scores_dim0.size())); + inputs.emplace_back(Ort::Value::CreateTensor( + memory_info, const_cast(desc1.data()), dim_desc * num_kpts1, + input_desc_dim1.data(), input_desc_dim1.size())); + inputs.emplace_back(Ort::Value::CreateTensor( + memory_info, _kpts1.data(), 2 * num_kpts1, input_kps_dim1.data(), + input_kps_dim1.size())); + inputs.emplace_back(Ort::Value::CreateTensor( + memory_info, const_cast(scores1.data()), num_kpts1, + input_scores_dim1.data(), input_scores_dim1.size())); + auto outputs = session_->Run(Ort::RunOptions{nullptr}, input_names, + inputs.data(), 6, output_names, 4); + for (int i = 0; i < num_kpts0; i++) { + auto id_j = outputs[0].GetTensorData()[i]; + if (id_j < 0) { + continue; + } + auto score = outputs[2].GetTensorData()[i]; + cv::DMatch match; + match.queryIdx = i; + match.trainIdx = id_j; + match.distance = 1.0 - score; + matches.emplace_back(match); + // printf("i->id_j %d->%d\n", i, id_j); + // printf("[SuperGlueOnnx] Match %d -> %d, score %f\n", i, id_j, score); + } + return matches; } -} \ No newline at end of file +} // namespace D2FrontEnd \ No newline at end of file diff --git a/d2frontend/src/CNN/superpoint.cpp b/d2frontend/src/CNN/superpoint.cpp new file mode 100644 index 00000000..bd7e8c83 --- /dev/null +++ b/d2frontend/src/CNN/superpoint.cpp @@ -0,0 +1,461 @@ +#include "d2frontend/CNN/superpoint.h" +#include "d2common/utils.hpp" + +#include +#include +#include + +#include "tensorrt_utils/buffers.h" +#include "tensorrt_utils/common.h" +#include "tensorrt_utils/logger.h" + + +namespace D2FrontEnd { + +const int32_t kSuperPointDescDim = 256; + +SuperPoint::SuperPoint(const SuperPointConfig &super_point_config): + super_point_config_(super_point_config),engine_(nullptr),context_(nullptr){ + tensorrt_log::setReportableSeverity(tensorrt_log::Logger::Severity::kINFO); +} + +bool SuperPoint::build() { + if(deserializeEngine()){ + return true; + } + auto builder = tensorrt_common::TensorRTUniquePtr(nvinfer1::createInferBuilder(tensorrt_log::gLogger.getTRTLogger())); + if (!builder) { + return false; + } + const auto explicit_batch = 1U << static_cast(NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); + auto network = tensorrt_common::TensorRTUniquePtr(builder->createNetworkV2(explicit_batch)); + if (!network) { + return false; + } + auto config = tensorrt_common::TensorRTUniquePtr(builder->createBuilderConfig()); + if (!config) { + return false; + } + auto parser = tensorrt_common::TensorRTUniquePtr( + nvonnxparser::createParser(*network, tensorrt_log::gLogger.getTRTLogger())); + if (!parser) { + return false; + } + + auto profile = builder->createOptimizationProfile(); + if (!profile) { + return false; + } + + profile->setDimensions(super_point_config_.input_tensor_names[0].c_str(), + OptProfileSelector::kMIN, Dims4(1, 1, 100, 100)); + profile->setDimensions(super_point_config_.input_tensor_names[0].c_str(), + OptProfileSelector::kOPT, Dims4(1, 1, 500, 500)); + profile->setDimensions(super_point_config_.input_tensor_names[0].c_str(), + OptProfileSelector::kMAX, Dims4(1, 1, 1500, 1500)); + config->addOptimizationProfile(profile); + + auto constructed = constructNetwork(builder, network, config, parser); + if (!constructed) { + return false; + } + auto profile_stream = tensorrt_common::makeCudaStream(); + if (!profile_stream) { + return false; + } + config->setProfileStream(*profile_stream); + tensorrt_common::TensorRTUniquePtr plan{builder->buildSerializedNetwork(*network, *config)}; + if (!plan) { + return false; + } + tensorrt_common::TensorRTUniquePtr runtime{createInferRuntime(tensorrt_log::gLogger.getTRTLogger())}; + if (!runtime) { + return false; + } + engine_ = std::shared_ptr(runtime->deserializeCudaEngine(plan->data(), plan->size())); + if (!engine_) { + return false; + } + saveEngine(); + ASSERT(network->getNbInputs() == 1); + input_dims_ = network->getInput(0)->getDimensions(); + ASSERT(input_dims_.nbDims == 4); + ASSERT(network->getNbOutputs() == 2); + semi_dims_ = network->getOutput(0)->getDimensions(); + ASSERT(semi_dims_.nbDims == 3); + desc_dims_ = network->getOutput(1)->getDimensions(); + ASSERT(desc_dims_.nbDims == 4); + //load PCA + if (super_point_config_.enable_pca){ + if (super_point_config_.pca_mean_path.empty() || super_point_config_.pca_comp_path.empty()) { + spdlog::error("PCA mean or PCA comp path is empty"); + super_point_config_.enable_pca = false; + pca_comp_T_.resize(0, 0); + pca_mean_.resize(0); + } else { + Eigen::MatrixXf pca_comp_T; + Eigen::RowVectorXf pca_mean; + spdlog::info("[Sp Debug] Loading PCA mean from {}\n", super_point_config_.pca_mean_path); + pca_comp_T = load_csv_mat_eigen(super_point_config_.pca_comp_path).transpose(); + pca_mean = load_csv_vec_eigen(super_point_config_.pca_mean_path).transpose(); + } + } else { + pca_comp_T_.resize(0, 0); + pca_mean_.resize(0); + } + return true; +} + +bool SuperPoint::constructNetwork(tensorrt_common::TensorRTUniquePtr &builder, + tensorrt_common::TensorRTUniquePtr &network, + tensorrt_common::TensorRTUniquePtr &config, + tensorrt_common::TensorRTUniquePtr &parser) const { + auto parsed = parser->parseFromFile(super_point_config_.onnx_path.c_str(), + static_cast(tensorrt_log::gLogger.getReportableSeverity())); + if (!parsed) { + return false; + } + config->setMaxWorkspaceSize(512_MiB); + if (super_point_config_.fp_16) { + config->setFlag(BuilderFlag::kFP16); + } + config->setFlag(BuilderFlag::kSTRICT_TYPES); + // enableDLA(builder.get(), config.get(), super_point_config_.dla_core); + return true; +} + +bool SuperPoint::infer(const cv::Mat &image, std::vector &keypoints, std::vector &descriptors, std::vector & scores) { + if (!context_) { + context_ = tensorrt_common::TensorRTUniquePtr(engine_->createExecutionContext()); + if (!context_) { + return false; + } + } + + assert(engine_->getNbBindings() == 3); + + const int input_index = engine_->getBindingIndex(super_point_config_.input_tensor_names[0].c_str()); + + context_->setBindingDimensions(input_index, Dims4(1, 1, image.rows, image.cols)); + + tensorrt_buffer::BufferManager buffers(engine_, 0, context_.get()); + + ASSERT(super_point_config_.input_tensor_names.size() == 1); + if (!processInput(buffers, image)) { + return false; + } + buffers.copyInputToDeviceAsync(); + cudaStream_t cuda_stream; + cudaStreamCreate(&cuda_stream); + bool status = context_->executeV2(buffers.getDeviceBindings().data()); + if (!status) { + return false; + } + buffers.copyOutputToHostAsync(); + if (!processOutput(buffers, keypoints ,descriptors, scores)) { + return false; + } + return true; +} + +bool SuperPoint::infer(const cv::Mat & input, std::vector & keypoints, std::vector & descriptors, std::vector & scores) { + std::vector raw_keypoints; + std::vector raw_descriptors; + if ( !infer(input,raw_keypoints,raw_descriptors,scores)) { + keypoints.clear(); + descriptors.clear(); + scores.clear(); + spdlog::error("superpoint infer failed\n"); + return false; + } + // vector 2f to cv::point2f + for (auto &keypoint : raw_keypoints) { + keypoints.emplace_back(cv::Point2f(keypoint[0], keypoint[1])); + } + // [n_keypoints_num,n] + for (auto &descriptor : raw_descriptors) { + for (int i = 0; i < descriptor.size(); ++i) { + descriptors.push_back(descriptor[i]); + } + } + return true; + +} + +bool SuperPoint::processInput(const tensorrt_buffer::BufferManager &buffers, const cv::Mat &image) { + input_dims_.d[2] = image.rows; + input_dims_.d[3] = image.cols; + semi_dims_.d[1] = image.rows; + semi_dims_.d[2] = image.cols; + desc_dims_.d[1] = 256; + desc_dims_.d[2] = image.rows / 8; + desc_dims_.d[3] = image.cols / 8; + auto *host_data_buffer = static_cast(buffers.getHostBuffer(super_point_config_.input_tensor_names[0])); + cv::Mat mono_image; + image.convertTo(mono_image, CV_32FC1, 1.0 / 255.0); + memcpy(host_data_buffer, mono_image.data, mono_image.rows * mono_image.cols * sizeof(float)); + return true; +} + +//replace to NMS +void SuperPoint::findHighScoreIndex(std::vector &scores, std::vector &keypoints, + int h, int w, float threshold) { + std::vector new_scores; + for (int i = 0; i < scores.size(); ++i) { + if (scores[i] > threshold) { + Eigen::Vector2f location = {i % w, int(i / w)}; //u,v + keypoints.emplace_back(location); + new_scores.push_back(scores[i]); + } + } + scores.swap(new_scores); +} + + +void SuperPoint::removeBorders( std::vector &keypoints, std::vector &scores, int border, + int height, + int width) { + std::vector keypoints_selected; + std::vector scores_selected; + for (int i = 0; i < keypoints.size(); ++i) { + bool flag_h = (keypoints[i][1] >= border) && (keypoints[i][1] < (height - border)); //v + bool flag_w = (keypoints[i][0] >= border) && (keypoints[i][0] < (width - border)); //u + if (flag_h && flag_w) { + keypoints_selected.push_back(keypoints[i]); + scores_selected.push_back(scores[i]); + } + } + keypoints.swap(keypoints_selected); + scores.swap(scores_selected); +} + + +std::vector SuperPoint::sortIndexes(std::vector &data) { + std::vector indexes(data.size()); + iota(indexes.begin(), indexes.end(), 0); + sort(indexes.begin(), indexes.end(), [&data](size_t i1, size_t i2) { return data[i1] > data[i2]; }); + return indexes; +} + + +void SuperPoint::topKeypoints( std::vector &keypoints, std::vector &scores, int k) { + if (k < keypoints.size() && k != -1) { + std::vector keypoints_top_k; + std::vector scores_top_k; + std::vector indexes = sortIndexes(scores); + for (int i = 0; i < k; ++i) { + keypoints_top_k.push_back(keypoints[indexes[i]]); + scores_top_k.push_back(scores[indexes[i]]); + } + keypoints.swap(keypoints_top_k); + scores.swap(scores_top_k); + } +} + +void normalize_keypoints(const std::vector &keypoints, std::vector &keypoints_norm, + int h, int w, int s) { + for (auto &keypoint : keypoints) { + Eigen::Vector2f kp = {keypoint[0] - s / 2 + 0.5, keypoint[1] - s / 2 + 0.5}; + kp[0] = kp[0] / (w * s - s / 2 - 0.5); + kp[1] = kp[1] / (h * s - s / 2 - 0.5); + kp[0] = kp[0] * 2 - 1; + kp[1] = kp[1] * 2 - 1; + keypoints_norm.push_back(kp); + } +} + +int clip(int val, int max) { + if (val < 0) return 0; + return std::min(val, max - 1); +} + +void grid_sample(const float *input, std::vector &grid, + std::vector &output, int dim, int h, int w) { + // descriptors 1x256x(W/8)X(H/8) + // keypoints 1x1xDynmx2 + // out 1x256x1xnumber + for (auto &g : grid) { + float ix = ((g[0] + 1) / 2) * (w - 1); + float iy = ((g[1] + 1) / 2) * (h - 1); + + int ix_nw = clip(std::floor(ix), w); + int iy_nw = clip(std::floor(iy), h); + + int ix_ne = clip(ix_nw + 1, w); + int iy_ne = clip(iy_nw, h); + + int ix_sw = clip(ix_nw, w); + int iy_sw = clip(iy_nw + 1, h); + + int ix_se = clip(ix_nw + 1, w); + int iy_se = clip(iy_nw + 1, h); + + float nw = (ix_se - ix) * (iy_se - iy); + float ne = (ix - ix_sw) * (iy_sw - iy); + float sw = (ix_ne - ix) * (iy - iy_ne); + float se = (ix - ix_nw) * (iy - iy_nw); + + Eigen::VectorXf descriptor(kSuperPointDescDim,1); + for (int i = 0; i < dim; ++i) { + // 256x60x106 whd + // x * Height * Depth + y * Depth + z + float nw_val = input[i * h * w + iy_nw * w + ix_nw]; + float ne_val = input[i * h * w + iy_ne * w + ix_ne]; + float sw_val = input[i * h * w + iy_sw * w + ix_sw]; + float se_val = input[i * h * w + iy_se * w + ix_se]; + descriptor[i] = (nw_val * nw + ne_val * ne + sw_val * sw + se_val * se); + } + output.push_back(descriptor); + } +} + +void normalize_descriptors(std::vector &dest_descriptors) { + for (auto &descriptor : dest_descriptors) { + double norm_inv = 1.0 / descriptor.norm(); + descriptor *= norm_inv; + } +} + +void SuperPoint::sampleDescriptors(std::vector &keypoints, float *des_map, + std::vector &descriptors, int dim, int h, int w, int s) { + std::vector keypoints_norm; + normalize_keypoints(keypoints, keypoints_norm, h, w, s); + grid_sample(des_map, keypoints_norm, descriptors, dim, h, w); + normalize_descriptors(descriptors); +} + +bool SuperPoint::processOutput(const tensorrt_buffer::BufferManager &buffers, std::vector &keypoints, + std::vector &descriptors, std::vector &scores) { + keypoints.clear(); + descriptors.clear(); + auto *output_score = static_cast(buffers.getHostBuffer(super_point_config_.output_tensor_names[0])); + auto *output_desc = static_cast(buffers.getHostBuffer(super_point_config_.output_tensor_names[1])); + int semi_feature_map_h = semi_dims_.d[1]; + int semi_feature_map_w = semi_dims_.d[2]; + std::vector scores_vec(output_score, output_score + semi_feature_map_h * semi_feature_map_w); + + findHighScoreIndex(scores_vec, keypoints, semi_feature_map_h, semi_feature_map_w, + super_point_config_.keypoint_threshold); + removeBorders(keypoints, scores_vec, super_point_config_.remove_borders, semi_feature_map_h, semi_feature_map_w); + topKeypoints(keypoints, scores_vec, super_point_config_.max_keypoints); + spdlog::debug("super point number is {}", keypoints.size()); + scores = scores_vec; + descriptors.resize(scores_vec.size()); + int desc_feature_dim = desc_dims_.d[1]; + int desc_feature_map_h = desc_dims_.d[2]; + int desc_feature_map_w = desc_dims_.d[3]; + sampleDescriptors(keypoints, output_desc, descriptors, desc_feature_dim, desc_feature_map_h, desc_feature_map_w); + + return true; +} + +void SuperPoint::visualization(const std::string &image_name, const cv::Mat &image,std::vector &keypoints ) { + cv::Mat image_display; + if(image.channels() == 1) + cv::cvtColor(image, image_display, cv::COLOR_GRAY2BGR); + else + image_display = image.clone(); + for (auto &keypoint : keypoints) { + cv::circle(image_display, cv::Point(int(keypoint[0]), int(keypoint[1])), 1, cv::Scalar(255, 0, 0), -1, 16); + } + cv::imshow("superpoint", image_display); + cv::waitKey(0); +} + +void SuperPoint::saveEngine(){ + if (super_point_config_.engine_path.empty()) return; + if (engine_ != nullptr) { + nvinfer1::IHostMemory *data = engine_->serialize(); + std::ofstream file(super_point_config_.engine_path, std::ios::binary); + if (!file) return; + file.write(reinterpret_cast(data->data()), data->size()); + } +} + +bool SuperPoint::deserializeEngine(){ + std::ifstream file(super_point_config_.engine_path.c_str(), std::ios::binary); + if (file.is_open()) { + file.seekg(0, std::ifstream::end); + size_t size = file.tellg(); + file.seekg(0, std::ifstream::beg); + char *model_stream = new char[size]; + file.read(model_stream, size); + file.close(); + IRuntime *runtime = createInferRuntime(tensorrt_log::gLogger); + if (runtime == nullptr) { + delete[] model_stream; + return false; + } + engine_ = std::shared_ptr(runtime->deserializeCudaEngine(model_stream, size)); + delete[] model_stream; + if (engine_ == nullptr){ + return false; + } + return true; + } + spdlog::warn("[Superpoint]: deserialize engine failed; try to read from {}", super_point_config_.engine_path); + return false; +} + +// void SuperPoint::computeDescriptors(const std::vector& keypoints, +// float *des_map, +// std::vector& local_descriptors, int width, +// int height, const Eigen::MatrixXf& pca_comp_T, +// const Eigen::RowVectorXf& pca_mean) { +// D2Common::Utility::TicToc tic; +// //des_map to mDesc +// auto mDesc = torch::from_blob(des_map, {1, kSuperPointDescDim, height / 8, width / 8}, torch::kFloat); + +// cv::Mat kpt_mat(keypoints.size(), 2, CV_32F); // [n_keypoints, 2] (y, x) +// for (size_t i = 0; i < keypoints.size(); i++) { +// kpt_mat.at(i, 0) = (float)keypoints[i].y; +// kpt_mat.at(i, 1) = (float)keypoints[i].x; +// } + +// auto fkpts = +// at::from_blob(kpt_mat.data, {keypoints.size(), 2}, torch::kFloat); + +// auto grid = torch::zeros({1, 1, fkpts.size(0), 2}); // [1, 1, n_keypoints, 2] +// grid[0][0].slice(1, 0, 1) = 2.0 * fkpts.slice(1, 1, 2) / width - 1; // x +// grid[0][0].slice(1, 1, 2) = 2.0 * fkpts.slice(1, 0, 1) / height - 1; // y + +// // mDesc.to(torch::kCUDA); +// // grid.to(torch::kCUDA); +// auto desc = torch::grid_sampler(mDesc, grid, 0, 0, 0); + +// desc = desc.squeeze(0).squeeze(1); + +// // normalize to 1 +// auto dn = torch::norm(desc, 2, 1); +// desc = desc.div(torch::unsqueeze(dn, 1)); + +// desc = desc.transpose(0, 1).contiguous(); +// desc = desc.to(torch::kCPU); + + +// Eigen::Map< +// Eigen::Matrix> +// _desc(desc.data(), desc.size(0), desc.size(1)); +// if (pca_comp_T.size() > 0) { +// Eigen::Matrix +// _desc_new = (_desc.rowwise() - pca_mean) * pca_comp_T; +// // Perform row wise normalization +// for (int i = 0; i < _desc_new.rows(); i++) { +// _desc_new.row(i) /= _desc_new.row(i).norm(); +// } +// local_descriptors = std::vector( +// _desc_new.data(), +// _desc_new.data() + _desc_new.cols() * _desc_new.rows()); +// } else { +// for (int i = 0; i < _desc.rows(); i++) { +// _desc.row(i) /= _desc.row(i).norm(); +// } +// local_descriptors = std::vector( +// _desc.data(), _desc.data() + _desc.cols() * _desc.rows()); +// } +// if (params->enable_perf_output) { +// std::cout << " computeDescriptors full " << tic.toc() << std::endl; +// } +// } + +} // namespace D2FrontEnd \ No newline at end of file diff --git a/d2frontend/src/CNN/superpoint_common.cpp b/d2frontend/src/CNN/superpoint_common.cpp index 1e218908..592c52c5 100644 --- a/d2frontend/src/CNN/superpoint_common.cpp +++ b/d2frontend/src/CNN/superpoint_common.cpp @@ -1,166 +1,174 @@ #include -#include #include +#include + #include "d2common/utils.hpp" using D2Common::Utility::TicToc; namespace D2FrontEnd { -void NMS2(std::vector det, cv::Mat conf, std::vector& pts, std::vector& scores, - int border, int dist_thresh, int img_width, int img_height, int max_num); -void getKeyPoints(const cv::Mat & prob, float threshold, int nms_dist, std::vector &keypoints, std::vector& scores, int width, int height, int max_num) -{ - TicToc getkps; - auto mask = (prob > threshold); - std::vector kps; - cv::findNonZero(mask, kps); - std::vector keypoints_no_nms; - for (int i = 0; i < kps.size(); i++) { - keypoints_no_nms.push_back(cv::Point2f(kps[i].x, kps[i].y)); - } - - cv::Mat conf(keypoints_no_nms.size(), 1, CV_32F); - for (size_t i = 0; i < keypoints_no_nms.size(); i++) { - int x = keypoints_no_nms[i].x; - int y = keypoints_no_nms[i].y; - conf.at(i, 0) = prob.at(y, x); - } - - int border = 0; - TicToc ticnms; - NMS2(keypoints_no_nms, conf, keypoints, scores, border, nms_dist, width, height, max_num); - if (params->enable_perf_output) { - printf(" NMS %f keypoints_no_nms %ld keypoints %ld/%ld\n", ticnms.toc(), keypoints_no_nms.size(), keypoints.size(), max_num); - } +void NMS2(std::vector det, cv::Mat conf, + std::vector& pts, std::vector& scores, int border, + int dist_thresh, int img_width, int img_height, int max_num); +void getKeyPoints(const cv::Mat& prob, float threshold, int nms_dist, + std::vector& keypoints, + std::vector& scores, int width, int height, + int max_num) { + TicToc getkps; + auto mask = (prob > threshold); + std::vector kps; + cv::findNonZero(mask, kps); + std::vector keypoints_no_nms; + for (int i = 0; i < kps.size(); i++) { + keypoints_no_nms.push_back(cv::Point2f(kps[i].x, kps[i].y)); + } + + cv::Mat conf(keypoints_no_nms.size(), 1, CV_32F); + for (size_t i = 0; i < keypoints_no_nms.size(); i++) { + int x = keypoints_no_nms[i].x; + int y = keypoints_no_nms[i].y; + conf.at(i, 0) = prob.at(y, x); + } + + int border = 0; + TicToc ticnms; + NMS2(keypoints_no_nms, conf, keypoints, scores, border, nms_dist, width, + height, max_num); + if (params->enable_perf_output) { + printf(" NMS %f keypoints_no_nms %ld keypoints %ld/%ld\n", ticnms.toc(), + keypoints_no_nms.size(), keypoints.size(), max_num); + } } - -void computeDescriptors(const torch::Tensor & mProb, const torch::Tensor & mDesc, - const std::vector &keypoints, - std::vector & local_descriptors, int width, int height, - const Eigen::MatrixXf & pca_comp_T, const Eigen::RowVectorXf & pca_mean) { - TicToc tic; - cv::Mat kpt_mat(keypoints.size(), 2, CV_32F); // [n_keypoints, 2] (y, x) - for (size_t i = 0; i < keypoints.size(); i++) { - kpt_mat.at(i, 0) = (float)keypoints[i].y; - kpt_mat.at(i, 1) = (float)keypoints[i].x; - } - - - auto fkpts = at::from_blob(kpt_mat.data, {keypoints.size(), 2}, torch::kFloat); - - auto grid = torch::zeros({1, 1, fkpts.size(0), 2}); // [1, 1, n_keypoints, 2] - grid[0][0].slice(1, 0, 1) = 2.0 * fkpts.slice(1, 1, 2) / width - 1; // x - grid[0][0].slice(1, 1, 2) = 2.0 * fkpts.slice(1, 0, 1) / height - 1; // y - - // mDesc.to(torch::kCUDA); - // grid.to(torch::kCUDA); - auto desc = torch::grid_sampler(mDesc, grid, 0, 0, 0); - - desc = desc.squeeze(0).squeeze(1); - - // normalize to 1 - auto dn = torch::norm(desc, 2, 1); - desc = desc.div(torch::unsqueeze(dn, 1)); - - desc = desc.transpose(0, 1).contiguous(); - desc = desc.to(torch::kCPU); - Eigen::Map> _desc(desc.data(), desc.size(0), desc.size(1)); - if (pca_comp_T.size() > 0) { - Eigen::Matrix _desc_new = (_desc.rowwise() - pca_mean) *pca_comp_T; - //Perform row wise normalization - for (int i = 0; i < _desc_new.rows(); i++) { - _desc_new.row(i) /= _desc_new.row(i).norm(); - } - local_descriptors = std::vector(_desc_new.data(), _desc_new.data()+_desc_new.cols()*_desc_new.rows()); - } else { - for (int i = 0; i < _desc.rows(); i++) { - _desc.row(i) /= _desc.row(i).norm(); - } - local_descriptors = std::vector(_desc.data(), _desc.data()+_desc.cols()*_desc.rows()); +void computeDescriptors(const torch::Tensor& mProb, const torch::Tensor& mDesc, + const std::vector& keypoints, + std::vector& local_descriptors, int width, + int height, const Eigen::MatrixXf& pca_comp_T, + const Eigen::RowVectorXf& pca_mean) { + TicToc tic; + cv::Mat kpt_mat(keypoints.size(), 2, CV_32F); // [n_keypoints, 2] (y, x) + for (size_t i = 0; i < keypoints.size(); i++) { + kpt_mat.at(i, 0) = (float)keypoints[i].y; + kpt_mat.at(i, 1) = (float)keypoints[i].x; + } + + auto fkpts = + at::from_blob(kpt_mat.data, {keypoints.size(), 2}, torch::kFloat); + + auto grid = torch::zeros({1, 1, fkpts.size(0), 2}); // [1, 1, n_keypoints, 2] + grid[0][0].slice(1, 0, 1) = 2.0 * fkpts.slice(1, 1, 2) / width - 1; // x + grid[0][0].slice(1, 1, 2) = 2.0 * fkpts.slice(1, 0, 1) / height - 1; // y + + // mDesc.to(torch::kCUDA); + // grid.to(torch::kCUDA); + auto desc = torch::grid_sampler(mDesc, grid, 0, 0, 0); // [1,256,w/8,h/8] [1,1,n_keypoints,2] [1,256,1,n_keypoints] + desc = desc.squeeze(0).squeeze(1); + + // normalize to 1 + auto dn = torch::norm(desc, 2, 1); + desc = desc.div(torch::unsqueeze(dn, 1)); + + desc = desc.transpose(0, 1).contiguous(); + desc = desc.to(torch::kCPU); + Eigen::Map< + Eigen::Matrix> + _desc(desc.data(), desc.size(0), desc.size(1)); //[256, n_keypoints] + if (pca_comp_T.size() > 0) { + Eigen::Matrix + _desc_new = (_desc.rowwise() - pca_mean) * pca_comp_T; + // Perform row wise normalization + for (int i = 0; i < _desc_new.rows(); i++) { + _desc_new.row(i) /= _desc_new.row(i).norm(); } - if (params->enable_perf_output) { - std::cout << " computeDescriptors full " << tic.toc() << std::endl; + local_descriptors = std::vector( + _desc_new.data(), + _desc_new.data() + _desc_new.cols() * _desc_new.rows()); + } else { + for (int i = 0; i < _desc.rows(); i++) { + _desc.row(i) /= _desc.row(i).norm(); } + local_descriptors = std::vector( + _desc.data(), _desc.data() + _desc.cols() * _desc.rows()); + } + if (params->enable_perf_output) { + std::cout << " computeDescriptors full " << tic.toc() << std::endl; + } } -bool pt_conf_comp(std::pair i1, std::pair i2) -{ - return (i1.second > i2.second); +bool pt_conf_comp(std::pair i1, + std::pair i2) { + return (i1.second > i2.second); } -//NMS code is modified from https://github.com/KinglittleQ/SuperPoint_SLAM -void NMS2(std::vector det, cv::Mat conf, std::vector& pts, - std::vector& scores, int border, int dist_thresh, int img_width, int img_height, int max_num) -{ +// NMS code is modified from https://github.com/KinglittleQ/SuperPoint_SLAM +void NMS2(std::vector det, cv::Mat conf, + std::vector& pts, std::vector& scores, int border, + int dist_thresh, int img_width, int img_height, int max_num) { + std::vector pts_raw = det; - std::vector pts_raw = det; + std::vector> pts_conf_vec; - std::vector> pts_conf_vec; + cv::Mat grid = cv::Mat(cv::Size(img_width, img_height), CV_8UC1); + cv::Mat inds = cv::Mat(cv::Size(img_width, img_height), CV_16UC1); - cv::Mat grid = cv::Mat(cv::Size(img_width, img_height), CV_8UC1); - cv::Mat inds = cv::Mat(cv::Size(img_width, img_height), CV_16UC1); + cv::Mat confidence = cv::Mat(cv::Size(img_width, img_height), CV_32FC1); - cv::Mat confidence = cv::Mat(cv::Size(img_width, img_height), CV_32FC1); + grid.setTo(0); + inds.setTo(0); + confidence.setTo(0); - grid.setTo(0); - inds.setTo(0); - confidence.setTo(0); + for (unsigned int i = 0; i < pts_raw.size(); i++) { + int uu = (int)pts_raw[i].x; + int vv = (int)pts_raw[i].y; - for (unsigned int i = 0; i < pts_raw.size(); i++) - { - int uu = (int) pts_raw[i].x; - int vv = (int) pts_raw[i].y; + grid.at(vv, uu) = 1; + inds.at(vv, uu) = i; - grid.at(vv, uu) = 1; - inds.at(vv, uu) = i; + confidence.at(vv, uu) = conf.at(i, 0); + } - confidence.at(vv, uu) = conf.at(i, 0); - } + for (int i = 0; i < pts_raw.size(); i++) { + int uu = (int)pts_raw[i].x; + int vv = (int)pts_raw[i].y; - for (int i = 0; i < pts_raw.size(); i++) - { - int uu = (int) pts_raw[i].x; - int vv = (int) pts_raw[i].y; - - if (grid.at(vv, uu) != 1) - continue; - - for(int k = -dist_thresh; k < (dist_thresh+1); k++) - for(int j = -dist_thresh; j < (dist_thresh+1); j++) - { - if(j==0 && k==0) continue; - if (uu+j < 0 || uu+j >= img_width || vv+k < 0 || vv+k >= img_height) continue; - - if ( confidence.at(vv + k, uu + j) < confidence.at(vv, uu) ) { - grid.at(vv + k, uu + j) = 0; - } - } - grid.at(vv, uu) = 2; - } + if (grid.at(vv, uu) != 1) continue; - size_t valid_cnt = 0; - - for (int v = 0; v < (img_height); v++){ - for (int u = 0; u < (img_width); u++) - { - if (u>= (img_width - border) || u < border || v >= (img_height - border) || v < border) - continue; - - if (grid.at(v,u) == 2) - { - int select_ind = (int) inds.at(v, u); - float _conf = confidence.at (v, u); - cv::Point2f p = pts_raw[select_ind]; - pts_conf_vec.push_back(std::make_pair(p, _conf)); - valid_cnt++; - } + for (int k = -dist_thresh; k < (dist_thresh + 1); k++) + for (int j = -dist_thresh; j < (dist_thresh + 1); j++) { + if (j == 0 && k == 0) continue; + if (uu + j < 0 || uu + j >= img_width || vv + k < 0 || + vv + k >= img_height) + continue; + + if (confidence.at(vv + k, uu + j) < + confidence.at(vv, uu)) { + grid.at(vv + k, uu + j) = 0; } + } + grid.at(vv, uu) = 2; + } + + size_t valid_cnt = 0; + + for (int v = 0; v < (img_height); v++) { + for (int u = 0; u < (img_width); u++) { + if (u >= (img_width - border) || u < border || + v >= (img_height - border) || v < border) + continue; + + if (grid.at(v, u) == 2) { + int select_ind = (int)inds.at(v, u); + float _conf = confidence.at(v, u); + cv::Point2f p = pts_raw[select_ind]; + pts_conf_vec.push_back(std::make_pair(p, _conf)); + valid_cnt++; + } } - - std::sort(pts_conf_vec.begin(), pts_conf_vec.end(), pt_conf_comp); - for (unsigned int i = 0; i < max_num && i < pts_conf_vec.size(); i ++) { - pts.push_back(pts_conf_vec[i].first); - scores.push_back(pts_conf_vec[i].second); - } + } + + std::sort(pts_conf_vec.begin(), pts_conf_vec.end(), pt_conf_comp); + for (unsigned int i = 0; i < max_num && i < pts_conf_vec.size(); i++) { + pts.push_back(pts_conf_vec[i].first); + scores.push_back(pts_conf_vec[i].second); + } } -} \ No newline at end of file +} // namespace D2FrontEnd \ No newline at end of file diff --git a/d2frontend/src/CNN/superpoint_onnx.cpp b/d2frontend/src/CNN/superpoint_onnx.cpp index a7c0b78a..1d5e95e4 100644 --- a/d2frontend/src/CNN/superpoint_onnx.cpp +++ b/d2frontend/src/CNN/superpoint_onnx.cpp @@ -1,91 +1,108 @@ +#include #include #include -#include #include + #include "ATen/Parallel.h" #include "d2common/utils.hpp" -using D2Common::Utility::TicToc; +using D2Common::Utility::TicToc; namespace D2FrontEnd { -SuperPointONNX::SuperPointONNX(std::string engine_path, - int _nms_dist, - std::string _pca_comp, - std::string _pca_mean, - int _width, int _height, - float _thres, int _max_num, - bool use_tensorrt, bool use_fp16, bool use_int8, std::string int8_calib_table_name): - ONNXInferenceGeneric(engine_path, "image", "semi", _width, _height, use_tensorrt, use_fp16, use_int8, int8_calib_table_name), - output_shape_semi_{1, _height, _width}, - output_shape_desc_{1, SP_DESC_RAW_LEN, _height/8, _width/8}, - input_shape_{1, 1, _height, _width}, - thres(_thres), - max_num(_max_num), - nms_dist(_nms_dist) { - at::set_num_threads(1); - std::cout << "Init SuperPointONNX: " << engine_path << " size " << _width << " " << _height << std::endl; +SuperPointONNX::SuperPointONNX(std::string engine_path, int _nms_dist, + std::string _pca_comp, std::string _pca_mean, + int _width, int _height, float _thres, + int _max_num, bool use_tensorrt, bool use_fp16, + bool use_int8, std::string int8_calib_table_name) + : ONNXInferenceGeneric(engine_path, "image", "semi", _width, _height, + use_tensorrt, use_fp16, use_int8, + int8_calib_table_name), + output_shape_semi_{1, _height, _width}, + output_shape_desc_{1, SP_DESC_RAW_LEN, _height / 8, _width / 8}, + input_shape_{1, 1, _height, _width}, + thres(_thres), + max_num(_max_num), + nms_dist(_nms_dist) { + at::set_num_threads(1); + std::cout << "Init SuperPointONNX: " << engine_path << " size " << _width + << " " << _height << std::endl; - input_image = new float[_width*_height]; - auto memory_info = Ort::MemoryInfo::CreateCpu(OrtDeviceAllocator, OrtMemTypeCPU); - input_tensor_ = Ort::Value::CreateTensor(memory_info, input_image, width*height, input_shape_.data(), 4); + input_image = new float[_width * _height]; + auto memory_info = + Ort::MemoryInfo::CreateCpu(OrtDeviceAllocator, OrtMemTypeCPU); + input_tensor_ = Ort::Value::CreateTensor( + memory_info, input_image, width * height, input_shape_.data(), 4); - results_desc_ = new float[1*SP_DESC_RAW_LEN*height/8*width/8]; - results_semi_ = new float[width*height]; - //semi - output_tensors_.emplace_back(Ort::Value::CreateTensor(memory_info, - results_semi_, height*width, output_shape_semi_.data(), output_shape_semi_.size())); - //desc - output_tensors_.emplace_back(Ort::Value::CreateTensor(memory_info, - results_desc_, SP_DESC_RAW_LEN*height/8*width/8, output_shape_desc_.data(), output_shape_desc_.size())); - if (params->enable_pca_superpoint) { - pca_comp_T = load_csv_mat_eigen(_pca_comp).transpose(); - pca_mean = load_csv_vec_eigen(_pca_mean).transpose(); - } else { - pca_comp_T.resize(0, 0); - pca_mean.resize(0); - } + results_desc_ = new float[1 * SP_DESC_RAW_LEN * height / 8 * width / 8]; + results_semi_ = new float[width * height]; + // semi + output_tensors_.emplace_back(Ort::Value::CreateTensor( + memory_info, results_semi_, height * width, output_shape_semi_.data(), + output_shape_semi_.size())); + // desc + output_tensors_.emplace_back(Ort::Value::CreateTensor( + memory_info, results_desc_, SP_DESC_RAW_LEN * height / 8 * width / 8, + output_shape_desc_.data(), output_shape_desc_.size())); + if (params->enable_pca_superpoint) { + pca_comp_T = load_csv_mat_eigen(_pca_comp).transpose(); + pca_mean = load_csv_vec_eigen(_pca_mean).transpose(); + } else { + pca_comp_T.resize(0, 0); + pca_mean.resize(0); + } } -void SuperPointONNX::doInference(const unsigned char* input, const uint32_t batchSize) { - const char* input_names[] = {m_InputBlobName.c_str()}; - const char* output_names_[] = {"semi", "desc"}; - memcpy(input_image, input, width*height*sizeof(float)); - session_->Run(Ort::RunOptions{nullptr}, input_names, &input_tensor_, 1, output_names_, output_tensors_.data(), 2); +void SuperPointONNX::doInference(const unsigned char* input, + const uint32_t batchSize) { + const char* input_names[] = {m_InputBlobName.c_str()}; + const char* output_names_[] = {"semi", "desc"}; + memcpy(input_image, input, width * height * sizeof(float)); + session_->Run(Ort::RunOptions{nullptr}, input_names, &input_tensor_, 1, + output_names_, output_tensors_.data(), 2); } -void SuperPointONNX::inference(const cv::Mat & input, std::vector & keypoints, std::vector & local_descriptors, std::vector & scores) { - TicToc tic; - cv::Mat _input; - keypoints.clear(); - local_descriptors.clear(); - assert(input.rows == height && input.cols == width && "Input image must have same size with network"); - if (input.channels() == 3) { - cv::cvtColor(input, _input, cv::COLOR_BGR2GRAY); - } else { - _input = input; - } - if (_input.rows != height || _input.cols != width) { - cv::resize(_input, _input, cv::Size(width, height)); - } - _input.convertTo(_input, CV_32F, 1/255.0); - ((CNNInferenceGeneric*) this)->doInference(_input); - double inference_time = tic.toc(); +void SuperPointONNX::infer(const cv::Mat& input, + std::vector& keypoints, + std::vector& local_descriptors, + std::vector& scores) { + TicToc tic; + cv::Mat _input; + keypoints.clear(); + local_descriptors.clear(); + assert(input.rows == height && input.cols == width && + "Input image must have same size with network"); + if (input.channels() == 3) { + cv::cvtColor(input, _input, cv::COLOR_BGR2GRAY); + } else { + _input = input; + } + if (_input.rows != height || _input.cols != width) { + cv::resize(_input, _input, cv::Size(width, height)); + } + _input.convertTo(_input, CV_32F, 1 / 255.0); + ((CNNInferenceGeneric*)this)->doInference(_input); + double inference_time = tic.toc(); - TicToc tic1; - auto options = torch::TensorOptions().dtype(torch::kFloat32); - - auto mProb = at::from_blob(results_semi_, {1, 1, height, width}, options); - auto mDesc = at::from_blob(results_desc_, {1, SP_DESC_RAW_LEN, height/8, width/8}, options); - cv::Mat Prob(height, width, CV_32F, results_semi_); - double copy_time = tic1.toc(); + TicToc tic1; + auto options = torch::TensorOptions().dtype(torch::kFloat32); - TicToc tic2; - getKeyPoints(Prob, thres, nms_dist, keypoints, scores, width, height, max_num); - double nms_time = tic2.toc(); - computeDescriptors(mProb, mDesc, keypoints, local_descriptors, width, height, pca_comp_T, pca_mean); - double desc_time = tic2.toc(); - if (params->enable_perf_output) { - printf("[SuperPointONNX] inference time: %f ms, copy time: %f ms, nms time: %f ms, desc time: %f ms\n", - inference_time, copy_time, nms_time, desc_time); - } -} + auto mProb = at::from_blob(results_semi_, {1, 1, height, width}, options); + auto mDesc = at::from_blob( + results_desc_, {1, SP_DESC_RAW_LEN, height / 8, width / 8}, options); + cv::Mat Prob(height, width, CV_32F, results_semi_); + double copy_time = tic1.toc(); + + TicToc tic2; + getKeyPoints(Prob, thres, nms_dist, keypoints, scores, width, height, + max_num); + double nms_time = tic2.toc(); + computeDescriptors(mProb, mDesc, keypoints, local_descriptors, width, height, + pca_comp_T, pca_mean); + double desc_time = tic2.toc(); + if (params->enable_perf_output) { + printf( + "[SuperPointONNX] inference time: %f ms, copy time: %f ms, nms time: " + "%f ms, desc time: %f ms\n", + inference_time, copy_time, nms_time, desc_time); + } } +} // namespace D2FrontEnd diff --git a/d2frontend/src/d2featuretracker.cpp b/d2frontend/src/d2featuretracker.cpp index 94e5b8ef..39949274 100644 --- a/d2frontend/src/d2featuretracker.cpp +++ b/d2frontend/src/d2featuretracker.cpp @@ -1,1010 +1,1372 @@ -#include #include -#include #include -#include +#include +#include #include +#include +#include + #include #define MIN_HOMOGRAPHY 6 using D2Common::Utility::TicToc; namespace D2FrontEnd { -D2FeatureTracker::D2FeatureTracker(D2FTConfig config): - _config(config) { - lmanager = new LandmarkManager; - if (config.enable_superglue_local || config.enable_superglue_remote) { - superglue = new SuperGlueOnnx(config.superglue_model_path); - } - image_width = params->width; - if (params->camera_configuration == CameraConfig::FOURCORNER_FISHEYE) { - image_width = params->width_undistort; - } - if (!_config.enable_search_local_aera) { - _config.search_local_max_dist = -1; - } - search_radius = _config.search_local_max_dist*image_width; - reference_frame_id = params->self_id; +D2FeatureTracker::D2FeatureTracker(D2FTConfig config) : _config(config) { + lmanager = new LandmarkManager; + if (config.enable_superglue_local || config.enable_superglue_remote) { + superglue = new SuperGlueOnnx(config.superglue_model_path); + } + image_width = params->width; + if (params->camera_configuration == CameraConfig::FOURCORNER_FISHEYE) { + image_width = params->width_undistort; + } + if (!_config.enable_search_local_aera) { + _config.search_local_max_dist = -1; + } + search_radius = _config.search_local_max_dist * image_width; + reference_frame_id = params->self_id; + _config.sp_track_use_lk = + _config.sp_track_use_lk && params->loopcamconfig->superpoint_max_num > 0; + SPDLOG_INFO("sp {} _config.sp_track_use_lk {}", + params->loopcamconfig->superpoint_max_num, + _config.sp_track_use_lk); } -void D2FeatureTracker::updatebySldWin(const std::vector sld_win) { - //update by sliding window - const Guard lock(keyframe_lock); - const Guard guard2(lmanager_lock); - if (current_keyframes.size() == 0 || sld_win.size() == 0) - return; - std::map sld_win_poses; - for (auto & frame: sld_win) { - sld_win_poses[frame->frame_id] = frame->odom.pose(); - } - reference_frame_id = sld_win.back()->reference_frame_id; - //Remove the keyframe not in the sliding window except the last one - for (auto it = current_keyframes.begin(); it != current_keyframes.end();) { - if (sld_win_poses.find(it->frame_id) == sld_win_poses.end() && it->frame_id != current_keyframes.back().frame_id) { - if (current_keyframes.size() <= 1 ) { - it++; - } else { - lmanager->popFrame(it->frame_id); - it = current_keyframes.erase(it); - } - } else { - if (sld_win_poses.find(it->frame_id) != sld_win_poses.end()) { - it->pose_drone = sld_win_poses[it->frame_id]; - for (auto & img : it->images) { - img.pose_drone = sld_win_poses[it->frame_id]; - } - } - it++; +void D2FeatureTracker::updatebySldWin(const std::vector sld_win) { + // update by sliding window + const Guard lock(keyframe_lock); + const Guard guard2(lmanager_lock); + if (current_keyframes.size() == 0 || sld_win.size() == 0) return; + std::map sld_win_poses; + for (auto &frame : sld_win) { + sld_win_poses[frame->frame_id] = frame->odom.pose(); + } + reference_frame_id = sld_win.back()->reference_frame_id; + // Remove the keyframe not in the sliding window except the last one + for (auto it = current_keyframes.begin(); it != current_keyframes.end();) { + if (sld_win_poses.find(it->frame_id) == sld_win_poses.end() && + it->frame_id != current_keyframes.back().frame_id) { + if (current_keyframes.size() <= 1) { + it++; + } else { + lmanager->popFrame(it->frame_id); + it = current_keyframes.erase(it); + } + } else { + if (sld_win_poses.find(it->frame_id) != sld_win_poses.end()) { + it->pose_drone = sld_win_poses[it->frame_id]; + for (auto &img : it->images) { + img.pose_drone = sld_win_poses[it->frame_id]; } + } + it++; } + } } -void D2FeatureTracker::updatebyLandmarkDB(const std::map & vins_landmark_db) { - //update by sliding window - const Guard guard2(lmanager_lock); - if (_config.enable_motion_prediction_local || _config.enable_search_local_aera_remote) { - auto & db = lmanager->getLandmarkDB(); - for (auto & kv : vins_landmark_db) { - if (db.find(kv.first) != db.end()) { - auto & lm = lmanager->at(kv.first); - lm.flag = kv.second.flag; - lm.position = kv.second.position; - } - } +void D2FeatureTracker::updatebyLandmarkDB( + const std::map &vins_landmark_db) { + // update by sliding window + const Guard guard2(lmanager_lock); + if (_config.enable_motion_prediction_local || + _config.enable_search_local_aera_remote) { + auto &db = lmanager->getLandmarkDB(); + for (auto &kv : vins_landmark_db) { + if (db.find(kv.first) != db.end()) { + auto &lm = lmanager->at(kv.first); + lm.flag = kv.second.flag; + lm.position = kv.second.position; + } } + } } -bool D2FeatureTracker::trackLocalFrames(VisualImageDescArray & frames) { - const Guard lock(track_lock); - const Guard guard(keyframe_lock); - const Guard guard2(lmanager_lock); - bool iskeyframe = false; - frame_count ++; - TrackReport report; - landmark_predictions_viz.clear(); - frames.send_to_backend = (frame_count % _config.frame_step) == 0; - TicToc tic; - if (!inited) { - inited = true; - ROS_INFO("[D2FeatureTracker] receive first, will init kf\n"); - processFrame(frames, true); - frames.send_to_backend = true; +bool D2FeatureTracker::trackLocalFrames(VisualImageDescArray &frames) { + const Guard lock(track_lock); + const Guard guard(keyframe_lock); + const Guard guard2(lmanager_lock); + bool iskeyframe = false; + frame_count++; + TrackReport report; + landmark_predictions_viz.clear(); + frames.send_to_backend = (frame_count % _config.frame_step) == 0; + TicToc tic; + if (!inited) { + inited = true; + SPDLOG_INFO("receive first, will init kf"); + iskeyframe = true; + if (!_config.sp_track_use_lk) { + processFrame(frames, true); } - + frames.send_to_backend = true; + } + if (params->camera_configuration == CameraConfig::STEREO_PINHOLE) { + report.compose(track(frames.images[0], frames.motion_prediction)); + if (_config.lr_match_use_lk) { + frames.images[1].landmarks.clear(); + frames.images[1].landmark_descriptor.clear(); + frames.images[1].landmark_scores.clear(); + } + report.compose(track(frames.images[0], frames.images[1], true, + WHOLE_IMG_MATCH, _config.lr_match_use_lk)); + } else if (params->camera_configuration == CameraConfig::PINHOLE_DEPTH) { + for (auto &frame : frames.images) { + report.compose(track(frame)); + } + } else if (params->camera_configuration == CameraConfig::FOURCORNER_FISHEYE) { + report.compose(track(frames.images[0], frames.motion_prediction)); + report.compose(track(frames.images[1], frames.motion_prediction)); + report.compose(track(frames.images[2], frames.motion_prediction)); + report.compose(track(frames.images[3], frames.motion_prediction)); + report.compose( + track(frames.images[0], frames.images[1], true, LEFT_RIGHT_IMG_MATCH)); + report.compose( + track(frames.images[1], frames.images[2], true, LEFT_RIGHT_IMG_MATCH)); + report.compose( + track(frames.images[2], frames.images[3], true, LEFT_RIGHT_IMG_MATCH)); + report.compose( + track(frames.images[0], frames.images[3], true, RIGHT_LEFT_IMG_MATCH)); + } + if (isKeyframe(report) && frames.send_to_backend) { + iskeyframe = true; + } + processFrame(frames, iskeyframe); + report.ft_time = tic.toc(); + SPDLOG_INFO( + "frame_id: {} is_kf {}, landmark_num: {}/{}, mean_para {:.2f}%, " + "time_cost: {:.1f}ms ", + frames.frame_id, iskeyframe, report.parallex_num, frames.landmarkNum(), + report.meanParallex() * 100, report.ft_time); + if (params->show) { if (params->camera_configuration == CameraConfig::STEREO_PINHOLE) { - report.compose(track(frames.images[0], frames.motion_prediction)); - report.compose(track(frames.images[0], frames.images[1])); + draw(frames.images[0], frames.images[1], iskeyframe, report); } else if (params->camera_configuration == CameraConfig::PINHOLE_DEPTH) { - for (auto & frame : frames.images) { - report.compose(track(frame)); - } - } else if(params->camera_configuration == CameraConfig::FOURCORNER_FISHEYE) { - report.compose(track(frames.images[0], frames.motion_prediction)); - report.compose(track(frames.images[1], frames.motion_prediction)); - report.compose(track(frames.images[2], frames.motion_prediction)); - report.compose(track(frames.images[3], frames.motion_prediction)); - report.compose(track(frames.images[0], frames.images[1], true, LEFT_RIGHT_IMG_MATCH)); - report.compose(track(frames.images[1], frames.images[2], true, LEFT_RIGHT_IMG_MATCH)); - report.compose(track(frames.images[2], frames.images[3], true, LEFT_RIGHT_IMG_MATCH)); - report.compose(track(frames.images[0], frames.images[3], true, RIGHT_LEFT_IMG_MATCH)); - } - if (isKeyframe(report) && frames.send_to_backend) { - iskeyframe = true; - } - processFrame(frames, iskeyframe); - report.ft_time = tic.toc(); - if (params->verbose || params->enable_perf_output) - printf("[D2FeatureTracker] frame_id: %d, landmark_num: %d, time_cost: %.1fms\n", frames.frame_id, frames.landmarkNum(), report.ft_time); - if (params->show) { - if (params->camera_configuration == CameraConfig::STEREO_PINHOLE) { - draw(frames.images[0], frames.images[1], iskeyframe, report); - } else if (params->camera_configuration == CameraConfig::PINHOLE_DEPTH) { - for (auto & frame : frames.images) { - draw(frame, iskeyframe, report); - } - } else if (params->camera_configuration == CameraConfig::FOURCORNER_FISHEYE) { - draw(frames, iskeyframe, report); - } + for (auto &frame : frames.images) { + draw(frame, iskeyframe, report); + } + } else if (params->camera_configuration == + CameraConfig::FOURCORNER_FISHEYE) { + draw(frames, iskeyframe, report); } - return iskeyframe; + } + if (report.stereo_point_num > _config.min_stereo_points) { + frames.is_stereo = true; + } + return iskeyframe; } -bool D2FeatureTracker::getMatchedPrevKeyframe(const VisualImageDescArray & frame_a, VisualImageDescArray& prev, int & dir_a, int & dir_b) { - const Guard lock(keyframe_lock); - if (current_keyframes.size() == 0) { +bool D2FeatureTracker::getMatchedPrevKeyframe( + const VisualImageDescArray &frame_a, VisualImageDescArray &prev, int &dir_a, + int &dir_b) { + const Guard lock(keyframe_lock); + if (current_keyframes.size() == 0) { + return false; + } + if (params->camera_configuration == CameraConfig::STEREO_PINHOLE || + params->camera_configuration == CameraConfig::PINHOLE_DEPTH) { + // Reverse seach in current keyframes + for (int i = current_keyframes.size() - 1; i >= 0; i--) { + const auto &last = current_keyframes[i]; + if (frame_a.images.size() == 0 || + frame_a.images[0].image_desc.size() != params->netvlad_dims) { + ROS_ERROR("No vaild frame.image_desc.size() frame_id {}", + frame_a.frame_id); return false; + } + const Map vlad_desc_remote( + frame_a.images[0].image_desc.data(), params->netvlad_dims); + const Map vlad_desc(last.images[0].image_desc.data(), + params->netvlad_dims); + double netvlad_similar = vlad_desc.dot(vlad_desc_remote); + if (netvlad_similar < params->track_remote_netvlad_thres) { + spdlog::debug( + "D{} Remote image does not match current image {:.2f}/{:.2f}", + params->self_id, netvlad_similar, + params->track_remote_netvlad_thres); + } else { + spdlog::debug("D{} Remote image match image {}({}) {:.2f}/{:.2f}", + params->self_id, i, last.frame_id, netvlad_similar, + params->track_remote_netvlad_thres); + prev = last; + dir_a = 0; + dir_b = 0; + return true; + } } - if (params->camera_configuration == CameraConfig::STEREO_PINHOLE || params->camera_configuration == CameraConfig::PINHOLE_DEPTH) { - //Reverse seach in current keyframes - for (int i = current_keyframes.size() - 1; i >= 0; i--) { - const auto & last = current_keyframes[i]; - if (frame_a.images.size() == 0 || - frame_a.images[0].image_desc.size() != params->netvlad_dims) { - ROS_ERROR("[D2FeatureTracker::trackRemote] Warn: no vaild frame.image_desc.size() frame_id %ld ", frame_a.frame_id); - return false; - } - const Map vlad_desc_remote(frame_a.images[0].image_desc.data(), params->netvlad_dims); - const Map vlad_desc(last.images[0].image_desc.data(), params->netvlad_dims); - double netvlad_similar = vlad_desc.dot(vlad_desc_remote); - if (netvlad_similar < params->track_remote_netvlad_thres) { - // if (params->verbose) - // printf("[D2FeatureTracker::trackRemote@%d] Remote image does not match current image %.2f/%.2f\n", params->self_id, netvlad_similar, params->track_remote_netvlad_thres); - } else { - if (params->verbose) - printf("[D2FeatureTracker::trackRemote@%d] Remote image match image %d(%ld) %.2f/%.2f\n", params->self_id, - i, last.frame_id, netvlad_similar, params->track_remote_netvlad_thres); - prev = last; - dir_a = 0; - dir_b = 0; - return true; - } - } - } - if (params->camera_configuration == CameraConfig::FOURCORNER_FISHEYE) { - std::vector dirs{2, 3, 0, 1}; - dir_a = 2; - // printf("[D2FeatureTracker::getMatchedPrevKeyframe] Remote frame %ld view 2/%ld: gdesc %ld\n", frame_a.frame_id, frame_a.images.size(), frame_a.images[2].image_desc.size()); - const Map vlad_desc_remote(frame_a.images[2].image_desc.data(), params->netvlad_dims); - for (int i = current_keyframes.size() - 1; i >= 0; i--) { - const auto & last = current_keyframes[i]; - for (int j = 0; j < last.images.size(); j++) { - const Map vlad_desc(last.images[dirs[j]].image_desc.data(), params->netvlad_dims); - double netvlad_similar = vlad_desc.dot(vlad_desc_remote); - if (netvlad_similar < params->track_remote_netvlad_thres) { - } else { - prev = last; - dir_b = dirs[j]; - if (params->verbose) { - printf("[D2FeatureTracker::trackRemote@%d] Remote image match image drone %d(%ld) dir %d:%d %.2f/%.2f\n", params->self_id, - i, last.frame_id, dir_a, dir_b, netvlad_similar, params->track_remote_netvlad_thres); - } - return true; - } - } + } + if (params->camera_configuration == CameraConfig::FOURCORNER_FISHEYE) { + std::vector dirs{2, 3, 0, 1}; + dir_a = 2; + // printf("[D2FeatureTracker::getMatchedPrevKeyframe] Remote frame %ld view + // 2/%ld: gdesc %ld\n", frame_a.frame_id, frame_a.images.size(), + // frame_a.images[2].image_desc.size()); + const Map vlad_desc_remote( + frame_a.images[2].image_desc.data(), params->netvlad_dims); + for (int i = current_keyframes.size() - 1; i >= 0; i--) { + const auto &last = current_keyframes[i]; + for (int j = 0; j < last.images.size(); j++) { + const Map vlad_desc( + last.images[dirs[j]].image_desc.data(), params->netvlad_dims); + double netvlad_similar = vlad_desc.dot(vlad_desc_remote); + if (netvlad_similar < params->track_remote_netvlad_thres) { + } else { + prev = last; + dir_b = dirs[j]; + spdlog::debug( + "D{} Remote image match image drone {}({}) dir {}:{} " + "{:.2f}/{:.2f}", + params->self_id, i, last.frame_id, dir_a, dir_b, netvlad_similar, + params->track_remote_netvlad_thres); + return true; } + } } - return false; + } + return false; } -bool D2FeatureTracker::trackRemoteFrames(VisualImageDescArray & frames) { - const Guard lock(track_lock); - if (frames.is_lazy_frame || frames.matched_frame >= 0) { - printf("[D2FeatureTracker::trackRemoteFrames] lazy frame or matched frame, skip\n"); - return false; - } - bool matched = false; - landmark_predictions_viz.clear(); - frame_count ++; - TrackReport report; - TicToc tic; - int dir_cur = 0, dir_prev = 0; - VisualImageDescArray prev; - bool succ = getMatchedPrevKeyframe(frames, prev, dir_cur, dir_prev); - if (!succ) { - return false; +bool D2FeatureTracker::trackRemoteFrames(VisualImageDescArray &frames) { + const Guard lock(track_lock); + if (frames.is_lazy_frame || frames.matched_frame >= 0) { + return false; + } + bool matched = false; + landmark_predictions_viz.clear(); + frame_count++; + TrackReport report; + TicToc tic; + int dir_cur = 0, dir_prev = 0; + VisualImageDescArray prev; + bool succ = getMatchedPrevKeyframe(frames, prev, dir_cur, dir_prev); + if (!succ) { + return false; + } + bool use_motion_predict = frames.reference_frame_id == reference_frame_id && + _config.enable_search_local_aera_remote; + // printf("[D2FeatureTracker::trackRemoteFrames] frame %ld ref %d cur_ref %d + // use_motion_predict %d\n", + // frames.frame_id, frames.reference_frame_id, reference_frame_id, + // use_motion_predict); + if (params->camera_configuration == CameraConfig::STEREO_PINHOLE || + params->camera_configuration == CameraConfig::PINHOLE_DEPTH) { + report.compose(trackRemote(frames.images[0], prev.images[0], + use_motion_predict, frames.pose_drone)); + if (report.remote_matched_num > 0 && + params->camera_configuration == CameraConfig::STEREO_PINHOLE && + !_config.lr_match_use_lk) { + report.compose(trackRemote(frames.images[1], prev.images[1], + use_motion_predict, frames.pose_drone)); } - bool use_motion_predict = frames.reference_frame_id == reference_frame_id && _config.enable_search_local_aera_remote; - // printf("[D2FeatureTracker::trackRemoteFrames] frame %ld ref %d cur_ref %d use_motion_predict %d\n", - // frames.frame_id, frames.reference_frame_id, reference_frame_id, use_motion_predict); - if (params->camera_configuration == CameraConfig::STEREO_PINHOLE || params->camera_configuration == CameraConfig::PINHOLE_DEPTH) { - report.compose(trackRemote(frames.images[0], prev.images[0], use_motion_predict, frames.pose_drone)); - if (report.remote_matched_num > 0 && params->camera_configuration == CameraConfig::STEREO_PINHOLE) { - report.compose(trackRemote(frames.images[1], prev.images[1], use_motion_predict, frames.pose_drone)); - } - } else if (params->camera_configuration == CameraConfig::FOURCORNER_FISHEYE) { - int max_dirs = 4; - std::vector dirs_cur; - std::vector dirs_prev; - for (int _dir_a = dir_cur; _dir_a < dir_cur + max_dirs; _dir_a ++) { - int dir_a = _dir_a % max_dirs; - int dir_b = ((dir_prev - dir_cur + max_dirs) % max_dirs + _dir_a)% max_dirs; - if (dir_a < frames.images.size() && dir_b < prev.images.size()) { - if (prev.images[dir_b].spLandmarkNum() > 0 && frames.images[dir_a].spLandmarkNum() > 0) { - dirs_cur.push_back(dir_a); - dirs_prev.push_back(dir_b); - } - } - } - for (size_t i = 0; i < dirs_cur.size(); i++) { - int dir_cur = dirs_cur[i]; - int dir_prev = dirs_prev[i]; - // printf("[D2FeatureTracker::trackRemoteFrames] dir %d:%d\n", dir_cur, dir_prev); - report.compose(trackRemote(frames.images[dir_cur], prev.images[dir_prev], use_motion_predict, frames.pose_drone)); + } else if (params->camera_configuration == CameraConfig::FOURCORNER_FISHEYE) { + int max_dirs = 4; + std::vector dirs_cur; + std::vector dirs_prev; + for (int _dir_a = dir_cur; _dir_a < dir_cur + max_dirs; _dir_a++) { + int dir_a = _dir_a % max_dirs; + int dir_b = + ((dir_prev - dir_cur + max_dirs) % max_dirs + _dir_a) % max_dirs; + if (dir_a < frames.images.size() && dir_b < prev.images.size()) { + if (prev.images[dir_b].spLandmarkNum() > 0 && + frames.images[dir_a].spLandmarkNum() > 0) { + dirs_cur.push_back(dir_a); + dirs_prev.push_back(dir_b); } + } } - if (params->show && params->send_whole_img_desc && params->send_img) { - if (params->camera_configuration == CameraConfig::STEREO_PINHOLE) { - drawRemote(frames, report); - } + for (size_t i = 0; i < dirs_cur.size(); i++) { + int dir_cur = dirs_cur[i]; + int dir_prev = dirs_prev[i]; + // printf("[D2FeatureTracker::trackRemoteFrames] dir %d:%d\n", dir_cur, + // dir_prev); + report.compose(trackRemote(frames.images[dir_cur], prev.images[dir_prev], + use_motion_predict, frames.pose_drone)); } - report.ft_time = tic.toc(); - if (params->verbose || params->enable_perf_output) - printf("[D2FeatureTracker::trackRemoteFrames] frame %ld, matched %d, time %.2fms\n", frames.frame_id, report.remote_matched_num, report.ft_time); - if (report.remote_matched_num > 0) { - return true; - } else { - return false; + } + if (params->show && params->send_whole_img_desc && params->send_img) { + if (params->camera_configuration == CameraConfig::STEREO_PINHOLE) { + drawRemote(frames, report); } + } + report.ft_time = tic.toc(); + if (params->enable_perf_output) + SPDLOG_INFO( + "[D2FeatureTracker::trackRemoteFrames] frame {}, matched {}, time " + "{:.2f}ms", + frames.frame_id, report.remote_matched_num, report.ft_time); + if (report.remote_matched_num > 0) { + return true; + } else { + return false; + } } -TrackReport D2FeatureTracker::trackRemote(VisualImageDesc & frame, const VisualImageDesc & prev_frame, - bool use_motion_predict, const Swarm::Pose & motion_prediction) { - TrackReport report; - if (current_keyframes.size() == 0) { - printf("[D2FeatureTracker::trackRemote] waiting for initialization.\n"); - return report; - } - if (prev_frame.frame_id != frame.frame_id) { - //Then current keyframe has been assigned, feature tracker by LK. - std::vector ids_b_to_a; - //TODO: use motion prediction - MatchLocalFeatureParams match_param; - match_param.enable_superglue = _config.enable_superglue_remote; - match_param.enable_prediction = use_motion_predict; - match_param.pose_b_prediction = motion_prediction; - match_param.pose_a = prev_frame.pose_drone; - match_param.type = WHOLE_IMG_MATCH; - match_param.search_radius = search_radius*2; // search radius is 100% larger - match_param.plot = false; - bool success = matchLocalFeatures(prev_frame, frame, ids_b_to_a, match_param); - if (!success) { - printf("[D2FeatureTracker::trackRemote] matchLocalFeatures failed.\n"); - return report; - } - for (size_t i = 0; i < ids_b_to_a.size(); i++) { - if (ids_b_to_a[i] >= 0) { - assert(ids_b_to_a[i] < prev_frame.landmarkNum() && "too large"); - auto local_index = ids_b_to_a[i]; - auto &remote_lm = frame.landmarks[i]; - auto &local_lm = prev_frame.landmarks[local_index]; - if (remote_lm.landmark_id >= 0 && local_lm.landmark_id>=0) { - if (local_to_remote.find(local_lm.landmark_id) == local_to_remote.end()) { - local_to_remote[local_lm.landmark_id] = std::unordered_map(); - } - if (local_to_remote[local_lm.landmark_id].find(frame.drone_id) != local_to_remote[local_lm.landmark_id].end() && - local_to_remote[local_lm.landmark_id][frame.drone_id] != remote_lm.landmark_id) { - // printf("[D2FeatureTracker::trackRemote] Possible ambiguous local landmark %ld for drone %ld prev matched to %ld now %ld \n", - // local_lm.landmark_id, frame.drone_id, remote_lm.landmark_id, remote_lm.landmark_id); - } - remote_to_local[remote_lm.landmark_id] = local_lm.landmark_id; - // printf("[D2FeatureTracker::trackRemote] remote landmark %ld (prev %ld) -> local landmark %ld camera %ld \n", - // remote_lm.landmark_id, local_to_remote[local_lm.landmark_id][frame.drone_id], local_lm.landmark_id, frame.camera_id); - local_to_remote[local_lm.landmark_id][frame.drone_id] = remote_lm.landmark_id; - remote_lm.landmark_id = local_lm.landmark_id; - if (_config.double_counting_common_feature || local_lm.stamp_discover < remote_lm.stamp_discover) { - remote_lm.solver_id = params->self_id; - } else { - remote_lm.solver_id = frame.drone_id; - } - // printf("[D2FeatureTracker::trackRemote] landmark %ld will solve by %ld stamp %.3f:%.3f\n", - // remote_lm.landmark_id, remote_lm.solver_id, local_lm.stamp_discover, remote_lm.stamp_discover); - report.remote_matched_num ++; - } - } +TrackReport D2FeatureTracker::trackRemote( + VisualImageDesc &frame, const VisualImageDesc &prev_frame, + bool use_motion_predict, const Swarm::Pose &motion_prediction) { + TrackReport report; + if (current_keyframes.size() == 0) { + SPDLOG_INFO( + "[D2FeatureTracker::trackRemote] waiting for initialization.\n"); + return report; + } + if (prev_frame.frame_id != frame.frame_id) { + // Then current keyframe has been assigned, feature tracker by LK. + std::vector ids_b_to_a; + // TODO: use motion prediction + MatchLocalFeatureParams match_param; + match_param.enable_superglue = _config.enable_superglue_remote; + match_param.enable_prediction = use_motion_predict; + match_param.pose_b_prediction = motion_prediction; + match_param.pose_a = prev_frame.pose_drone; + match_param.type = WHOLE_IMG_MATCH; + match_param.search_radius = + search_radius * 2; // search radius is 100% larger + match_param.plot = false; + bool success = + matchLocalFeatures(prev_frame, frame, ids_b_to_a, match_param); + if (!success) { + SPDLOG_WARN("matchLocalFeatures failed"); + return report; + } + for (size_t i = 0; i < ids_b_to_a.size(); i++) { + if (ids_b_to_a[i] >= 0) { + assert(ids_b_to_a[i] < prev_frame.landmarkNum() && "too large"); + auto local_index = ids_b_to_a[i]; + auto &remote_lm = frame.landmarks[i]; + auto &local_lm = prev_frame.landmarks[local_index]; + if (remote_lm.landmark_id >= 0 && local_lm.landmark_id >= 0) { + if (local_to_remote.find(local_lm.landmark_id) == + local_to_remote.end()) { + local_to_remote[local_lm.landmark_id] = + std::unordered_map(); + } + if (local_to_remote[local_lm.landmark_id].find(frame.drone_id) != + local_to_remote[local_lm.landmark_id].end() && + local_to_remote[local_lm.landmark_id][frame.drone_id] != + remote_lm.landmark_id) { + // printf("[D2FeatureTracker::trackRemote] Possible ambiguous local + // landmark %ld for drone %ld prev matched to %ld now %ld \n", + // local_lm.landmark_id, frame.drone_id, remote_lm.landmark_id, + // remote_lm.landmark_id); + } + remote_to_local[remote_lm.landmark_id] = local_lm.landmark_id; + // printf("[D2FeatureTracker::trackRemote] remote landmark %ld (prev + // %ld) -> local landmark %ld camera %ld \n", + // remote_lm.landmark_id, + // local_to_remote[local_lm.landmark_id][frame.drone_id], + // local_lm.landmark_id, frame.camera_id); + local_to_remote[local_lm.landmark_id][frame.drone_id] = + remote_lm.landmark_id; + remote_lm.landmark_id = local_lm.landmark_id; + if (_config.double_counting_common_feature || + local_lm.stamp_discover < remote_lm.stamp_discover) { + remote_lm.solver_id = params->self_id; + } else { + remote_lm.solver_id = frame.drone_id; + } + // printf("[D2FeatureTracker::trackRemote] landmark %ld will solve by + // %ld stamp %.3f:%.3f\n", + // remote_lm.landmark_id, remote_lm.solver_id, + // local_lm.stamp_discover, remote_lm.stamp_discover); + report.remote_matched_num++; } + } } - // printf("[D2Frontend::D2FeatureTracker] match %d@cam%d<->%d@cam%d report.remote_matched_num %d\n", - // frame.drone_id, frame.camera_index, prev_frame.drone_id, frame.camera_index, report.remote_matched_num); - return report; + } + spdlog::debug( + "[D2Frontend::D2FeatureTracker] match {}@cam{}<->{}@cam{} " + "report.remote_matched_num {}", + frame.drone_id, frame.camera_index, prev_frame.drone_id, + frame.camera_index, report.remote_matched_num); + return report; } -void D2FeatureTracker::cvtRemoteLandmarkId(VisualImageDesc & frame) const { - int count = 0; - for (auto & lm : frame.landmarks) { - if (lm.landmark_id > 0 && remote_to_local.find(lm.landmark_id) != remote_to_local.end()) { - // printf("Lm remote %ld -> %ld camera %ld\n", lm.landmark_id, remote_to_local.at(lm.landmark_id), lm.camera_id); - lm.landmark_id = remote_to_local.at(lm.landmark_id); - count ++; - } +void D2FeatureTracker::cvtRemoteLandmarkId(VisualImageDesc &frame) const { + int count = 0; + for (auto &lm : frame.landmarks) { + if (lm.landmark_id > 0 && + remote_to_local.find(lm.landmark_id) != remote_to_local.end()) { + // printf("Lm remote %ld -> %ld camera %ld\n", lm.landmark_id, + // remote_to_local.at(lm.landmark_id), lm.camera_id); + lm.landmark_id = remote_to_local.at(lm.landmark_id); + count++; } - // printf("[D2FeatureTracker::cvtRemoteLandmarkId] Remote eff stereo %d\n", count); + } + // printf("[D2FeatureTracker::cvtRemoteLandmarkId] Remote eff stereo %d\n", + // count); } - -TrackReport D2FeatureTracker::track(VisualImageDesc & frame, const Swarm::Pose & motion_prediction) { - TrackReport report; - if (current_keyframes.size() > 0 && current_keyframes.back().frame_id != frame.frame_id) { - auto & current_keyframe = current_keyframes.back(); - //Then current keyframe has been assigned, feature tracker by LK. - auto & previous = current_keyframe.images[params->camera_seq[frame.camera_index]]; - std::vector ids_b_to_a; - MatchLocalFeatureParams match_param; - match_param.enable_superglue = _config.enable_superglue_local; - match_param.enable_prediction = _config.enable_motion_prediction_local; - match_param.pose_a = previous.pose_drone; - match_param.pose_b_prediction = motion_prediction; - match_param.search_radius = search_radius; - match_param.enable_search_in_local = true; - matchLocalFeatures(previous, frame, ids_b_to_a, match_param); - for (size_t i = 0; i < ids_b_to_a.size(); i++) { - if (ids_b_to_a[i] >= 0) { - assert(ids_b_to_a[i] < previous.spLandmarkNum() && "too large"); - auto prev_index = ids_b_to_a[i]; - auto landmark_id = previous.landmarks[prev_index].landmark_id; - auto &cur_lm = frame.landmarks[i]; - auto &prev_lm = previous.landmarks[prev_index]; - cur_lm.landmark_id = landmark_id; - cur_lm.velocity = cur_lm.pt3d_norm - prev_lm.pt3d_norm; - cur_lm.velocity /= (frame.stamp - current_keyframe.stamp); - cur_lm.stamp_discover = prev_lm.stamp_discover; - lmanager->updateLandmark(cur_lm); - report.sum_parallex += (prev_lm.pt3d_norm - cur_lm.pt3d_norm).norm(); - // printf("[D2FeatureTracker::track] landmark %ld cam_idx %d<->%d frame_cam_idx %d<->%d parallex %.1f%% prev_2d %.1f %.1f cur_2d %.3f %.3f prev_3d %.3f %.3f %.3f cur_3d %.3f %.3f %.3f\n", - // landmark_id, prev_lm.camera_index, cur_lm.camera_index, previous.camera_index, frame.camera_index, - // (prev_lm.pt3d_norm - cur_lm.pt3d_norm).norm()*100, prev_lm.pt2d.x, prev_lm.pt2d.y, cur_lm.pt2d.x, cur_lm.pt2d.y, - // prev_lm.pt3d_norm.x(), prev_lm.pt3d_norm.y(), prev_lm.pt3d_norm.z(), cur_lm.pt3d_norm.x(), cur_lm.pt3d_norm.y(), cur_lm.pt3d_norm.z()); - report.parallex_num ++; - if (lmanager->at(landmark_id).track.size() >= _config.long_track_frames) { - report.long_track_num ++; - } else { - report.unmatched_num ++; - } - } +TrackReport D2FeatureTracker::track(VisualImageDesc &frame, + const Swarm::Pose &motion_prediction) { + TrackReport report; + if (!_config.sp_track_use_lk && current_keyframes.size() > 0 && + current_keyframes.back().frame_id != frame.frame_id) { + const auto &base_frame = _config.track_from_keyframe + ? getLatestKeyframe() + : current_keyframes.back(); + const auto &base_kfframe = getLatestKeyframe(); + // Then current keyframe has been assigned, feature tracker by LK. + auto &previous = base_frame.images[params->camera_seq[frame.camera_index]]; + std::vector ids_b_to_a; + MatchLocalFeatureParams match_param; + match_param.enable_superglue = _config.enable_superglue_local; + match_param.enable_prediction = _config.enable_motion_prediction_local; + match_param.pose_a = previous.pose_drone; + match_param.pose_b_prediction = motion_prediction; + match_param.search_radius = search_radius; + match_param.enable_search_in_local = true; + matchLocalFeatures(previous, frame, ids_b_to_a, match_param); + for (size_t i = 0; i < ids_b_to_a.size(); i++) { + if (ids_b_to_a[i] >= 0) { + assert(ids_b_to_a[i] < previous.spLandmarkNum() && "too large"); + auto prev_index = ids_b_to_a[i]; + auto landmark_id = previous.landmarks[prev_index].landmark_id; + auto &cur_lm = frame.landmarks[i]; + auto [succ, prev_lm] = getPreviousLandmarkFrame( + previous.landmarks[prev_index], base_kfframe.frame_id); + if (!succ) { + continue; } + cur_lm.landmark_id = landmark_id; + cur_lm.velocity = cur_lm.pt3d_norm - prev_lm.pt3d_norm; + cur_lm.velocity /= (frame.stamp - base_frame.stamp); + cur_lm.stamp_discover = prev_lm.stamp_discover; + lmanager->updateLandmark(cur_lm); + report.sum_parallex += (prev_lm.pt3d_norm - cur_lm.pt3d_norm).norm(); + // printf("[D2FeatureTracker::track] landmark %ld cam_idx %d<->%d + // frame_cam_idx %d<->%d parallex %.1f%% prev_2d %.1f %.1f cur_2d %.3f + // %.3f prev_3d %.3f %.3f %.3f cur_3d %.3f %.3f %.3f\n", + // landmark_id, prev_lm.camera_index, cur_lm.camera_index, + // previous.camera_index, frame.camera_index, (prev_lm.pt3d_norm - + // cur_lm.pt3d_norm).norm()*100, prev_lm.pt2d.x, prev_lm.pt2d.y, + // cur_lm.pt2d.x, cur_lm.pt2d.y, prev_lm.pt3d_norm.x(), + // prev_lm.pt3d_norm.y(), prev_lm.pt3d_norm.z(), + // cur_lm.pt3d_norm.x(), cur_lm.pt3d_norm.y(), + // cur_lm.pt3d_norm.z()); + report.parallex_num++; + if (lmanager->at(landmark_id).track.size() >= + _config.long_track_frames) { + report.long_track_num++; + } else { + report.unmatched_num++; + } + } } - if (_config.enable_lk_optical_flow) { - //Enable LK optical flow feature tracker also. - //This is for the case that the superpoint features is not tracked well. - report.compose(trackLK(frame)); - } - return report; + } + if (_config.enable_lk_optical_flow || _config.sp_track_use_lk) { + // Enable LK optical flow feature tracker also. + // This is for the case that the superpoint features is not tracked well. + report.compose(trackLK(frame)); + } + return report; } -TrackReport D2FeatureTracker::trackLK(VisualImageDesc & frame) { - //Track LK points - TrackReport report; - if (prev_lk_info.find(frame.camera_index) == prev_lk_info.end()) { - prev_lk_info[frame.camera_index] = LKImageInfo(); - cv::cuda::GpuMat image_cuda(frame.raw_image); - prev_lk_info[frame.camera_index].pyr = buildImagePyramid(image_cuda); - } - auto cur_lk_pts = prev_lk_info[frame.camera_index].lk_pts; - auto cur_lk_ids = prev_lk_info[frame.camera_index].lk_ids; - if (!cur_lk_ids.empty()) { - int prev_lk_num = cur_lk_ids.size(); - cur_lk_pts = opticalflowTrackPyr(frame.raw_image, prev_lk_info[frame.camera_index].pyr, cur_lk_pts, cur_lk_ids, - TrackLRType::WHOLE_IMG_MATCH, true); - if (params->verbose) { - printf("[D2FeatureTracker::trackLK] track %d LK points, %d lost, track rate %.1f%%\n", - prev_lk_num, prev_lk_num - cur_lk_pts.size(), cur_lk_pts.size() * 100.0 / prev_lk_num); - } +const VisualImageDescArray &D2FeatureTracker::getLatestKeyframe() const { + // Return the previous keyframe + assert(current_keyframes.size() > 0 && "Must have previous keyframe"); + for (auto it = current_keyframes.rbegin(); it != current_keyframes.rend(); + it++) { + if (it->is_keyframe) { + spdlog::debug("Found previous keyframe {}", it->frame_id); + return *it; } - auto cur_all_pts = frame.landmarks2D(); - cur_all_pts.insert(cur_all_pts.end(), cur_lk_pts.begin(), cur_lk_pts.end()); - for (int i = 0; i < cur_lk_pts.size(); i++) { - auto ret = createLKLandmark(frame, cur_lk_pts[i], cur_lk_ids[i]); - if (!ret.first) { + } + spdlog::debug("Not found previous keyframe {}, returning beginning"); + return *current_keyframes.begin(); +} + +TrackReport D2FeatureTracker::trackLK(VisualImageDesc &frame) { + // Track LK points + TrackReport report; + LKImageInfoGPU cur_lk_info; + auto cur_landmarks = frame.landmarks; + auto cur_landmark_desc = frame.landmark_descriptor; + auto cur_landmark_scores = frame.landmark_scores; + if (_config.sp_track_use_lk) { + frame.clearLandmarks(); + } + + if (_config.sp_track_use_lk || _config.lr_match_use_lk) { + bool pyr_has_built = false; + if (keyframe_lk_infos.size() > 0 && current_keyframes.size() > 0) { + const auto &prev_frame = current_keyframes.back(); + const auto &prev_image = prev_frame.images[frame.camera_index]; + const auto &prev_keyframe = getLatestKeyframe(); + const auto &prev_lk = + keyframe_lk_infos.at(prev_frame.frame_id).at(frame.camera_index); + if (!prev_lk.lk_ids.empty()) { + int prev_lk_num = prev_lk.lk_ids.size(); + cur_lk_info = opticalflowTrackPyr(frame.raw_image, prev_lk, + TrackLRType::WHOLE_IMG_MATCH); + pyr_has_built = true; + cur_lk_info.lk_pts_3d_norm.resize(cur_lk_info.lk_pts.size()); + for (int i = 0; i < cur_lk_info.lk_pts.size(); i++) { + auto ret = + createLKLandmark(frame, cur_lk_info.lk_pts[i], + cur_lk_info.lk_ids[i], cur_lk_info.lk_types[i]); + cur_lk_info.lk_pts_3d_norm[i] = ret.second.pt3d_norm; + if (!ret.first) { continue; + } + if (_config.sp_track_use_lk) { + // Copy the landmark descriptor from previous frame + frame.landmark_descriptor.insert( + frame.landmark_descriptor.end(), + prev_image.landmark_descriptor.begin() + + cur_lk_info.lk_local_index[i] * params->superpoint_dims, + prev_image.landmark_descriptor.begin() + + (cur_lk_info.lk_local_index[i] + 1) * + params->superpoint_dims); + frame.landmark_scores.emplace_back( + prev_image.landmark_scores[cur_lk_info.lk_local_index[i]]); + cur_lk_info.lk_local_index[i] = frame.landmarks.size(); + } + auto &lm = ret.second; + auto track = lmanager->at(cur_lk_info.lk_ids[i]).track; + lm.velocity = extractPointVelocity(lm); + frame.landmarks.emplace_back(lm); + auto [succ, prev_lm] = + getPreviousLandmarkFrame(lm, prev_keyframe.frame_id); + if (succ) { + lm.stamp_discover = prev_lm.stamp_discover; + } else { + // SPDLOG_INFO("getPreviousLandmarkFrame failed"); + continue; + } + if (lmanager->at(cur_lk_info.lk_ids[i]).track.size() >= + _config.long_track_frames) { + report.long_track_num++; + } + + report.sum_parallex += (lm.pt3d_norm - prev_lm.pt3d_norm).norm(); + // spdlog::debug("LM {} prev_2d {:.1f} {:.1f} cur_2d {:.3f} {:.3f} + // para_2d {:.1f}% prev_3d {:.3f} {:.3f} {:.3f} cur_3d {:.3f} {:.3f} + // {:.3f} para_3d {:.1f}%", + // prev_lm.landmark_id, prev_lm.pt2d.x, prev_lm.pt2d.y, + // lm.pt2d.x, lm.pt2d.y, cv::norm(prev_lm.pt2d - + // lm.pt2d)*100.0, prev_lm.pt3d_norm.x(), + // prev_lm.pt3d_norm.y(), prev_lm.pt3d_norm.z(), + // lm.pt3d_norm.x(), lm.pt3d_norm.y(), lm.pt3d_norm.z(), + // (prev_lm.pt3d_norm - lm.pt3d_norm).norm()*100.0); + report.parallex_num++; } - auto &lm = ret.second; - auto track = lmanager->at(cur_lk_ids[i]).track; - lm.velocity = extractPointVelocity(lm); - auto prev_found = getPreviousLandmarkFrame(lm); - if (prev_found.first) { - lm.stamp_discover = prev_found.second.stamp_discover; - } - frame.landmarks.emplace_back(lm); - if (lmanager->at(cur_lk_ids[i]).track.size() >= _config.long_track_frames) { - report.long_track_num ++; + // SPDLOG_INFO("[D2FeatureTracker::trackLK] track {} LK points, {} lost, + // track rate {:.1f}% para {:.2f}% num {} {}->{}", + // prev_lk_num, prev_lk_num - cur_lk_info.lk_pts.size(), + // cur_lk_info.lk_pts.size() * 100.0 / prev_lk_num, + // report.meanParallex()*100, report.parallex_num, + // prev_frame.frame_id, frame.frame_id); + } + } + if (!pyr_has_built) { + cv::cuda::GpuMat image_cuda(frame.raw_image); + cur_lk_info.pyr = buildImagePyramid(image_cuda); + } + } + // Discover new points. + if (!frame.raw_image.empty()) { + if (_config.sp_track_use_lk) { + // In this case, select from cur_landmarks + int count_new = 0; + for (size_t i = 0; i < cur_landmarks.size(); i++) { + if (cur_lk_info.lk_pts.size() > params->total_feature_num) { + break; } - //When computing parallex, we go to last keyframe - if (!prev_found.first) { - continue; + auto lm = cur_landmarks[i]; + // If not near to any existing landmarks, add it + bool has_near = false; + for (auto pt : cur_lk_info.lk_pts) { + if (cv::norm(pt - lm.pt2d) < params->feature_min_dist) { + has_near = true; + break; + } } - report.sum_parallex += (lm.pt3d_norm - prev_found.second.pt3d_norm).norm(); - report.parallex_num ++; - } - //Discover new points. - std::vector n_pts; - if (!frame.raw_image.empty()) { - TicToc t_det; - detectPoints(frame.raw_image, n_pts, cur_all_pts, params->total_feature_num, true, _config.lk_use_fast); - if (params->enable_perf_output) { - printf("[D2FeatureTracker::trackLK] detect %ld points in %.2fms\n", n_pts.size(), t_det.toc()); + if (!has_near) { + auto _id = lmanager->addLandmark(lm); + lm.landmark_id = _id; + frame.landmarks.emplace_back(lm); + frame.landmark_descriptor.insert( + frame.landmark_descriptor.end(), + cur_landmark_desc.begin() + i * params->superpoint_dims, + cur_landmark_desc.begin() + (i + 1) * params->superpoint_dims); + frame.landmark_scores.emplace_back(cur_landmark_scores[i]); + + cur_lk_info.lk_pts.emplace_back(lm.pt2d); + cur_lk_info.lk_ids.emplace_back(lm.landmark_id); + cur_lk_info.lk_local_index.emplace_back(frame.landmarks.size() - 1); + cur_lk_info.lk_pts_3d_norm.emplace_back(lm.pt3d_norm); + cur_lk_info.lk_types.emplace_back(LandmarkType::SuperPointLandmark); + count_new++; } + } + spdlog::debug("{} new points added", cur_lk_info.lk_pts.size(), + count_new); } else { - printf("[D2FeatureTracker::trackLK] empty image\n"); - } - report.unmatched_num += n_pts.size(); - for (auto & pt : n_pts) { + std::vector n_pts; + TicToc t_det; + detectPoints(frame.raw_image, n_pts, frame.landmarks2D(), + params->total_feature_num, true, _config.lk_use_fast); + spdlog::debug( + "[D2FeatureTracker::trackLK] detect {} points in {:.2f}ms\n", + n_pts.size(), t_det.toc()); + report.unmatched_num += n_pts.size(); + for (auto &pt : n_pts) { auto ret = createLKLandmark(frame, pt); if (!ret.first) { - continue; + continue; } auto &lm = ret.second; auto _id = lmanager->addLandmark(lm); lm.landmark_id = _id; frame.landmarks.emplace_back(lm); - cur_lk_pts.emplace_back(pt); - cur_lk_ids.emplace_back(_id); + cur_lk_info.lk_pts.emplace_back(pt); + cur_lk_info.lk_ids.emplace_back(_id); + cur_lk_info.lk_local_index.emplace_back(frame.landmarks.size() - 1); + cur_lk_info.lk_pts_3d_norm.emplace_back(lm.pt3d_norm); + cur_lk_info.lk_types.emplace_back(LandmarkType::FlowLandmark); + } } - prev_lk_info[frame.camera_index].lk_pts = cur_lk_pts; - prev_lk_info[frame.camera_index].lk_ids = cur_lk_ids; - prev_lk_info[frame.camera_index].image = frame.raw_image.clone(); - prev_lk_info[frame.camera_index].frame_id = frame.frame_id; - return report; + } else { + SPDLOG_ERROR("[D2FeatureTracker::trackLK] empty image\n"); + } + + keyframe_lk_infos[frame.frame_id][frame.camera_index] = cur_lk_info; + return report; } -std::pair D2FeatureTracker::getPreviousLandmarkFrame(const LandmarkPerFrame & lpf) const { - auto landmark_id = lpf.landmark_id; - // printf("[D2FeatureTracker::extractPointVelocity] landmark_id %d\n", landmark_id); - if (lmanager->hasLandmark(landmark_id) && lmanager->at(landmark_id).track.size() > 0) { - auto lm_per_id = lmanager->at(landmark_id); - for (int i = lm_per_id.track.size() - 1 ; i >= 0; i--) { - auto lm = lm_per_id.track[i]; - if (lm.landmark_id == landmark_id && lm.frame_id != lpf.frame_id && lm.camera_id == lpf.camera_id) { - return std::make_pair(true, lm); - } - } +std::pair D2FeatureTracker::getPreviousLandmarkFrame( + const LandmarkPerFrame &lpf, FrameIdType keyframe_id) const { + auto landmark_id = lpf.landmark_id; + // printf("[D2FeatureTracker::extractPointVelocity] landmark_id %d\n", + // landmark_id); + if (lmanager->hasLandmark(landmark_id) && + lmanager->at(landmark_id).track.size() > 0) { + auto lm_per_id = lmanager->at(landmark_id); + for (int i = lm_per_id.track.size() - 1; i >= 0; i--) { + auto lm = lm_per_id.track[i]; + if (lm.landmark_id == landmark_id && lm.camera_id == lpf.camera_id && + (keyframe_id < 0 || lm.frame_id == keyframe_id)) { + return {true, lm}; + } } - LandmarkPerFrame lm; - return std::make_pair(false, lm); + } + LandmarkPerFrame lm; + return {false, lm}; } -Vector3d D2FeatureTracker::extractPointVelocity(const LandmarkPerFrame & lpf) const { - auto landmark_id = lpf.landmark_id; - // printf("[D2FeatureTracker::extractPointVelocity] landmark_id %d\n", landmark_id); - auto ret = getPreviousLandmarkFrame(lpf); - if (ret.first) { - auto lm = ret.second; - Vector3d movement = lpf.pt3d_norm - lm.pt3d_norm; - auto vel = movement / (lpf.stamp - lm.stamp); - // printf("[D2FeatureTracker::extractPointVelocity] landmark %d, frame %d->%d, movement %f %f %f vel %f %f %f \n", - // landmark_id, lm.frame_id, lpf.frame_id, movement.x(), movement.y(), movement.z(), vel.x(), vel.y(), vel.z()); - return vel; - } - return Vector3d(0, 0, 0); +Vector3d D2FeatureTracker::extractPointVelocity( + const LandmarkPerFrame &lpf) const { + auto landmark_id = lpf.landmark_id; + // printf("[D2FeatureTracker::extractPointVelocity] landmark_id %d\n", + // landmark_id); + auto [succ, lm] = getPreviousLandmarkFrame(lpf); + if (succ) { + Vector3d movement = lpf.pt3d_norm - lm.pt3d_norm; + auto vel = movement / (lpf.stamp - lm.stamp); + // printf("[D2FeatureTracker::extractPointVelocity] landmark %d, frame + // %d->%d, movement %f %f %f vel %f %f %f \n", + // landmark_id, lm.frame_id, lpf.frame_id, movement.x(), movement.y(), + // movement.z(), vel.x(), vel.y(), vel.z()); + return vel; + } + return Vector3d(0, 0, 0); } -TrackReport D2FeatureTracker::track(const VisualImageDesc & left_frame, VisualImageDesc & right_frame, bool enable_lk, TrackLRType type) { - auto prev_pts = left_frame.landmarks2D(); - auto cur_pts = right_frame.landmarks2D(); - std::vector ids_b_to_a; - TrackReport report; - double search_radius_lr = search_radius; +TrackReport D2FeatureTracker::track(const VisualImageDesc &left_frame, + VisualImageDesc &right_frame, + bool enable_lk, TrackLRType type, + bool use_lk_for_left_right_track) { + auto prev_pts = left_frame.landmarks2D(); + auto cur_pts = right_frame.landmarks2D(); + std::vector ids_b_to_a; + TrackReport report; + if (!use_lk_for_left_right_track) { MatchLocalFeatureParams match_param; match_param.enable_superglue = _config.enable_superglue_local; - match_param.search_radius = search_radius_lr; + match_param.search_radius = _config.search_local_max_dist_lr * image_width; match_param.type = type; match_param.enable_prediction = true; match_param.prediction_using_extrinsic = true; match_param.enable_search_in_local = true; matchLocalFeatures(left_frame, right_frame, ids_b_to_a, match_param); - for (size_t i = 0; i < ids_b_to_a.size(); i++) { - if (ids_b_to_a[i] >= 0) { - assert(ids_b_to_a[i] < left_frame.spLandmarkNum() && "too large"); - auto prev_index = ids_b_to_a[i]; - auto landmark_id = left_frame.landmarks[prev_index].landmark_id; - auto &cur_lm = right_frame.landmarks[i]; - auto &prev_lm = left_frame.landmarks[prev_index]; - cur_lm.landmark_id = landmark_id; - cur_lm.stamp_discover = prev_lm.stamp_discover; - cur_lm.velocity = extractPointVelocity(cur_lm); - lmanager->updateLandmark(cur_lm); - report.stereo_point_num ++; - } - } - if (_config.enable_lk_optical_flow && enable_lk) { - trackLK(left_frame, right_frame, type); + for (size_t i = 0; i < ids_b_to_a.size(); i++) { + if (ids_b_to_a[i] >= 0) { + assert(ids_b_to_a[i] < left_frame.spLandmarkNum() && "too large"); + auto prev_index = ids_b_to_a[i]; + auto landmark_id = left_frame.landmarks[prev_index].landmark_id; + auto &cur_lm = right_frame.landmarks[i]; + auto &prev_lm = left_frame.landmarks[prev_index]; + cur_lm.landmark_id = landmark_id; + cur_lm.stamp_discover = prev_lm.stamp_discover; + cur_lm.velocity = extractPointVelocity(cur_lm); + lmanager->updateLandmark(cur_lm); + report.stereo_point_num++; + } } - return report; + } + if (_config.enable_lk_optical_flow && enable_lk || + use_lk_for_left_right_track) { + trackLK(left_frame, right_frame, type, use_lk_for_left_right_track); + } + return report; } -TrackReport D2FeatureTracker::trackLK(const VisualImageDesc & left_frame, VisualImageDesc & right_frame, TrackLRType type) { - //Track LK points - //This function MUST run after track(...) - TrackReport report; - auto cur_lk_pts = prev_lk_info[left_frame.camera_index].lk_pts; - auto cur_lk_ids = prev_lk_info[left_frame.camera_index].lk_ids; - auto cur_lk_pyr = prev_lk_info[left_frame.camera_index].pyr; - assert(left_frame.frame_id == prev_lk_info[left_frame.camera_index].frame_id); - if (!cur_lk_ids.empty()) { - cur_lk_pts = opticalflowTrackPyr(right_frame.raw_image, cur_lk_pyr, cur_lk_pts, cur_lk_ids, type, false); - } - // printf("[trackLK] indices %d<->%d track type %d LK points: %lu\n", left_frame.camera_index, right_frame.camera_index, type, cur_lk_pts.size()); - for (int i = 0; i < cur_lk_pts.size(); i++) { - auto ret = createLKLandmark(right_frame, cur_lk_pts[i], cur_lk_ids[i]); - if (!ret.first) { - continue; - } - auto &lm = ret.second; - lm.stamp_discover = lmanager->at(cur_lk_ids[i]).stamp_discover; - lm.velocity = extractPointVelocity(lm); - lmanager->updateLandmark(lm); +TrackReport D2FeatureTracker::trackLK(const VisualImageDesc &left_frame, + VisualImageDesc &right_frame, + TrackLRType type, + bool use_lk_for_left_right_track) { + // Track LK points + // This function MUST run after track(...) + TrackReport report; + auto left_lk_info = + keyframe_lk_infos.at(left_frame.frame_id).at(left_frame.camera_index); + // Add the SP points to the LK points if use_lk_for_left_right_track is true + if (use_lk_for_left_right_track && !_config.sp_track_use_lk) { + for (int i = 0; i < left_frame.landmarkNum(); i++) { + if (left_frame.landmarks[i].landmark_id >= 0 && + left_frame.landmarks[i].type == LandmarkType::SuperPointLandmark) { + left_lk_info.lk_pts.emplace_back(left_frame.landmarks[i].pt2d); + left_lk_info.lk_pts_3d_norm.emplace_back( + left_frame.landmarks[i].pt3d_norm); + left_lk_info.lk_ids.emplace_back(left_frame.landmarks[i].landmark_id); + left_lk_info.lk_local_index.emplace_back(i); + left_lk_info.lk_types.emplace_back(left_frame.landmarks[i].type); + } + } + } + std::map pts_pred_a_on_b; + if (_config.lk_lk_use_pred) { + pts_pred_a_on_b = predictLandmarksWithExtrinsic( + left_frame.camera_index, left_lk_info.lk_ids, + left_lk_info.lk_pts_3d_norm, left_frame.extrinsic, + right_frame.extrinsic); + } + if (!left_lk_info.lk_ids.empty()) { + auto cur_lk_info = + opticalflowTrackPyr(right_frame.raw_image, left_lk_info, type); + for (int i = 0; i < cur_lk_info.lk_pts.size(); i++) { + auto ret = + createLKLandmark(right_frame, cur_lk_info.lk_pts[i], + cur_lk_info.lk_ids[i], cur_lk_info.lk_types[i]); + if (!ret.first) { + continue; + } + auto &lm = ret.second; + lm.stamp_discover = lmanager->at(cur_lk_info.lk_ids[i]).stamp_discover; + lm.velocity = extractPointVelocity(lm); + lmanager->updateLandmark(lm); + auto pred = pts_pred_a_on_b.at(cur_lk_info.lk_ids[i]); + if (!_config.lk_lk_use_pred || + cv::norm(pred - cur_lk_info.lk_pts[i]) < + _config.search_local_max_dist_lr * image_width) { right_frame.landmarks.emplace_back(lm); + } } - report.stereo_point_num = cur_lk_pts.size(); - return report; + + report.stereo_point_num = right_frame.landmarks.size(); + } + return report; } -bool D2FeatureTracker::isKeyframe(const TrackReport & report) { - int prev_num = current_keyframes.size() > 0 ? current_keyframes.back().landmarkNum(): 0; - if (report.meanParallex() > 0.5) { - printf("[D2FeatureTracker] unexcepted mean parallex %f\n", report.meanParallex()); - } - if (keyframe_count < _config.min_keyframe_num || - report.long_track_num < _config.long_track_thres || - prev_num < _config.last_track_thres || - report.unmatched_num > _config.new_feature_thres*prev_num || //Unmatched is assumed to be new - report.meanParallex() > _config.parallex_thres) { //Attenion, if mismatch this will be big - if (params->verbose) { - printf("[D2FeatureTracker] keyframe_count: %d, long_track_num: %d, prev_num:%d, unmatched_num: %d, parallex: %f\n", - keyframe_count, report.long_track_num, prev_num, report.unmatched_num, report.meanParallex()); - } - return true; - } - return false; +bool D2FeatureTracker::isKeyframe(const TrackReport &report) { + int prev_num = + current_keyframes.size() > 0 ? current_keyframes.back().landmarkNum() : 0; + if (report.meanParallex() > 0.5) { + printf("unexcepted mean parallex %f\n", report.meanParallex()); + } + if (report.parallex_num < _config.min_keyframe_num || + report.long_track_num < _config.long_track_thres || + prev_num < _config.last_track_thres || + report.unmatched_num > _config.new_feature_thres * + prev_num || // Unmatched is assumed to be new + report.meanParallex() > + _config.parallex_thres) { // Attenion, if mismatch this will be big + spdlog::debug( + "New KF: keyframe_count: {}, long_track_num: {}, prev_num: {}, " + "unmatched_num: {}, parallex: {:.1f}%", + keyframe_count, report.long_track_num, prev_num, report.unmatched_num, + report.meanParallex() * 100); + return true; + } + return false; } -std::pair D2FeatureTracker::createLKLandmark(const VisualImageDesc & frame, cv::Point2f pt, LandmarkIdType landmark_id) { - Vector3d pt3d_norm; - cams.at(frame.camera_index)->liftProjective(Eigen::Vector2d(pt.x, pt.y), pt3d_norm); - pt3d_norm.normalize(); - if (pt3d_norm.hasNaN()) { - return std::make_pair(false, LandmarkPerFrame()); - } - LandmarkPerFrame lm = LandmarkPerFrame::createLandmarkPerFrame(landmark_id, frame.frame_id, frame.stamp, - LandmarkType::FlowLandmark, params->self_id, frame.camera_index, frame.camera_id, pt, pt3d_norm); - if (params->camera_configuration == CameraConfig::PINHOLE_DEPTH) { - //Add depth information - auto dep = frame.raw_depth_image.at(pt)/1000.0; - if (dep > params->loopcamconfig->DEPTH_NEAR_THRES && dep < params->loopcamconfig->DEPTH_FAR_THRES) { - auto pt3dcam = pt3d_norm*dep; - lm.depth = pt3dcam.norm(); - lm.depth_mea = true; - } +std::pair D2FeatureTracker::createLKLandmark( + const VisualImageDesc &frame, cv::Point2f pt, LandmarkIdType landmark_id, + LandmarkType type) { + Vector3d pt3d_norm = Vector3d::Zero(); + cams.at(frame.camera_index) + ->liftProjective(Eigen::Vector2d(pt.x, pt.y), pt3d_norm); + pt3d_norm.normalize(); + if (pt3d_norm.hasNaN()) { + return std::make_pair(false, LandmarkPerFrame()); + } + LandmarkPerFrame lm = LandmarkPerFrame::createLandmarkPerFrame( + landmark_id, frame.frame_id, frame.stamp, type, params->self_id, + frame.camera_index, frame.camera_id, pt, pt3d_norm); + if (params->camera_configuration == CameraConfig::PINHOLE_DEPTH) { + // Add depth information + auto dep = frame.raw_depth_image.at(pt) / 1000.0; + if (dep > params->loopcamconfig->DEPTH_NEAR_THRES && + dep < params->loopcamconfig->DEPTH_FAR_THRES) { + auto pt3dcam = pt3d_norm * dep; + lm.depth = pt3dcam.norm(); + lm.depth_mea = true; } - lm.color = extractColor(frame.raw_image, pt); - return std::make_pair(true, lm); + } + lm.color = extractColor(frame.raw_image, pt); + return std::make_pair(true, lm); } -void D2FeatureTracker::processFrame(VisualImageDescArray & frames, bool is_keyframe) { - if (current_keyframes.size() > 0 && current_keyframes.back().frame_id == frames.frame_id) { - return; - } - keyframe_count ++; - for (auto & frame: frames.images) { - for (unsigned int i = 0; i < frame.landmarkNum(); i++) { - if (frame.landmarks[i].landmark_id < 0) { - if (params->camera_configuration == CameraConfig::STEREO_PINHOLE && frame.camera_index == 1) { - //We do not create new landmark for right camera - continue; - } - auto _id = lmanager->addLandmark(frame.landmarks[i]); - frame.landmarks[i].setLandmarkId(_id); - } else { - lmanager->updateLandmark(frame.landmarks[i]); - } +void D2FeatureTracker::processFrame(VisualImageDescArray &frames, + bool is_keyframe) { + if (current_keyframes.size() > 0 && + current_keyframes.back().frame_id == frames.frame_id) { + return; + } + frames.is_keyframe = is_keyframe; + keyframe_count++; + for (auto &frame : frames.images) { + for (unsigned int i = 0; i < frame.landmarkNum(); i++) { + if (frame.landmarks[i].landmark_id < 0) { + if (params->camera_configuration == CameraConfig::STEREO_PINHOLE && + frame.camera_index == 1) { + // We do not create new landmark for right camera + continue; } + auto _id = lmanager->addLandmark(frame.landmarks[i]); + frame.landmarks[i].setLandmarkId(_id); + } else { + lmanager->updateLandmark(frame.landmarks[i]); + } } - // Before solve, use motion prediction as pose - for (auto & frame: frames.images) { - frame.pose_drone = frames.motion_prediction; + } + // Before solve, use motion prediction as pose + for (auto &frame : frames.images) { + frame.pose_drone = frames.motion_prediction; + } + frames.pose_drone = frames.motion_prediction; + if (is_keyframe || !_config.track_from_keyframe) { + spdlog::debug( + "[D2FeatureTracker::processFrame] Add to keyframe list, frame_id: {}", + frames.frame_id); + if (current_keyframes.size() > 0) { + keyframe_lk_infos.erase(current_keyframes.back().frame_id); } - frames.pose_drone = frames.motion_prediction; current_keyframes.emplace_back(frames); + } else { + keyframe_lk_infos.erase(frames.frame_id); + } } -cv::Mat D2FeatureTracker::drawToImage(const VisualImageDesc & frame, bool is_keyframe, const TrackReport & report, bool is_right, bool is_remote) const { - // ROS_INFO("Drawing ... %d", keyframe_count); - cv::Mat img = frame.raw_image; - int width = img.cols; - auto & current_keyframe = current_keyframes.back(); - if (is_remote) { - img = cv::imdecode(frame.image, cv::IMREAD_UNCHANGED); - width = img.cols; - if (img.empty()) { - return cv::Mat(); - } - // cv::hconcat(img, current_keyframe.images[0].raw_image, img); - } - auto cur_pts = frame.landmarks2D(); - if (img.channels() == 1) { - cv::cvtColor(img, img, cv::COLOR_GRAY2BGR); - } - char buf[64] = {0}; - int stereo_num = 0; - for (size_t j = 0; j < cur_pts.size(); j++) { - cv::Scalar color = cv::Scalar(0, 140, 255); - if (frame.landmarks[j].type == SuperPointLandmark) { - color = cv::Scalar(255, 0, 0); //Superpoint blue - } - cv::circle(img, cur_pts[j], 2, color, 2); - auto _id = frame.landmarks[j].landmark_id; - if (!lmanager->hasLandmark(_id)) { - continue; - } - auto lm = lmanager->at(_id); - if (_id >= 0) { - cv::Point2f prev; - bool prev_found = false; - if (!lmanager->hasLandmark(_id)) { - continue; - } - auto & pts2d = lmanager->at(_id).track; - if (pts2d.size() == 0) - continue; - if (is_right || is_remote) { - prev = pts2d.back().pt2d; - prev_found = true; - } else { - for (int index = pts2d.size()-1; index >= 0; index--) { - if (pts2d[index].camera_id == frame.camera_id && pts2d[index].frame_id != frame.frame_id) { - prev = lmanager->at(_id).track[index].pt2d; - prev_found = true; - break; - } - } - } - if (!prev_found) { - continue; - } - if (is_remote) { - // Random color - // cv::Scalar color(rand()%255, rand()%255, rand()%255); - // cv::line(img, prev + cv::Point2f(width, 0), cur_pts[j], color); - } else { - cv::arrowedLine(img, prev, cur_pts[j], cv::Scalar(0, 255, 0), 1, 8, 0, 0.2); - } - } - if (frame.landmarks[j].landmark_id >= 0) { - stereo_num++; - } - if (_config.show_feature_id && frame.landmarks[j].landmark_id >= 0) { - sprintf(buf, "%d", frame.landmarks[j].landmark_id%MAX_FEATURE_NUM); - cv::putText(img, buf, cur_pts[j] - cv::Point2f(5, 0), cv::FONT_HERSHEY_SIMPLEX, 1, color, 1); - } +cv::Mat D2FeatureTracker::drawToImage(const VisualImageDesc &frame, + bool is_keyframe, + const TrackReport &report, bool is_right, + bool is_remote) const { + // ROS_INFO("Drawing ... %d", keyframe_count); + cv::Mat img = frame.raw_image; + int width = img.cols; + auto ¤t_keyframe = current_keyframes.back(); + FrameIdType last_keyframe = -1; + if (current_keyframes.size() > 1) { + last_keyframe = current_keyframes[current_keyframes.size() - 2].frame_id; + } + if (is_remote) { + img = cv::imdecode(frame.image, cv::IMREAD_UNCHANGED); + width = img.cols; + if (img.empty()) { + return cv::Mat(); + } + // cv::hconcat(img, current_keyframe.images[0].raw_image, img); + } + auto cur_pts = frame.landmarks2D(); + if (img.channels() == 1) { + cv::cvtColor(img, img, cv::COLOR_GRAY2BGR); + } + char buf[64] = {0}; + int stereo_num = 0; + for (size_t j = 0; j < cur_pts.size(); j++) { + cv::Scalar color = cv::Scalar(0, 140, 255); + if (frame.landmarks[j].type == SuperPointLandmark) { + color = cv::Scalar(255, 0, 0); // Superpoint blue } - //Draw predictions - if (landmark_predictions_viz.find(frame.camera_id) != landmark_predictions_viz.end()) { - //Draw predictions here - auto & predictions = landmark_predictions_viz.at(frame.camera_id); - auto & prev = landmark_predictions_matched_viz.at(frame.camera_id); - for (int i = 0; i < predictions.size(); i++) { - cv::circle(img, predictions[i], 3, cv::Scalar(0, 255, 0), 2); - cv::line(img, prev[i], predictions[i], cv::Scalar(0, 0, 255), 1, 8, 0); - // if (cv::norm(prev[i] - predictions[i]) > 20) { - // //Show the landmark id and flag - // auto lm_id = frame.landmarks[i].landmark_id; - // if (lmanager->hasLandmark(lm_id)) { - // printf("Landmark %ld: flag %d %.2f %.2f err %.2f\n", frame.landmarks[i].landmark_id, - // lmanager->at(lm_id).flag, prev[i].x, prev[i].y, cv::norm(prev[i] - predictions[i])); - // } - // } + cv::circle(img, cur_pts[j], 2, color, 2); + auto _id = frame.landmarks[j].landmark_id; + if (!lmanager->hasLandmark(_id)) { + continue; + } + auto lm = lmanager->at(_id); + if (_id >= 0) { + cv::Point2f prev; + bool prev_found = false; + if (!lmanager->hasLandmark(_id)) { + continue; + } + auto &pts2d = lmanager->at(_id).track; + if (pts2d.size() == 0) continue; + if (is_remote) { + prev = pts2d.back().pt2d; + prev_found = true; + } else { + for (int index = pts2d.size() - 1; index >= 0; index--) { + if (!is_right && pts2d[index].camera_id == frame.camera_id && + pts2d[index].frame_id == last_keyframe) { + prev = lmanager->at(_id).track[index].pt2d; + prev_found = true; + break; + } + + if (is_right && pts2d[index].frame_id == frame.frame_id && + pts2d[index].camera_id != frame.camera_id) { + prev = lmanager->at(_id).track[index].pt2d; + prev_found = true; + break; + } } + } + if (!prev_found) { + continue; + } + if (is_remote) { + // Random color + // cv::Scalar color(rand()%255, rand()%255, rand()%255); + // cv::line(img, prev + cv::Point2f(width, 0), cur_pts[j], color); + } else { + cv::arrowedLine(img, prev, cur_pts[j], cv::Scalar(0, 255, 0), 1, 8, 0, + 0.2); + } } - cv::Scalar color = cv::Scalar(255, 0, 0); - if (is_keyframe) { - color = cv::Scalar(0, 0, 255); + if (frame.landmarks[j].landmark_id >= 0) { + stereo_num++; } - if (is_right) { - sprintf(buf, "Stereo points: %d", stereo_num); - cv::putText(img, buf, cv::Point2f(20, 20), cv::FONT_HERSHEY_SIMPLEX, 0.6, color, 2); - } else if (is_remote) { - sprintf(buf, "Drone %d<->%d Matched points: %d", params->self_id, frame.drone_id, report.remote_matched_num); - cv::putText(img, buf, cv::Point2f(20, 20), cv::FONT_HERSHEY_SIMPLEX, 0.6, color, 2); + if (_config.show_feature_id && frame.landmarks[j].landmark_id >= 0) { + sprintf(buf, "%d", frame.landmarks[j].landmark_id % MAX_FEATURE_NUM); + cv::putText(img, buf, cur_pts[j] - cv::Point2f(5, 0), + cv::FONT_HERSHEY_SIMPLEX, 1, color, 1); } - else { - sprintf(buf, "KF/FRAME %d/%d @CAM %d ISKF: %d", keyframe_count, frame_count, - frame.camera_index, is_keyframe); - cv::putText(img, buf, cv::Point2f(20, 20), cv::FONT_HERSHEY_SIMPLEX, 0.6, color, 2); - sprintf(buf, "TRACK %.1fms NUM %d LONG %d Parallex %.1f\%/%.1f", - report.ft_time, report.parallex_num, report.long_track_num, report.meanParallex()*100, _config.parallex_thres*100); - cv::putText(img, buf, cv::Point2f(20, 40), cv::FONT_HERSHEY_SIMPLEX, 0.6, color, 2); + } + // Draw predictions + if (landmark_predictions_viz.find(frame.camera_id) != + landmark_predictions_viz.end()) { + // Draw predictions here + auto &predictions = landmark_predictions_viz.at(frame.camera_id); + auto &prev = landmark_predictions_matched_viz.at(frame.camera_id); + for (int i = 0; i < predictions.size(); i++) { + cv::circle(img, predictions[i], 3, cv::Scalar(0, 255, 0), 2); + cv::line(img, prev[i], predictions[i], cv::Scalar(0, 0, 255), 1, 8, 0); + // if (cv::norm(prev[i] - predictions[i]) > 20) { + // //Show the landmark id and flag + // auto lm_id = frame.landmarks[i].landmark_id; + // if (lmanager->hasLandmark(lm_id)) { + // printf("Landmark %ld: flag %d %.2f %.2f err %.2f\n", + // frame.landmarks[i].landmark_id, + // lmanager->at(lm_id).flag, prev[i].x, prev[i].y, + // cv::norm(prev[i] - predictions[i])); + // } + // } } - return img; + } + cv::Scalar color = cv::Scalar(255, 0, 0); + if (is_keyframe) { + color = cv::Scalar(0, 0, 255); + } + if (is_right) { + sprintf(buf, "Stereo points: %d", stereo_num); + cv::putText(img, buf, cv::Point2f(20, 20), cv::FONT_HERSHEY_SIMPLEX, 0.6, + color, 2); + } else if (is_remote) { + sprintf(buf, "Drone %d<->%d Matched points: %d", params->self_id, + frame.drone_id, report.remote_matched_num); + cv::putText(img, buf, cv::Point2f(20, 20), cv::FONT_HERSHEY_SIMPLEX, 0.6, + color, 2); + } else { + sprintf(buf, "KF/FRAME %d/%d @CAM %d ISKF: %d", keyframe_count, frame_count, + frame.camera_index, is_keyframe); + cv::putText(img, buf, cv::Point2f(20, 20), cv::FONT_HERSHEY_SIMPLEX, 0.6, + color, 2); + sprintf(buf, "TRACK %.1fms NUM %d LONG %d Parallex %.1f\%/%.1f", + report.ft_time, report.parallex_num, report.long_track_num, + report.meanParallex() * 100, _config.parallex_thres * 100); + cv::putText(img, buf, cv::Point2f(20, 40), cv::FONT_HERSHEY_SIMPLEX, 0.6, + color, 2); + } + return img; } -void D2FeatureTracker::drawRemote(const VisualImageDescArray & frames, const TrackReport & report) const { - cv::Mat img = drawToImage(frames.images[0], false, report, false, true); - cv::Mat img_r = drawToImage(frames.images[1], false, report, true, true); - if (img.empty()) { - printf("[D2FeatureTracker::drawRemote] Unable to draw remote image, empty image found\n"); - return; - } - char buf[64] = {0}; - sprintf(buf, "RemoteMatched @ Drone %d", params->self_id); - cv::hconcat(img, img_r, img); - cv::imshow(buf, img); - cv::waitKey(1); - if (_config.write_to_file) { - sprintf(buf, "%s/featureTracker_remote%06d.jpg", _config.output_folder.c_str(), frame_count); - cv::imwrite(buf, img); - } +void D2FeatureTracker::drawRemote(const VisualImageDescArray &frames, + const TrackReport &report) const { + cv::Mat img = drawToImage(frames.images[0], false, report, false, true); + cv::Mat img_r = drawToImage(frames.images[1], false, report, true, true); + if (img.empty()) { + printf( + "[D2FeatureTracker::drawRemote] Unable to draw remote image, empty " + "image found\n"); + return; + } + char buf[64] = {0}; + sprintf(buf, "RemoteMatched @ Drone %d", params->self_id); + cv::hconcat(img, img_r, img); + cv::imshow(buf, img); + cv::waitKey(1); + if (_config.write_to_file) { + sprintf(buf, "%s/featureTracker_remote%06d.jpg", + _config.output_folder.c_str(), frame_count); + cv::imwrite(buf, img); + } } -void D2FeatureTracker::draw(const VisualImageDesc & frame, bool is_keyframe, const TrackReport & report) const { - cv::Mat img = drawToImage(frame, is_keyframe, report); - char buf[64] = {0}; - sprintf(buf, "featureTracker @ Drone %d", params->self_id); - cv::imshow(buf, img); - cv::waitKey(1); - if (_config.write_to_file) { - sprintf(buf, "%s/featureTracker%06d.jpg", _config.output_folder.c_str(), frame_count); - cv::imwrite(buf, img); - } +void D2FeatureTracker::draw(const VisualImageDesc &frame, bool is_keyframe, + const TrackReport &report) const { + cv::Mat img = drawToImage(frame, is_keyframe, report); + char buf[64] = {0}; + sprintf(buf, "featureTracker @ Drone %d", params->self_id); + cv::imshow(buf, img); + cv::waitKey(1); + if (_config.write_to_file) { + sprintf(buf, "%s/featureTracker%06d.jpg", _config.output_folder.c_str(), + frame_count); + cv::imwrite(buf, img); + } } -void D2FeatureTracker::draw(const VisualImageDesc & lframe, VisualImageDesc & rframe, bool is_keyframe, const TrackReport & report) const { - cv::Mat img = drawToImage(lframe, is_keyframe, report); - cv::Mat img_r = drawToImage(rframe, is_keyframe, report, true); - cv::hconcat(img, img_r, img); - char buf[64] = {0}; - sprintf(buf, "featureTracker @ Drone %d", params->self_id); - cv::imshow(buf, img); - cv::waitKey(1); - if (_config.write_to_file) { - sprintf(buf, "%s/featureTracker%06d.jpg", _config.output_folder.c_str(), frame_count); - cv::imwrite(buf, img); - } +void D2FeatureTracker::draw(const VisualImageDesc &lframe, + VisualImageDesc &rframe, bool is_keyframe, + const TrackReport &report) const { + cv::Mat img = drawToImage(lframe, is_keyframe, report); + cv::Mat img_r = drawToImage(rframe, is_keyframe, report, true); + cv::hconcat(img, img_r, img); + char buf[64] = {0}; + sprintf(buf, "featureTracker @ Drone %d", params->self_id); + cv::imshow(buf, img); + cv::waitKey(1); + if (_config.write_to_file) { + sprintf(buf, "%s/featureTracker%06d.jpg", _config.output_folder.c_str(), + frame_count); + cv::imwrite(buf, img); + } } -void D2FeatureTracker::draw(const VisualImageDescArray & frames, bool is_keyframe, const TrackReport & report) const { - cv::Mat img = drawToImage(frames.images[0], is_keyframe, report); - cv::Mat img_r = drawToImage(frames.images[2], is_keyframe, report); - cv::hconcat(img, img_r, img); - cv::Mat img1 = drawToImage(frames.images[1], is_keyframe, report); - cv::Mat img1_r = drawToImage(frames.images[3], is_keyframe, report); - cv::hconcat(img1, img1_r, img1); - cv::vconcat(img, img1, img); - - char buf[64] = {0}; - sprintf(buf, "featureTracker @ Drone %d", params->self_id); - cv::imshow(buf, img); - cv::waitKey(1); - if (_config.write_to_file) { - sprintf(buf, "%s/featureTracker%06d.jpg", _config.output_folder.c_str(), frame_count); - cv::imwrite(buf, img); - } +void D2FeatureTracker::draw(const VisualImageDescArray &frames, + bool is_keyframe, const TrackReport &report) const { + cv::Mat img = drawToImage(frames.images[0], is_keyframe, report); + cv::Mat img_r = drawToImage(frames.images[2], is_keyframe, report); + cv::hconcat(img, img_r, img); + cv::Mat img1 = drawToImage(frames.images[1], is_keyframe, report); + cv::Mat img1_r = drawToImage(frames.images[3], is_keyframe, report); + cv::hconcat(img1, img1_r, img1); + cv::vconcat(img, img1, img); + + char buf[64] = {0}; + sprintf(buf, "featureTracker @ Drone %d", params->self_id); + cv::imshow(buf, img); + cv::waitKey(1); + if (_config.write_to_file) { + sprintf(buf, "%s/featureTracker%06d.jpg", _config.output_folder.c_str(), + frame_count); + cv::imwrite(buf, img); + } } -std::pair, std::vector> getFeatureHalfImg(const std::vector & pts, const std::vector & desc, bool require_left, - std::map & tmp_to_idx) { - std::vector desc_half; - std::vector pts_new; - desc_half.resize(desc.size()); - int c = 0; - float move_cols = params->width_undistort *90.0/params->undistort_fov; //slightly lower than 0.5 cols when fov=200 - for (int i = 0; i < pts.size(); i++) { - if (require_left && pts[i].x < params->width_undistort - move_cols || !require_left && pts[i].x >= move_cols) { - tmp_to_idx[c] = i; - //Copy from desc to desc_half - std::copy(desc.begin() + i*params->superpoint_dims, desc.begin() + (i+1)*params->superpoint_dims, desc_half.begin() + c*params->superpoint_dims); - pts_new.push_back(pts[i]); - c += 1; - } +std::pair, std::vector> getFeatureHalfImg( + const std::vector &pts, const std::vector &desc, + bool require_left, std::map &tmp_to_idx) { + std::vector desc_half; + std::vector pts_new; + desc_half.resize(desc.size()); + int c = 0; + float move_cols = + params->width_undistort * 90.0 / + params->undistort_fov; // slightly lower than 0.5 cols when fov=200 + for (int i = 0; i < pts.size(); i++) { + if (require_left && pts[i].x < params->width_undistort - move_cols || + !require_left && pts[i].x >= move_cols) { + tmp_to_idx[c] = i; + // Copy from desc to desc_half + std::copy(desc.begin() + i * params->superpoint_dims, + desc.begin() + (i + 1) * params->superpoint_dims, + desc_half.begin() + c * params->superpoint_dims); + pts_new.push_back(pts[i]); + c += 1; } - desc_half.resize(c*params->superpoint_dims); - return std::make_pair(desc_half, pts_new); + } + desc_half.resize(c * params->superpoint_dims); + return std::make_pair(desc_half, pts_new); } -bool D2FeatureTracker::matchLocalFeatures(const VisualImageDesc & img_desc_a, const VisualImageDesc & img_desc_b, std::vector & ids_b_to_a, - const D2FeatureTracker::MatchLocalFeatureParams & param) { - TicToc tic; - auto & raw_desc_a = img_desc_a.landmark_descriptor; - auto & raw_desc_b = img_desc_b.landmark_descriptor; - auto pts_a = img_desc_a.landmarks2D(true); - auto pts_b = img_desc_b.landmarks2D(true); - auto pts_a_normed = img_desc_a.landmarks2D(true, true); - auto pts_b_normed = img_desc_b.landmarks2D(true, true); - std::vector ids_a, ids_b; - std::vector _matches; - std::vector pts_pred_a_on_b; - ids_b_to_a.resize(pts_b.size()); - std::fill(ids_b_to_a.begin(), ids_b_to_a.end(), -1); - double search_radius = param.search_radius; - // printf("[D2FT] Frame_a %ld<->%ld enable_prediction %d pose_a %s motion_prediction %s\n", - // img_desc_a.frame_id, img_desc_b.frame_id, param.enable_prediction, param.pose_a.toStr().c_str(), param.pose_b_prediction.toStr().c_str()); - if (param.enable_prediction) { - if (param.prediction_using_extrinsic) { - pts_pred_a_on_b = predictLandmarks(img_desc_a, img_desc_a.extrinsic, img_desc_b.extrinsic, true); +bool D2FeatureTracker::matchLocalFeatures( + const VisualImageDesc &img_desc_a, const VisualImageDesc &img_desc_b, + std::vector &ids_b_to_a, + const D2FeatureTracker::MatchLocalFeatureParams ¶m) { + TicToc tic; + auto &raw_desc_a = img_desc_a.landmark_descriptor; + auto &raw_desc_b = img_desc_b.landmark_descriptor; + auto pts_a = img_desc_a.landmarks2D(true); + auto pts_b = img_desc_b.landmarks2D(true); + auto pts_a_normed = img_desc_a.landmarks2D(true, true); + auto pts_b_normed = img_desc_b.landmarks2D(true, true); + std::vector ids_a, ids_b; + std::vector _matches; + std::vector pts_pred_a_on_b; + ids_b_to_a.resize(pts_b.size()); + std::fill(ids_b_to_a.begin(), ids_b_to_a.end(), -1); + double search_radius = param.search_radius; + spdlog::debug( + "Match {}<->{} enable_prediction {} pose_a {} enable_search_in_local {} " + "motion_prediction {} prediction_using_extrinsic {}", + img_desc_a.frame_id, img_desc_b.frame_id, param.enable_prediction, + param.pose_a.toStr(), param.enable_search_in_local, + param.pose_b_prediction.toStr(), param.prediction_using_extrinsic); + if (param.enable_prediction) { + if (param.prediction_using_extrinsic) { + pts_pred_a_on_b = predictLandmarks(img_desc_a, img_desc_a.extrinsic, + img_desc_b.extrinsic, true); + } else { + pts_pred_a_on_b = + predictLandmarks(img_desc_a, param.pose_a * img_desc_a.extrinsic, + param.pose_b_prediction * img_desc_b.extrinsic); + } + + } else { + pts_pred_a_on_b = pts_a; + if (!param.enable_search_in_local) { + search_radius = -1; + } + } + if (param.enable_superglue) { + // Superglue only support whole image matching + auto &scores0 = img_desc_a.landmark_scores; + auto &scores1 = img_desc_b.landmark_scores; + _matches = superglue->inference(pts_a, pts_b, raw_desc_a, raw_desc_b, + scores0, scores1); + } else { + if (param.type == WHOLE_IMG_MATCH) { + const cv::Mat desc_a(raw_desc_a.size() / params->superpoint_dims, + params->superpoint_dims, CV_32F, + const_cast(raw_desc_a.data())); + const cv::Mat desc_b(raw_desc_b.size() / params->superpoint_dims, + params->superpoint_dims, CV_32F, + const_cast(raw_desc_b.data())); + if (_config.enable_knn_match) { + if (img_desc_a.drone_id == img_desc_b.drone_id && + img_desc_a.camera_id == img_desc_b.camera_id) { + // Is continuous frame + _matches = matchKNN(desc_a, desc_b, _config.knn_match_ratio, + pts_pred_a_on_b, pts_b, search_radius); } else { - pts_pred_a_on_b = predictLandmarks(img_desc_a, param.pose_a * img_desc_a.extrinsic, - param.pose_b_prediction*img_desc_b.extrinsic); + _matches = matchKNN(desc_a, desc_b, _config.knn_match_ratio, + pts_pred_a_on_b, pts_b, search_radius); } - + } else { + cv::BFMatcher bfmatcher(cv::NORM_L2, true); + bfmatcher.match(desc_a, desc_b, _matches); // Query train result + } } else { - pts_pred_a_on_b = pts_a; - if (!param.enable_search_in_local) { - search_radius = -1; + // TODO: motion prediction for quadcam on stereo + std::map tmp_to_idx_a, tmp_to_idx_b; + auto features_a = getFeatureHalfImg( + pts_a, raw_desc_a, param.type == LEFT_RIGHT_IMG_MATCH, tmp_to_idx_a); + auto features_b = getFeatureHalfImg( + pts_b, raw_desc_b, param.type == RIGHT_LEFT_IMG_MATCH, tmp_to_idx_b); + if (tmp_to_idx_a.size() == 0 || tmp_to_idx_b.size() == 0) { + spdlog::debug("matchLocalFeatures failed: no feature to match.\n"); + return false; + } + cv::BFMatcher bfmatcher(cv::NORM_L2, true); + const cv::Mat desc_a(tmp_to_idx_a.size(), params->superpoint_dims, CV_32F, + const_cast(features_a.first.data())); + const cv::Mat desc_b(tmp_to_idx_b.size(), params->superpoint_dims, CV_32F, + const_cast(features_b.first.data())); + if (_config.enable_knn_match) { + if (param.enable_search_in_local) { + float move_cols = + params->width_undistort * 90.0 / + params + ->undistort_fov; // slightly lower than 0.5 cols when fov=200 + for (int i = 0; i < features_a.second.size(); i++) { + features_a.second[i].x += + param.type == LEFT_RIGHT_IMG_MATCH ? move_cols : -move_cols; + } } + _matches = + matchKNN(desc_a, desc_b, _config.knn_match_ratio, features_a.second, + features_b.second, search_radius); + } else { + cv::BFMatcher bfmatcher(cv::NORM_L2, true); + bfmatcher.match(desc_a, desc_b, _matches); + } + for (auto &match : _matches) { + match.queryIdx = tmp_to_idx_a[match.queryIdx]; + match.trainIdx = tmp_to_idx_b[match.trainIdx]; + } } - if (param.enable_superglue) { - //Superglue only support whole image matching - auto & scores0 = img_desc_a.landmark_scores; - auto & scores1 = img_desc_b.landmark_scores; - _matches = superglue->inference(pts_a, pts_b, raw_desc_a, raw_desc_b, scores0, scores1); - } else { - if (param.type == WHOLE_IMG_MATCH) { - const cv::Mat desc_a(raw_desc_a.size()/params->superpoint_dims, params->superpoint_dims, CV_32F, const_cast(raw_desc_a.data())); - const cv::Mat desc_b(raw_desc_b.size()/params->superpoint_dims, params->superpoint_dims, CV_32F, const_cast(raw_desc_b.data())); - if (_config.enable_knn_match) { - if (img_desc_a.drone_id == img_desc_b.drone_id && img_desc_a.camera_id == img_desc_b.camera_id) { - // Is continuous frame - _matches = matchKNN(desc_a, desc_b, _config.knn_match_ratio, pts_pred_a_on_b, pts_b, search_radius); - } else { - _matches = matchKNN(desc_a, desc_b, _config.knn_match_ratio, pts_pred_a_on_b, pts_b, search_radius); - } - } else { - cv::BFMatcher bfmatcher(cv::NORM_L2, true); - bfmatcher.match(desc_a, desc_b, _matches); //Query train result - } - } else { - //TODO: motion prediction for quadcam on stereo - std::map tmp_to_idx_a, tmp_to_idx_b; - auto features_a = getFeatureHalfImg(pts_a, raw_desc_a, param.type==LEFT_RIGHT_IMG_MATCH, tmp_to_idx_a); - auto features_b = getFeatureHalfImg(pts_b, raw_desc_b, param.type==RIGHT_LEFT_IMG_MATCH, tmp_to_idx_b); - if (tmp_to_idx_a.size() == 0 || tmp_to_idx_b.size() == 0) { - if (params->verbose) - printf("[D2FeatureTracker] matchLocalFeatures failed: no feature to match.\n"); - return false; - } - cv::BFMatcher bfmatcher(cv::NORM_L2, true); - const cv::Mat desc_a(tmp_to_idx_a.size(), params->superpoint_dims, CV_32F, const_cast(features_a.first.data())); - const cv::Mat desc_b(tmp_to_idx_b.size(), params->superpoint_dims, CV_32F, const_cast(features_b.first.data())); - if (_config.enable_knn_match) { - if (_config.enable_search_local_aera) { - float move_cols = params->width_undistort *90.0/params->undistort_fov; //slightly lower than 0.5 cols when fov=200 - for (int i = 0; i < features_a.second.size(); i++) { - features_a.second[i].x += param.type==LEFT_RIGHT_IMG_MATCH ? move_cols : -move_cols; - } - } - _matches = matchKNN(desc_a, desc_b, _config.knn_match_ratio, features_a.second, features_b.second, search_radius); - } else { - cv::BFMatcher bfmatcher(cv::NORM_L2, true); - bfmatcher.match(desc_a, desc_b, _matches); - } - for (auto & match : _matches) { - match.queryIdx = tmp_to_idx_a[match.queryIdx]; - match.trainIdx = tmp_to_idx_b[match.trainIdx]; - } - } + } + std::vector matched_pts_a_normed, matched_pts_b_normed, + matched_pts_a, matched_pts_b; + if (params->show) { + landmark_predictions_viz[img_desc_b.camera_id] = std::vector(); + landmark_predictions_matched_viz[img_desc_b.camera_id] = + std::vector(); + } + for (auto match : _matches) { + ids_a.push_back(match.queryIdx); + ids_b.push_back(match.trainIdx); + matched_pts_a_normed.push_back(pts_a_normed[match.queryIdx]); + matched_pts_b_normed.push_back(pts_b_normed[match.trainIdx]); + matched_pts_a.push_back(pts_a[match.queryIdx]); + matched_pts_b.push_back(pts_b[match.trainIdx]); + if (params->show && param.enable_prediction) { + landmark_predictions_viz[img_desc_b.camera_id].push_back( + pts_pred_a_on_b[match.queryIdx]); + landmark_predictions_matched_viz[img_desc_b.camera_id].push_back( + pts_b[match.trainIdx]); + // printf("Point %d: (%f, %f) -> (%f, %f)\n", match.queryIdx, + // pts_pred_a_on_b[match.queryIdx].x, + // pts_pred_a_on_b[match.queryIdx].y, pts_b[match.trainIdx].x, + // pts_b[match.trainIdx].y); } - std::vector matched_pts_a_normed, matched_pts_b_normed, matched_pts_a, matched_pts_b; - if (params->show) { - landmark_predictions_viz[img_desc_b.camera_id] = std::vector(); - landmark_predictions_matched_viz[img_desc_b.camera_id] = std::vector(); - } - for (auto match : _matches) { - ids_a.push_back(match.queryIdx); - ids_b.push_back(match.trainIdx); - matched_pts_a_normed.push_back(pts_a_normed[match.queryIdx]); - matched_pts_b_normed.push_back(pts_b_normed[match.trainIdx]); - matched_pts_a.push_back(pts_a[match.queryIdx]); - matched_pts_b.push_back(pts_b[match.trainIdx]); - if (params->show && param.enable_prediction) { - landmark_predictions_viz[img_desc_b.camera_id].push_back(pts_pred_a_on_b[match.queryIdx]); - landmark_predictions_matched_viz[img_desc_b.camera_id].push_back(pts_b[match.trainIdx]); - // printf("Point %d: (%f, %f) -> (%f, %f)\n", match.queryIdx, - // pts_pred_a_on_b[match.queryIdx].x, pts_pred_a_on_b[match.queryIdx].y, pts_b[match.trainIdx].x, pts_b[match.trainIdx].y); - } + } + if (img_desc_a.drone_id != img_desc_b.drone_id && _config.check_essential && + !param.enable_superglue) { + // only perform this for remote + std::vector mask; + if (matched_pts_a_normed.size() < MIN_HOMOGRAPHY) { + spdlog::debug( + "matchLocalFeatures failed only %ld pts not meet MIN_HOMOGRAPHY", + matched_pts_a_normed.size()); + return false; } - if (img_desc_a.drone_id != img_desc_b.drone_id && - params->ftconfig->check_essential && !param.enable_superglue) { - //only perform this for remote - std::vector mask; - if (matched_pts_a_normed.size() < MIN_HOMOGRAPHY) { - if (params->verbose) - printf("[D2FeatureTracker] matchLocalFeatures failed only %ld pts not meet MIN_HOMOGRAPHY\n", matched_pts_a_normed.size()); - return false; - } - // cv::findHomography(matched_pts_a, matched_pts_b, cv::RANSAC, params->ftconfig->ransacReprojThreshold, mask); - // Find essential matrix with normalized points - cv::Mat K = (cv::Mat_(3,3) << 1.0, 0, 0.0, 0, 1.0, 0.0, 0, 0, 1); - cv::findEssentialMat(matched_pts_a_normed, matched_pts_b_normed, K, cv::RANSAC, 0.999, params->ftconfig->ransacReprojThreshold/params->focal_length, mask); - reduceVector(ids_a, mask); - reduceVector(ids_b, mask); - reduceVector(matched_pts_a, mask); - reduceVector(matched_pts_b, mask); - } - for (auto i = 0; i < ids_a.size(); i++) { - if (ids_a[i] >= pts_a.size()) { - printf("ids_a[i] > pts_a.size() why is this case?\n"); - continue; - } - ids_b_to_a[ids_b[i]] = ids_a[i]; - } - - // //Plot matches - if (param.plot) { - char name[100]; - std::vector kps_a, kps_b; - //Kps from points - for (int i = 0; i < pts_a.size(); i++) { - kps_a.push_back(cv::KeyPoint(pts_a[i].x, pts_a[i].y, 1)); - } - for (int i = 0; i < pts_b.size(); i++) { - kps_b.push_back(cv::KeyPoint(pts_b[i].x, pts_b[i].y, 1)); - } - cv::Mat show; - cv::Mat image_b = img_desc_b.raw_image; - if (image_b.empty()) { - cv::imdecode(img_desc_b.image, cv::IMREAD_GRAYSCALE, &image_b); - } - cv::drawMatches(img_desc_a.raw_image, kps_a, image_b, kps_b, _matches, show); - sprintf(name, "Matched points: %d", _matches.size()); - cv::putText(show, name, cv::Point2f(20, 20), cv::FONT_HERSHEY_SIMPLEX, 0.8, cv::Scalar(0, 255, 0), 2); - sprintf(name, "matches_CAM@Drone %d@%d_%d@%d", img_desc_a.camera_index, - img_desc_a.drone_id, img_desc_b.camera_index, img_desc_b.drone_id); - if (params->ftconfig->check_essential) { - cv::Mat show_check; - cv::hconcat(img_desc_a.raw_image, image_b, show_check); - cv::cvtColor(show_check, show_check, cv::COLOR_GRAY2BGR); - for (int i = 0; i < matched_pts_a.size(); i++) { - // random color - cv::Scalar color(rand()&255, rand()&255, rand()&255); - cv::line(show_check, matched_pts_a[i], matched_pts_b[i] + cv::Point2f(show.cols/2, 0), color, 1); - } - sprintf(name, "filtered_matches_CAM@Drone %d@%d_%d@%d", img_desc_a.camera_index, - img_desc_a.drone_id, img_desc_b.camera_index, img_desc_b.drone_id); - cv::vconcat(show, show_check, show); - cv::imshow(name, show); - } else { - cv::imshow(name, show); - } + // cv::findHomography(matched_pts_a, matched_pts_b, cv::RANSAC, + // _config.ransacReprojThreshold, mask); Find essential matrix with + // normalized points + cv::Mat K = (cv::Mat_(3, 3) << 1.0, 0, 0.0, 0, 1.0, 0.0, 0, 0, 1); + cv::findEssentialMat( + matched_pts_a_normed, matched_pts_b_normed, K, cv::RANSAC, 0.999, + _config.ransacReprojThreshold / params->focal_length, mask); + reduceVector(ids_a, mask); + reduceVector(ids_b, mask); + reduceVector(matched_pts_a, mask); + reduceVector(matched_pts_b, mask); + } + for (auto i = 0; i < ids_a.size(); i++) { + if (ids_a[i] >= pts_a.size()) { + SPDLOG_ERROR("ids_a[i] > pts_a.size() why is this case?"); + continue; } + ids_b_to_a[ids_b[i]] = ids_a[i]; + } - if (params->verbose || params->enable_perf_output) - printf("[D2FeatureTracker::matchLocalFeatures] match features %d:%d matched %ld frame %d:%d t: %.3f ms enable_knn %d search_dist %.2f check_essential %d sp_dims %d\n", - pts_a.size(), pts_b.size(), ids_b.size(), img_desc_a.frame_id, img_desc_b.frame_id, tic.toc(), - params->ftconfig->enable_knn_match, _config.search_local_max_dist*image_width, params->ftconfig->check_essential, params->superpoint_dims); - if (ids_b.size() >= params->ftconfig->remote_min_match_num) { - return true; + // //Plot matches + if (param.plot) { + char name[100]; + std::vector kps_a, kps_b; + // Kps from points + for (int i = 0; i < pts_a.size(); i++) { + kps_a.push_back(cv::KeyPoint(pts_a[i].x, pts_a[i].y, 1)); } - return false; -} + for (int i = 0; i < pts_b.size(); i++) { + kps_b.push_back(cv::KeyPoint(pts_b[i].x, pts_b[i].y, 1)); + } + cv::Mat show; + cv::Mat image_b = img_desc_b.raw_image; + if (image_b.empty()) { + cv::imdecode(img_desc_b.image, cv::IMREAD_GRAYSCALE, &image_b); + } + cv::drawMatches(img_desc_a.raw_image, kps_a, image_b, kps_b, _matches, + show); + sprintf(name, "Matched points: %d", _matches.size()); + cv::putText(show, name, cv::Point2f(20, 20), cv::FONT_HERSHEY_SIMPLEX, 0.8, + cv::Scalar(0, 255, 0), 2); + sprintf(name, "matches_CAM@Drone %d@%d_%d@%d", img_desc_a.camera_index, + img_desc_a.drone_id, img_desc_b.camera_index, img_desc_b.drone_id); + if (_config.check_essential) { + cv::Mat show_check; + cv::hconcat(img_desc_a.raw_image, image_b, show_check); + cv::cvtColor(show_check, show_check, cv::COLOR_GRAY2BGR); + for (int i = 0; i < matched_pts_a.size(); i++) { + // random color + cv::Scalar color(rand() & 255, rand() & 255, rand() & 255); + cv::line(show_check, matched_pts_a[i], + matched_pts_b[i] + cv::Point2f(show.cols / 2, 0), color, 1); + } + sprintf(name, "filtered_matches_CAM@Drone %d@%d_%d@%d", + img_desc_a.camera_index, img_desc_a.drone_id, + img_desc_b.camera_index, img_desc_b.drone_id); + cv::vconcat(show, show_check, show); + cv::imshow(name, show); + } else { + cv::imshow(name, show); + } + } + spdlog::debug( + "match features {}:{} matched inliers{}/all{} frame {}:{} t: {:.3f}ms " + "enable_knn {} kNN ratio {} search_dist {:.2f} check_essential {} " + "sp_dims {}", + pts_a.size(), pts_b.size(), ids_b.size(), _matches.size(), + img_desc_a.frame_id, img_desc_b.frame_id, tic.toc(), + _config.enable_knn_match, _config.knn_match_ratio, search_radius, + _config.check_essential, params->superpoint_dims); + if (ids_b.size() >= _config.remote_min_match_num) { + return true; + } + return false; +} -std::vector D2FeatureTracker::predictLandmarks(const VisualImageDesc & img_desc_a, const Swarm::Pose & cam_pose_a, - const Swarm::Pose & cam_pose_b, bool use_extrinsic) const { - std::vector pts_a_pred_on_b; - assert(img_desc_a.drone_id == params->self_id); - auto cam = cams.at(img_desc_a.camera_index); - for (int i = 0; i < img_desc_a.spLandmarkNum(); i++) { - auto landmark_id = img_desc_a.landmarks[i].landmark_id; - //Query 3d landmark position - bool find_position = false; - Vector3d pt3d(0., 0., 0.); - if (!use_extrinsic && lmanager->hasLandmark(landmark_id)) { - const auto & lm = lmanager->at(landmark_id); - if (lm.flag == LandmarkFlag::INITIALIZED || lm.flag == LandmarkFlag::ESTIMATED) { - pt3d = lm.position; - find_position = true; - } - } - if (!find_position) { - Vector3d landmark_pos_cam = img_desc_a.landmarks[i].pt3d_norm * _config.landmark_distance_assumption; - pt3d = cam_pose_a * landmark_pos_cam; - } - //Predict 2d position on b - Vector2d pt2d_pred; - Vector3d pos_cam_b_pred = cam_pose_b.inverse() * pt3d; - cam->spaceToPlane(pos_cam_b_pred, pt2d_pred); - pts_a_pred_on_b.emplace_back(pt2d_pred.x(), pt2d_pred.y()); - // printf("[D2FT] Frame_a %ld landmark %ld find_pos:%d position %.2f %.2f %.2f pred %.2f %.2f\n", - // img_desc_a.frame_id, landmark_id, find_position, pt3d.x(), pt3d.y(), pt3d.z(), pt2d_pred.x(), pt2d_pred.y()); - } - return pts_a_pred_on_b; +std::map +D2FeatureTracker::predictLandmarksWithExtrinsic( + int camera_index, std::vector pts_ids, + std::vector pts_3d_norm, const Swarm::Pose &cam_pose_a, + const Swarm::Pose &cam_pose_b) const { + std::map pts_a_pred_on_b; + auto cam = cams.at(camera_index); + for (int i = 0; i < pts_3d_norm.size(); i++) { + Vector3d landmark_pos_cam = + pts_3d_norm[i] * _config.landmark_distance_assumption; + Vector3d pt3d = cam_pose_a * landmark_pos_cam; + Vector2d pt2d_pred; + Vector3d pos_cam_b_pred = cam_pose_b.inverse() * pt3d; + cam->spaceToPlane(pos_cam_b_pred, pt2d_pred); + pts_a_pred_on_b[pts_ids[i]] = {pt2d_pred.x(), pt2d_pred.y()}; + } + return pts_a_pred_on_b; } +std::vector D2FeatureTracker::predictLandmarks( + const VisualImageDesc &img_desc_a, const Swarm::Pose &cam_pose_a, + const Swarm::Pose &cam_pose_b, bool use_extrinsic) const { + std::vector pts_a_pred_on_b; + assert(img_desc_a.drone_id == params->self_id); + auto cam = cams.at(img_desc_a.camera_index); + for (int i = 0; i < img_desc_a.spLandmarkNum(); i++) { + auto landmark_id = img_desc_a.landmarks[i].landmark_id; + // Query 3d landmark position + bool find_position = false; + Vector3d pt3d(0., 0., 0.); + if (!use_extrinsic && lmanager->hasLandmark(landmark_id)) { + const auto &lm = lmanager->at(landmark_id); + if (lm.flag == LandmarkFlag::INITIALIZED || + lm.flag == LandmarkFlag::ESTIMATED) { + pt3d = lm.position; + find_position = true; + } + } + if (!find_position) { + Vector3d landmark_pos_cam = img_desc_a.landmarks[i].pt3d_norm * + _config.landmark_distance_assumption; + pt3d = cam_pose_a * landmark_pos_cam; + } + // Predict 2d position on b + Vector2d pt2d_pred; + Vector3d pos_cam_b_pred = cam_pose_b.inverse() * pt3d; + cam->spaceToPlane(pos_cam_b_pred, pt2d_pred); + pts_a_pred_on_b.emplace_back(pt2d_pred.x(), pt2d_pred.y()); + // printf("[D2FT] Frame_a %ld landmark %ld find_pos:%d position %.2f %.2f + // %.2f pred %.2f %.2f\n", + // img_desc_a.frame_id, landmark_id, find_position, pt3d.x(), pt3d.y(), + // pt3d.z(), pt2d_pred.x(), pt2d_pred.y()); + } + return pts_a_pred_on_b; } + +} // namespace D2FrontEnd diff --git a/d2frontend/src/d2frontend.cpp b/d2frontend/src/d2frontend.cpp index 32b51678..75808ada 100644 --- a/d2frontend/src/d2frontend.cpp +++ b/d2frontend/src/d2frontend.cpp @@ -1,18 +1,23 @@ +#include #include #include -#include -#include "ros/ros.h" -#include -#include "d2frontend/loop_net.h" -#include "d2frontend/loop_cam.h" -#include "d2frontend/loop_detector.h" -#include -#include +#include +#include #include -#include +#include #include +#include + +#include #include -#include +#include +#include +#include + +#include "d2frontend/loop_cam.h" +#include "d2frontend/loop_detector.h" +#include "d2frontend/loop_net.h" +#include "ros/ros.h" // #define BACKWARD_HAS_DW 1 // #include @@ -22,262 +27,373 @@ // } namespace D2FrontEnd { + + typedef std::lock_guard lock_guard; -void D2Frontend::onLoopConnection(LoopEdge & loop_con, bool is_local) { - if(is_local && params->pgo_mode == PGO_MODE::PGO_MODE_NON_DIST) { - //Only PGO is non-distributed we broadcast the loops. - loop_net->broadcastLoopConnection(loop_con); - } +void D2Frontend::onLoopConnection(LoopEdge &loop_con, bool is_local) { + if (is_local && params->pgo_mode == PGO_MODE::PGO_MODE_NON_DIST) { + // Only PGO is non-distributed we broadcast the loops. + loop_net->broadcastLoopConnection(loop_con); + } - // ROS_INFO("Pub loop conn. is local %d", is_local); - loopconn_pub.publish(loop_con); + // ROS_INFO("Pub loop conn. is local %d", is_local); + loopconn_pub.publish(loop_con); } -StereoFrame D2Frontend::findImagesRaw(const nav_msgs::Odometry & odometry) { - // ROS_INFO("findImagesRaw %f", odometry.header.stamp.toSec()); - auto stamp = odometry.header.stamp; - StereoFrame ret; - raw_stereo_image_lock.lock(); - while (raw_stereo_images.size() > 0 && stamp.toSec() - raw_stereo_images.front().stamp.toSec() > 1e-3) { - // ROS_INFO("Removing d stamp %f", stamp.toSec() - raw_stereo_images.front().stamp.toSec()); - raw_stereo_images.pop(); - } - - if (raw_stereo_images.size() > 0 && fabs(stamp.toSec() - raw_stereo_images.front().stamp.toSec()) < 1e-3) { - auto ret = raw_stereo_images.front(); - raw_stereo_images.pop(); - ret.pose_drone = odometry.pose.pose; - // ROS_INFO("VIO KF found, returning..."); - raw_stereo_image_lock.unlock(); - return ret; - } +StereoFrame D2Frontend::findImagesRaw(const nav_msgs::Odometry &odometry) { + // ROS_INFO("findImagesRaw %f", odometry.header.stamp.toSec()); + auto stamp = odometry.header.stamp; + StereoFrame ret; + raw_stereo_image_lock.lock(); + while (raw_stereo_images.size() > 0 && + stamp.toSec() - raw_stereo_images.front().stamp.toSec() > 1e-3) { + // ROS_INFO("Removing d stamp %f", stamp.toSec() - + // raw_stereo_images.front().stamp.toSec()); + raw_stereo_images.pop(); + } + if (raw_stereo_images.size() > 0 && + fabs(stamp.toSec() - raw_stereo_images.front().stamp.toSec()) < 1e-3) { + auto ret = raw_stereo_images.front(); + raw_stereo_images.pop(); + ret.pose_drone = odometry.pose.pose; + // ROS_INFO("VIO KF found, returning..."); raw_stereo_image_lock.unlock(); return ret; + } + + raw_stereo_image_lock.unlock(); + return ret; } -void D2Frontend::stereoImagesCallback(const sensor_msgs::ImageConstPtr left, const sensor_msgs::ImageConstPtr right) { - auto _l = getImageFromMsg(left); - auto _r = getImageFromMsg(right); - StereoFrame sframe(_l->header.stamp, _l->image, _r->image, params->extrinsics[0], params->extrinsics[1], params->self_id); - processStereoframe(sframe); +void D2Frontend::stereoImagesCallback(const sensor_msgs::ImageConstPtr left, + const sensor_msgs::ImageConstPtr right) { + auto _l = getImageFromMsg(left); + auto _r = getImageFromMsg(right); + // StereoFrame sframe(_l->header.stamp, _l->image, _r->image, + // params->extrinsics[0], params->extrinsics[1], + // params->self_id); + if (stereo_frame_buffer_lock_.try_lock()) { + stereo_frame_q_.push(std::make_shared(_l->header.stamp, _l->image, _r->image, + params->extrinsics[0], params->extrinsics[1], + params->self_id)); + while (stereo_frame_q_.size() > visual_frame_size_) { + spdlog::warn("D2VINS frontend process is slow; dropping frames\n"); + stereo_frame_q_.pop(); + } + stereo_frame_buffer_lock_.unlock(); + } + return; + // processStereoframe(sframe); } -void D2Frontend::depthImagesCallback(const sensor_msgs::ImageConstPtr left, const sensor_msgs::ImageConstPtr depth) { - auto _l = getImageFromMsg(left); - auto _d = getImageFromMsg(depth); - StereoFrame sframe(left->header.stamp, _l->image, _d->image, params->extrinsics[0], params->self_id); - processStereoframe(sframe); +void D2Frontend::depthImagesCallback(const sensor_msgs::ImageConstPtr left, + const sensor_msgs::ImageConstPtr depth) { + auto _l = getImageFromMsg(left); + auto _d = getImageFromMsg(depth); + StereoFrame sframe(left->header.stamp, _l->image, _d->image, + params->extrinsics[0], params->self_id); + if (stereo_frame_buffer_lock_.try_lock()) { + stereo_frame_q_.push(std::make_shared(left->header.stamp, _l->image, _d->image, + params->extrinsics[0], params->self_id)); + while (stereo_frame_q_.size() > visual_frame_size_) { + spdlog::warn("D2VINS frontend process is slow; dropping frames\n"); + stereo_frame_q_.pop(); + } + stereo_frame_buffer_lock_.unlock(); + } + return ; + // processStereoframe(sframe); } -void D2Frontend::monoImageCallback(const sensor_msgs::ImageConstPtr & image) { - auto _l = getImageFromMsg(image); - auto img = _l->image; - //Horizon split image to four images: - std::vector imgs; - const int num_imgs = 4; - for (int i = 0; i < 4; i++) { - imgs.emplace_back(img(cv::Rect(i * img.cols /num_imgs, 0, img.cols /num_imgs, img.rows))); - if (imgs.back().channels() == 3) { - cv::cvtColor(imgs.back(), imgs.back(), cv::COLOR_BGR2GRAY); - } +void D2Frontend::monoImageCallback(const sensor_msgs::ImageConstPtr &image) { + auto _l = getImageFromMsg(image); + auto img = _l->image; + // Horizon split image to four images: + std::vector imgs; + const int num_imgs = 4; + for (int i = 0; i < 4; i++) { + imgs.emplace_back(img( + cv::Rect(i * img.cols / num_imgs, 0, img.cols / num_imgs, img.rows))); + if (imgs.back().channels() == 3) { + cv::cvtColor(imgs.back(), imgs.back(), cv::COLOR_BGR2GRAY); } - if (params->show_raw_image) { - cv::namedWindow("raw_image", cv::WINDOW_NORMAL | cv::WINDOW_GUI_EXPANDED); - cv::imshow("RawImage", img); + } + if (params->show_raw_image) { + cv::namedWindow("raw_image", cv::WINDOW_NORMAL | cv::WINDOW_GUI_EXPANDED); + cv::imshow("RawImage", img); + } + // StereoFrame sframe(image->header.stamp, imgs, params->extrinsics, + // params->self_id); + if (stereo_frame_buffer_lock_.try_lock()) { + stereo_frame_q_.push(std::make_shared(image->header.stamp, imgs, params->extrinsics, + params->self_id)); + while (stereo_frame_q_.size() > visual_frame_size_) { + spdlog::warn("D2VINS frontend process is slow; dropping frames\n"); + stereo_frame_q_.pop(); } - StereoFrame sframe(image->header.stamp, imgs, params->extrinsics, params->self_id); - processStereoframe(sframe); + stereo_frame_buffer_lock_.unlock(); + } + return ; + // processStereoframe(sframe); } -void D2Frontend::processStereoframe(const StereoFrame & stereoframe) { - static int count = 0; - // ROS_INFO("[D2Frontend::processStereoframe] %d", count ++); - auto vframearry = loop_cam->processStereoframe(stereoframe); - vframearry.motion_prediction = getMotionPredict(vframearry.stamp); - bool is_keyframe = feature_tracker->trackLocalFrames(vframearry); - vframearry.prevent_adding_db = !is_keyframe; - vframearry.is_keyframe = is_keyframe; - received_image = true; - if (!params->show) { - vframearry.releaseRawImages(); - } - if (vframearry.send_to_backend) { - backendFrameCallback(vframearry); +void D2Frontend::processStereoFrameThread() { + //set thread name + prctl(PR_SET_NAME, "D2FrontendStereoFrame", 0, 0, 0); + while (stereo_frame_thread_running_) { + if (stereo_frame_buffer_lock_.try_lock()) { + if (stereo_frame_q_.size() > 0) { + auto sframe = stereo_frame_q_.front(); + stereo_frame_q_.pop(); + processStereoframe(*sframe); + } + stereo_frame_buffer_lock_.unlock(); } + stereo_frame_thread_rate_ptr_->sleep(); + } } -void D2Frontend::addToLoopQueue(const VisualImageDescArray & viokf) { - if (params->enable_loop) { +void D2Frontend::loopDetectionThread() { + //set thread name + prctl(PR_SET_NAME, "D2FrontendLoopDetection", 0, 0, 0); + while (loop_detection_thread_running_) { + if (loop_queue.size() > 0) { + VisualImageDescArray vframearry; + { //minimal lock scope lock_guard guard(loop_lock); - Utility::TicToc tic; - loop_queue.push(viokf); + vframearry = loop_queue.front(); + loop_queue.pop(); + } + if (loop_queue.size() > 10) { + SPDLOG_WARN("Loop queue size is {}", loop_queue.size()); + } + loop_detector->processImageArray(vframearry); } + loop_detection_thread_rate_ptr_->sleep(); + } +} + +void D2Frontend::lcmThread() { + //set thread name + prctl(PR_SET_NAME, "D2FrontendLCM", 0, 0, 0); + while (lcm_thread_running_) { + loop_net->lcmHandle(); + lcm_thread_rate_ptr_->sleep(); + } +} + +void D2Frontend::processStereoframe(const StereoFrame &stereoframe) { + static int count = 0; + // ROS_INFO("[D2Frontend::processStereoframe] %d", count ++); + auto vframearry = loop_cam->processStereoframe(stereoframe); + vframearry.motion_prediction = getMotionPredict(vframearry.stamp); + bool is_keyframe = feature_tracker->trackLocalFrames(vframearry); + vframearry.prevent_adding_db = !is_keyframe; + vframearry.is_keyframe = is_keyframe; + received_image = true; + if (!params->show) { + vframearry.releaseRawImages(); + } + if (vframearry.send_to_backend) { + backendFrameCallback(vframearry); + } +} + +void D2Frontend::addToLoopQueue(const VisualImageDescArray &viokf) { + if (params->enable_loop) { + lock_guard guard(loop_lock); + Utility::TicToc tic; + loop_queue.push(viokf); + } } void D2Frontend::onRemoteImage(VisualImageDescArray frame_desc) { - if (frame_desc.is_lazy_frame || frame_desc.matched_frame >= 0) { - processRemoteImage(frame_desc, false); - } else { - bool succ = feature_tracker->trackRemoteFrames(frame_desc); - processRemoteImage(frame_desc, succ); + if (frame_desc.is_lazy_frame || frame_desc.matched_frame >= 0) { + processRemoteImage(frame_desc, false); + } else { + bool succ = false; + if (params->estimation_mode != SINGLE_DRONE_MODE) { + succ = feature_tracker->trackRemoteFrames(frame_desc); } + processRemoteImage(frame_desc, succ); + } } -void D2Frontend::processRemoteImage(VisualImageDescArray & frame_desc, bool succ_track) { - if (params->enable_loop) { - if (!frame_desc.isMatchedFrame()) { - if (params->verbose) - printf("[D2Frontend] Remote image %d is not matched, directly pass to detector\n", frame_desc.frame_id); - //Check if keyframe!!! - if (frame_desc.is_keyframe) { - addToLoopQueue(frame_desc); - } - } else { - //We need to wait the matched frame is added to loop detector. - if (loop_detector->hasFrame(frame_desc.matched_frame) || frame_desc.matched_drone != params->self_id) { - if (params->verbose) { - printf("[D2Frontend] Remote image %d is matched with %d drone %d add to loop queue\n", - frame_desc.frame_id, frame_desc.matched_frame, frame_desc.drone_id); +void D2Frontend::processRemoteImage(VisualImageDescArray &frame_desc, + bool succ_track) { + if (params->enable_loop) { + if (!frame_desc.isMatchedFrame()) { + spdlog::debug("Remote image {} is not matched, directly pass to detector", + frame_desc.frame_id); + // Check if keyframe!!! + if (frame_desc.is_keyframe) { + addToLoopQueue(frame_desc); + } + } else { + // We need to wait the matched frame is added to loop detector. + if (loop_detector->hasFrame(frame_desc.matched_frame) || + frame_desc.matched_drone != params->self_id) { + spdlog::debug( + "Remote image {} is matched with {} drone {} add " + "to loop queue", + frame_desc.frame_id, frame_desc.matched_frame, frame_desc.drone_id); + addToLoopQueue(frame_desc); + } else { + VisualImageDescArray _frame_desc = frame_desc; + spdlog::debug( + "Remote image {} is matched with {}, waiting for " + "matched frame", + frame_desc.frame_id, frame_desc.matched_frame); + new std::thread( + [&](VisualImageDescArray frame) { + int count = 0; + while (count < 1000) { + if (loop_detector->hasFrame(frame.matched_frame)) { + SPDLOG_INFO( + "Frame {} waited {} us for matched " + "frame {}", + frame.frame_id, count * 1000, frame.matched_frame); + addToLoopQueue(frame); + break; } - addToLoopQueue(frame_desc); - } else { - VisualImageDescArray _frame_desc = frame_desc; - if (params->verbose) - printf("[D2Frontend] Remote image %d is matched with %d, waiting for matched frame\n", frame_desc.frame_id, frame_desc.matched_frame); - new std::thread([&](VisualImageDescArray frame) { - int count = 0; - while (count < 1000) { - if (loop_detector->hasFrame(frame.matched_frame)) { - printf("[D2Frontend] frame %ld waited %d us for matched frame %d\n", frame.frame_id, count * 1000, - frame.matched_frame); - addToLoopQueue(frame); - break; - } - usleep(1000); - count += 1; - } - }, (_frame_desc)); - } - } + usleep(1000); + count += 1; + } + }, + (_frame_desc)); + } } + } } - -void D2Frontend::loopDetectionThread() { - while (ros::ok()) { - if (loop_queue.size() > 0) { - VisualImageDescArray vframearry; - { - lock_guard guard(loop_lock); - vframearry = loop_queue.front(); - loop_queue.pop(); - } - if (loop_queue.size() > 10) { - ROS_WARN("[D2Frontend] Loop queue size is %d", loop_queue.size()); - } - loop_detector->processImageArray(vframearry); - } - usleep(10000); - } +void D2Frontend::pubNodeFrame(const VisualImageDescArray &viokf) { + auto _kf = viokf.toROS(); + keyframe_pub.publish(_kf); } -void D2Frontend::pubNodeFrame(const VisualImageDescArray & viokf) { - auto _kf = viokf.toROS(); - keyframe_pub.publish(_kf); +void D2Frontend::onRemoteFrameROS( + const swarm_msgs::ImageArrayDescriptor &remote_img_desc) { + // ROS_INFO("Remote"); + if (received_image) { + this->onRemoteImage(remote_img_desc); + } } -void D2Frontend::onRemoteFrameROS(const swarm_msgs::ImageArrayDescriptor & remote_img_desc) { - // ROS_INFO("Remote"); +D2Frontend::D2Frontend() {} + +void D2Frontend::Init(ros::NodeHandle &nh) { + // Init Loop Net + params = new D2FrontendParams(nh); + it_ = new image_transport::ImageTransport(nh); + cv::setNumThreads(1); + + loop_net = + new LoopNet(params->_lcm_uri, params->send_img, + params->send_whole_img_desc, params->recv_msg_duration); + loop_cam = new LoopCam(*(params->loopcamconfig), nh); + feature_tracker = new D2FeatureTracker(*(params->ftconfig)); + feature_tracker->cams = loop_cam->cams; + loop_detector = + new LoopDetector(params->self_id, *(params->loopdetectorconfig)); + loop_detector->loop_cam = loop_cam; + + loop_detector->on_loop_cb = [&](LoopEdge &loop_con) { + this->onLoopConnection(loop_con, true); + }; + + loop_detector->broadcast_keyframe_cb = [&](VisualImageDescArray &viokf) { + loop_net->broadcastVisualImageDescArray(viokf, true); + }; + + loop_net->frame_desc_callback = [&](const VisualImageDescArray &frame_desc) { if (received_image) { - this->onRemoteImage(remote_img_desc); + if (params->enable_pub_remote_frame) { + remote_image_desc_pub.publish(frame_desc.toROS()); + } + this->onRemoteImage(frame_desc); + this->pubNodeFrame(frame_desc); } -} + }; -D2Frontend::D2Frontend () {} - -void D2Frontend::Init(ros::NodeHandle & nh) { - //Init Loop Net - params = new D2FrontendParams(nh); - it_ = new image_transport::ImageTransport(nh); - cv::setNumThreads(1); - - loop_net = new LoopNet(params->_lcm_uri, params->send_img, params->send_whole_img_desc, params->recv_msg_duration); - loop_cam = new LoopCam(*(params->loopcamconfig), nh); - feature_tracker = new D2FeatureTracker(*(params->ftconfig)); - feature_tracker->cams = loop_cam->cams; - loop_detector = new LoopDetector(params->self_id, *(params->loopdetectorconfig)); - loop_detector->loop_cam = loop_cam; - - loop_detector->on_loop_cb = [&] (LoopEdge & loop_con) { - this->onLoopConnection(loop_con, true); - }; - - loop_detector->broadcast_keyframe_cb = [&] (VisualImageDescArray & viokf) { - loop_net->broadcastVisualImageDescArray(viokf, true); - }; - - loop_net->frame_desc_callback = [&] (const VisualImageDescArray & frame_desc) { - if (received_image) { - if (params->enable_pub_remote_frame) { - remote_image_desc_pub.publish(frame_desc.toROS()); - } - this->onRemoteImage(frame_desc); - this->pubNodeFrame(frame_desc); - } - }; - - loop_net->loopconn_callback = [&] (const LoopEdge_t & loop_conn) { - auto loc = toROSLoopEdge(loop_conn); - onLoopConnection(loc, false); - }; - std::string format = "raw"; - if (params->is_comp_images) { - format = "compressed"; - } - image_transport::TransportHints hints(format, ros::TransportHints().tcpNoDelay(true)); - if (params->camera_configuration == CameraConfig::STEREO_PINHOLE || params->camera_configuration == CameraConfig::STEREO_FISHEYE) { - ROS_INFO("[D2Frontend] Input: images %s and %s", params->image_topics[0].c_str(), params->image_topics[1].c_str()); - image_sub_l = new ImageSubscriber(*it_, params->image_topics[0], 1000, hints); - image_sub_r = new ImageSubscriber(*it_, params->image_topics[1], 1000, hints); - sync = new message_filters::TimeSynchronizer (*image_sub_l, *image_sub_r, 1000); - sync->registerCallback(boost::bind(&D2Frontend::stereoImagesCallback, this, _1, _2)); - } else if (params->camera_configuration == CameraConfig::PINHOLE_DEPTH) { - ROS_INFO("[D2Frontend] Input: raw images %s and depth %s", params->image_topics[0].c_str(), params->depth_topics[0].c_str()); - image_sub_l = new ImageSubscriber(*it_, params->image_topics[0], 1000, hints); - image_sub_r = new ImageSubscriber(*it_, params->depth_topics[0], 1000, hints); - sync = new message_filters::TimeSynchronizer (*image_sub_l, *image_sub_r, 1000); - sync->registerCallback(boost::bind(&D2Frontend::depthImagesCallback, this, _1, _2)); - } else if (params->camera_configuration == CameraConfig::FOURCORNER_FISHEYE) { - //Default we accept only horizon-concated image - image_sub_single = it_->subscribe(params->image_topics[0], 1000, &D2Frontend::monoImageCallback, this, hints); - } - - keyframe_pub = nh.advertise("keyframe", 10); - - loopconn_pub = nh.advertise("loop", 10); - - if (params->enable_sub_remote_frame) { - ROS_INFO("[SWARM_LOOP] Subscribing remote image from bag"); - remote_img_sub = nh.subscribe("/swarm_loop/remote_frame_desc", 1, &D2Frontend::onRemoteFrameROS, this, ros::TransportHints().tcpNoDelay()); - } + loop_net->loopconn_callback = [&](const LoopEdge_t &loop_conn) { + auto loc = toROSLoopEdge(loop_conn); + onLoopConnection(loc, false); + }; + std::string format = "raw"; + if (params->is_comp_images) { + format = "compressed"; + } + image_transport::TransportHints hints(format, + ros::TransportHints().tcpNoDelay(true)); - if (params->enable_pub_remote_frame) { - remote_image_desc_pub = nh.advertise("remote_frame_desc", 10); - } + if (params->camera_configuration == CameraConfig::STEREO_PINHOLE || + params->camera_configuration == CameraConfig::STEREO_FISHEYE) { + SPDLOG_INFO("Input: images {} and {}", params->image_topics[0], + params->image_topics[1]); + image_sub_l = + new ImageSubscriber(*it_, params->image_topics[0], 10, hints); + image_sub_r = + new ImageSubscriber(*it_, params->image_topics[1], 10, hints); + sync = new message_filters::Synchronizer( + ApproSync(10), *image_sub_l, *image_sub_r); + sync->registerCallback( + boost::bind(&D2Frontend::stereoImagesCallback, this, _1, _2)); + } else if (params->camera_configuration == CameraConfig::PINHOLE_DEPTH) { + SPDLOG_INFO("Input: raw images {} and depth {}", params->image_topics[0], + params->depth_topics[0]); + image_sub_l = + new ImageSubscriber(*it_, params->image_topics[0], 10, hints); + image_sub_r = + new ImageSubscriber(*it_, params->depth_topics[0], 10, hints); + sync = new message_filters::Synchronizer( + ApproSync(10), *image_sub_l, *image_sub_r); + sync->registerCallback( + boost::bind(&D2Frontend::depthImagesCallback, this, _1, _2)); + } else if (params->camera_configuration == CameraConfig::FOURCORNER_FISHEYE) { + // Default we accept only horizon-concated image + image_sub_single = + it_->subscribe(params->image_topics[0], 10, + &D2Frontend::monoImageCallback, this, hints); + } - timer = nh.createTimer(ros::Duration(0.01), [&](const ros::TimerEvent & e) { - loop_net->scanRecvPackets(); - }); + keyframe_pub = nh.advertise("keyframe", 10); - // loop_timer = nh.createTimer(ros::Duration(0.01), &D2Frontend::loopTimerCallback, this); - th_loop_det = std::thread(&D2Frontend::loopDetectionThread, this); - th = std::thread([&] { - while(0 == loop_net->lcmHandle()) { - } - }); -} + loopconn_pub = nh.advertise("loop", 10); + + if (params->enable_sub_remote_frame) { + SPDLOG_INFO("Subscribing remote image from bag"); + remote_img_sub = nh.subscribe("/swarm_loop/remote_frame_desc", 1, + &D2Frontend::onRemoteFrameROS, this, + ros::TransportHints().tcpNoDelay()); + } + + if (params->enable_pub_remote_frame) { + remote_image_desc_pub = + nh.advertise("remote_frame_desc", 10); + } + + timer = nh.createTimer(ros::Duration(0.01), [&](const ros::TimerEvent &e) { + loop_net->scanRecvPackets(); + }); + + stereo_frame_thread_rate_ptr_ = std::make_unique(params->ftconfig->stereo_frame_thread_rate); + loop_detection_thread_rate_ptr_ = std::make_unique(params->ftconfig->loop_detection_thread_rate); + lcm_thread_rate_ptr_ = std::make_unique(params->ftconfig->lcm_thread_rate); + + //start d2frontend thread + startThread(); + spdlog::info("D2Frontend initialized"); + + // loop_timer = nh.createTimer(ros::Duration(0.01), + // &D2Frontend::loopTimerCallback, this); + // th_loop_det = std::thread(&D2Frontend::loopDetectionThread, this); + // th = std::thread([&] { + // while (0 == loop_net->lcmHandle()) { + // } + // }); } - \ No newline at end of file + +} // namespace D2FrontEnd diff --git a/d2frontend/src/d2frontend_node.cpp b/d2frontend/src/d2frontend_node.cpp index 2e601202..31c31885 100644 --- a/d2frontend/src/d2frontend_node.cpp +++ b/d2frontend/src/d2frontend_node.cpp @@ -1,24 +1,19 @@ #include "d2frontend/d2frontend.h" -class D2FrontendNode : public D2FrontEnd::D2Frontend -{ -public: - D2FrontendNode(ros::NodeHandle & nh) - { - Init(nh); - } +class D2FrontendNode : public D2FrontEnd::D2Frontend { + public: + D2FrontendNode(ros::NodeHandle &nh) { Init(nh); } }; -int main(int argc, char **argv) -{ - cv::setNumThreads(1); - ros::init(argc, argv, "d2frontend"); - ros::NodeHandle n("~"); - ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info); +int main(int argc, char **argv) { + cv::setNumThreads(1); + ros::init(argc, argv, "d2frontend"); + ros::NodeHandle n("~"); + ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, + ros::console::levels::Info); - D2FrontendNode frontend(n); - ros::MultiThreadedSpinner spinner(3); - spinner.spin(); - return 0; + D2FrontendNode frontend(n); + ros::MultiThreadedSpinner spinner(3); + spinner.spin(); + return 0; } - diff --git a/d2frontend/src/d2frontend_nodelet.cpp b/d2frontend/src/d2frontend_nodelet.cpp index b5111306..c490bcd8 100644 --- a/d2frontend/src/d2frontend_nodelet.cpp +++ b/d2frontend/src/d2frontend_nodelet.cpp @@ -1,31 +1,29 @@ -#include "d2frontend/d2frontend.h" #include #include +#include "d2frontend/d2frontend.h" + #define BACKWARD_HAS_DW 1 #include -namespace backward -{ - backward::SignalHandling sh; +namespace backward { +backward::SignalHandling sh; } #include #include -namespace D2FrontEnd -{ - class D2FrontendNode : public nodelet::Nodelet, public D2Frontend - { - public: - D2FrontendNode() {} - private: - virtual void onInit() override - { - ros::NodeHandle & n = getMTPrivateNodeHandle(); - Init(n); - cv::setNumThreads(1); - } - }; -} +namespace D2FrontEnd { +class D2FrontendNode : public nodelet::Nodelet, public D2Frontend { + public: + D2FrontendNode() {} + + private: + virtual void onInit() override { + ros::NodeHandle& n = getMTPrivateNodeHandle(); + Init(n); + cv::setNumThreads(1); + } +}; +} // namespace D2FrontEnd PLUGINLIB_EXPORT_CLASS(D2FrontEnd::D2FrontendNode, nodelet::Nodelet); \ No newline at end of file diff --git a/d2frontend/src/d2frontend_params.cpp b/d2frontend/src/d2frontend_params.cpp index de8a82c3..3bda80bf 100644 --- a/d2frontend/src/d2frontend_params.cpp +++ b/d2frontend/src/d2frontend_params.cpp @@ -1,328 +1,462 @@ #include "d2frontend/d2frontend_params.h" -#include "d2frontend/loop_cam.h" -#include "d2frontend/loop_detector.h" -#include "d2frontend/d2featuretracker.h" -#include "swarm_msgs/swarm_lcm_converter.hpp" -#include -#include + #include #include #include +#include +#include + +#include + +#include "d2frontend/d2featuretracker.h" +#include "d2frontend/loop_cam.h" +#include "d2frontend/loop_detector.h" +#include "swarm_msgs/swarm_lcm_converter.hpp" namespace D2FrontEnd { - D2FrontendParams * params; - std::pair readCameraConfig(const std::string & camera_name, const YAML::Node & config); - D2FrontendParams::D2FrontendParams(ros::NodeHandle & nh) - { - //Read VINS params. - nh.param("vins_config_path",vins_config_path, ""); - cv::FileStorage fsSettings; - fsSettings.open(vins_config_path.c_str(), cv::FileStorage::READ); - int pn = vins_config_path.find_last_of('/'); - std::string configPath = vins_config_path.substr(0, pn); - - loopcamconfig = new LoopCamConfig; - loopdetectorconfig = new LoopDetectorConfig; - ftconfig = new D2FTConfig; - - //Basic confi - nh.param("self_id", self_id, -1); - int _camconfig = fsSettings["camera_configuration"]; - camera_configuration = (CameraConfig) _camconfig; - nh.param("nonkeyframe_waitsec", ACCEPT_NONKEYFRAME_WAITSEC, 5.0); - nh.param("min_movement_keyframe", min_movement_keyframe, 0.3); - - //Debug configs - nh.param("send_img", send_img, false); - nh.param("jpg_quality", JPG_QUALITY, 50); - nh.param("send_whole_img_desc", send_whole_img_desc, false); - nh.param("show", show, false); - nh.param("debug_no_rejection", loopdetectorconfig->DEBUG_NO_REJECT, false); - nh.param("enable_pub_remote_frame", enable_pub_remote_frame, false); - nh.param("enable_pub_local_frame", enable_pub_local_frame, false); - nh.param("enable_sub_remote_frame", enable_sub_remote_frame, false); - nh.param("output_path", OUTPUT_PATH, ""); - enable_perf_output = (int) fsSettings["enable_perf_output"]; - print_network_status = (int) fsSettings["print_network_status"]; - verbose = (int) fsSettings["verbose"]; - ftconfig->write_to_file = (int) fsSettings["write_tracking_image_to_file"]; - - //Loopcam configs - loopcamconfig->superpoint_max_num = (int) fsSettings["max_superpoint_cnt"]; - total_feature_num = (int) fsSettings["max_cnt"]; - loopcamconfig->DEPTH_FAR_THRES = fsSettings["depth_far_thres"]; - loopcamconfig->DEPTH_NEAR_THRES = fsSettings["depth_near_thres"]; - loopcamconfig->show = (int) fsSettings["show_feature_extraction"]; - enable_pca_superpoint = (int) fsSettings["enable_pca_superpoint"]; - enable_pca_netvlad = (int) fsSettings["enable_pca_netvlad"]; - loopdetectorconfig->loop_detection_netvlad_thres = fsSettings["loop_detection_netvlad_thres"]; - track_remote_netvlad_thres = fsSettings["track_remote_netvlad_thres"]; //This is for d2featuretracker - if (enable_pca_superpoint) { - superpoint_dims = (int) fsSettings["superpoint_pca_dims"]; - } - if (enable_pca_netvlad) { - netvlad_dims = (int) fsSettings["netvlad_pca_dims"]; - track_remote_netvlad_thres = 1.46 * track_remote_netvlad_thres - 0.499128; //This is computed in pca_decomp.ipynb - loopdetectorconfig->loop_detection_netvlad_thres = 1.46 * loopdetectorconfig->loop_detection_netvlad_thres - 0.499128; - } - nh.param("superpoint_thres", loopcamconfig->superpoint_thres, 0.012); - nh.param("pca_comp_path",loopcamconfig->pca_comp, ""); - nh.param("pca_mean_path",loopcamconfig->pca_mean, ""); - nh.param("pca_netvlad", pca_netvlad, ""); - nh.param("superpoint_model_path", loopcamconfig->superpoint_model, ""); - nh.param("netvlad_model_path", loopcamconfig->netvlad_model, ""); - loopcamconfig->cnn_enable_tensorrt = (int) fsSettings["cnn_enable_tensorrt"]; - loopcamconfig->cnn_enable_tensorrt_int8 = (int) fsSettings["cnn_enable_tensorrt_int8"]; - if (loopcamconfig->cnn_enable_tensorrt_int8) { - loopcamconfig->netvlad_int8_calib_table_name = (std::string) fsSettings["netvlad_int8_calib_table_name"]; - loopcamconfig->superpoint_int8_calib_table_name = (std::string) fsSettings["superpoint_int8_calib_table_name"]; - } - - nh.param("lower_cam_as_main", loopcamconfig->right_cam_as_main, false); - nh.param("triangle_thres", loopcamconfig->TRIANGLE_THRES, 0.006); - nh.param("output_raw_superpoint_desc", loopcamconfig->OUTPUT_RAW_SUPERPOINT_DESC, false); - nh.param("accept_min_3d_pts", loopcamconfig->ACCEPT_MIN_3D_PTS, 50); - loopcamconfig->camera_configuration = camera_configuration; - loopcamconfig->self_id = self_id; - loopcamconfig->cnn_use_onnx = (int) fsSettings["cnn_use_onnx"]; - loopcamconfig->send_img = send_img; - - //Feature tracker. - ftconfig->show_feature_id = (int) fsSettings["show_track_id"]; - ftconfig->long_track_frames = fsSettings["landmark_estimate_tracks"]; - ftconfig->check_essential = (int) fsSettings["check_essential"]; - ftconfig->enable_lk_optical_flow = (int) fsSettings["enable_lk_optical_flow"]; - ftconfig->lk_use_fast = (int) fsSettings["lk_use_fast"]; - ftconfig->remote_min_match_num = fsSettings["remote_min_match_num"]; - ftconfig->double_counting_common_feature = (int) fsSettings["double_counting_common_feature"]; - ftconfig->enable_superglue_local = (int) fsSettings["enable_superglue_local"]; - ftconfig->enable_superglue_remote = (int) fsSettings["enable_superglue_remote"]; - ftconfig->ransacReprojThreshold = fsSettings["ransacReprojThreshold"]; - ftconfig->parallex_thres = fsSettings["parallex_thres"]; - ftconfig->knn_match_ratio = fsSettings["knn_match_ratio"]; - ftconfig->frame_step = fsSettings["frame_step"]; - nh.param("long_track_thres", ftconfig->long_track_thres, 20); - nh.param("last_track_thres", ftconfig->last_track_thres, 20); - nh.param("new_feature_thres", ftconfig->new_feature_thres, 0.5); - nh.param("min_keyframe_num", ftconfig->min_keyframe_num, 2); - nh.param("superglue_model_path", ftconfig->superglue_model_path, ""); - ftconfig->enable_search_local_aera_remote = (int) fsSettings["enable_search_local_aera_remote"]; - ftconfig->enable_motion_prediction_local = (int) fsSettings["enable_motion_prediction_local"]; - if (!fsSettings["enable_search_local_aera"].empty()) { - ftconfig->enable_search_local_aera = (int) fsSettings["enable_search_local_aera"]; - ftconfig->search_local_max_dist = fsSettings["search_local_max_dist"]; - } else { - printf("[D2FrontendParams] enable_search_local_aera not found, use default\n"); - } - if (!fsSettings["feature_min_dist"].empty()) { - feature_min_dist = fsSettings["feature_min_dist"]; - } else { - printf("[D2FrontendParams] feature_min_dist not found, use default\n"); - } - //Loop detector - loopdetectorconfig->enable_homography_test = (int) fsSettings["enable_homography_test"]; - loopdetectorconfig->accept_loop_max_yaw = (double) fsSettings["accept_loop_max_yaw"]; - loopdetectorconfig->accept_loop_max_pos = (double) fsSettings["accept_loop_max_pos"]; - loopdetectorconfig->loop_inlier_feature_num = fsSettings["loop_inlier_feature_num"]; - loopdetectorconfig->knn_match_ratio = fsSettings["knn_match_ratio"]; - loopdetectorconfig->gravity_check_thres = fsSettings["gravity_check_thres"]; - nh.param("enable_loop", enable_loop, true); - nh.param("is_4dof", loopdetectorconfig->is_4dof, true); - nh.param("match_index_dist", loopdetectorconfig->match_index_dist, 10); - nh.param("match_index_dist_remote", loopdetectorconfig->match_index_dist_remote, 10); - nh.param("min_match_per_dir", loopdetectorconfig->MIN_MATCH_PRE_DIR, 15); - nh.param("inter_drone_init_frames", loopdetectorconfig->inter_drone_init_frames, 50); - nh.param("detector_match_thres", loopdetectorconfig->DETECTOR_MATCH_THRES, 0.9); - nh.param("odometry_consistency_threshold", loopdetectorconfig->odometry_consistency_threshold, 2.0); - nh.param("pos_covariance_per_meter", loopdetectorconfig->pos_covariance_per_meter, 0.01); - nh.param("yaw_covariance_per_meter", loopdetectorconfig->yaw_covariance_per_meter, 0.003); - nh.param("loop_cov_pos", loopdetectorconfig->loop_cov_pos, 0.013); - nh.param("loop_cov_ang", loopdetectorconfig->loop_cov_ang, 2.5e-04); - nh.param("min_direction_loop", loopdetectorconfig->MIN_DIRECTION_LOOP, 3); - pgo_mode = static_cast((int) fsSettings["pgo_mode"]); - nh.param("superglue_model_path", loopdetectorconfig->superglue_model_path, ""); - - //Network config - nh.param("lcm_uri", _lcm_uri, "udpm://224.0.0.251:7667?ttl=1"); - nh.param("recv_msg_duration", recv_msg_duration, 0.5); - nh.param("enable_network", enable_network, true); - lazy_broadcast_keyframe = (int) fsSettings["lazy_broadcast_keyframe"]; - printf("[D2Frontend] Using lazy broadcast keyframe: %d\n", lazy_broadcast_keyframe); - - if (camera_configuration == CameraConfig::STEREO_PINHOLE) { - loopdetectorconfig->MAX_DIRS = 1; - min_receive_images = 2; - } else if (camera_configuration == CameraConfig::STEREO_FISHEYE) { - loopdetectorconfig->MAX_DIRS = 4; - min_receive_images = 2; - } else if (camera_configuration == CameraConfig::PINHOLE_DEPTH) { - loopdetectorconfig->MAX_DIRS = 1; - min_receive_images = 1; - } else if (camera_configuration == CameraConfig::FOURCORNER_FISHEYE) { - loopdetectorconfig->MAX_DIRS = 4; - min_receive_images = 4; - } else { - ROS_ERROR("[D2Frontend] Camera configuration %d not implement yet.", camera_configuration); - exit(-1); - } - depth_topics.emplace_back((std::string) fsSettings["depth_topic"]); - is_comp_images = (int) fsSettings["is_compressed_images"]; - generateCameraModels(fsSettings, configPath); - } +D2FrontendParams* params; + +D2FrontendParams::D2FrontendParams(ros::NodeHandle& nh) { + // Read VINS params. + nh.param("vins_config_path", vins_config_path, ""); + cv::FileStorage fsSettings; + printf("Read VINS config from %s\n", vins_config_path.c_str()); + fsSettings.open(vins_config_path.c_str(), cv::FileStorage::READ); + + int pn = vins_config_path.find_last_of('/'); + std::string configPath = vins_config_path.substr(0, pn); + + loopcamconfig = new LoopCamConfig; + loopdetectorconfig = new LoopDetectorConfig; + ftconfig = new D2FTConfig; + + // Basic confi + nh.param("self_id", self_id, -1); + int _camconfig = fsSettings["camera_configuration"]; + camera_configuration = (CameraConfig)_camconfig; + nh.param("nonkeyframe_waitsec", ACCEPT_NONKEYFRAME_WAITSEC, 5.0); + nh.param("min_movement_keyframe", min_movement_keyframe, 0.3); + estimation_mode = (ESTIMATION_MODE)(int)fsSettings["estimation_mode"]; - void D2FrontendParams::generateCameraModels(cv::FileStorage & fsSettings, std::string configPath) { - readCameraConfigs(fsSettings, configPath); - camodocal::CameraFactory cam_factory; - for (auto & cam_calib_path : camera_config_paths) { - ROS_INFO("[D2Frontend] Read camera from %s", cam_calib_path.c_str()); - auto cam = cam_factory.generateCameraFromYamlFile(cam_calib_path); - if (cam) { - camera_ptrs.push_back(cam); - } else { - ROS_ERROR("[D2Frontend]Failed to read camera from %s", cam_calib_path.c_str()); - } - } - std::string photometric_calib_file = fsSettings["photometric_calib"]; - cv::Mat photometric; - if ( photometric_calib_file != "") { - double avg_photometric = fsSettings["avg_photometric"]; - photometric = cv::imread(configPath + "/" + photometric_calib_file, cv::IMREAD_GRAYSCALE); - photometric.convertTo(photometric, CV_32FC1, 1.0/255.0); - cv::divide(avg_photometric, photometric, photometric); - printf("[D2Frontend] Read photometric calibration from: %s\n", photometric_calib_file.c_str()); - } else { - printf("[D2Frontend] No photometric calibration file provided.\n"); - } - if (enable_undistort_image) { - raw_camera_ptrs = camera_ptrs; - camera_ptrs.clear(); - for (auto cam: raw_camera_ptrs) { - auto ptr = new FisheyeUndist(cam, 0, undistort_fov, true, FisheyeUndist::UndistortCylindrical, - width_undistort, height_undistort, photometric); - auto cylind_cam = ptr->cam_top; - camera_ptrs.push_back(cylind_cam); - undistortors.emplace_back(ptr); - // focal_length = static_cast(cylind_cam.get())->getParameters().fx(); - } - } else { - // auto cam = camera_ptrs[0]; - // if (cam->modelType() == camodocal::Camera::MEI) { - // focal_length = static_cast(cam.get())->getParameters().gamma1(); - // } else if (cam->modelType() == camodocal::Camera::PINHOLE) { - // focal_length = static_cast(cam.get())->getParameters().fx(); - // } - } - printf("[D2Frontend] Focal length initialize to: %.1f\n", focal_length); + //frontend settings + + + // Debug configs + nh.param("send_img", send_img, false); + nh.param("jpg_quality", JPG_QUALITY, 50); + nh.param("send_whole_img_desc", send_whole_img_desc, false); + nh.param("show", show, false); + nh.param("debug_no_rejection", loopdetectorconfig->DEBUG_NO_REJECT, + false); + nh.param("enable_pub_remote_frame", enable_pub_remote_frame, false); + nh.param("enable_pub_local_frame", enable_pub_local_frame, false); + nh.param("enable_sub_remote_frame", enable_sub_remote_frame, false); + nh.param("output_path", OUTPUT_PATH, ""); + enable_perf_output = (int)fsSettings["enable_perf_output"]; + print_network_status = (int)fsSettings["print_network_status"]; + verbose = (int)fsSettings["verbose"]; + ftconfig->write_to_file = (int)fsSettings["write_tracking_image_to_file"]; + + // Loopcam configs + loopcamconfig->superpoint_max_num = (int)fsSettings["max_superpoint_cnt"]; + total_feature_num = (int)fsSettings["max_cnt"]; + loopcamconfig->DEPTH_FAR_THRES = fsSettings["depth_far_thres"]; + loopcamconfig->DEPTH_NEAR_THRES = fsSettings["depth_near_thres"]; + loopcamconfig->show = (int)fsSettings["show_feature_extraction"]; + enable_pca_superpoint = (int)fsSettings["enable_pca_superpoint"]; + enable_pca_netvlad = (int)fsSettings["enable_pca_netvlad"]; + loopdetectorconfig->loop_detection_netvlad_thres = + fsSettings["loop_detection_netvlad_thres"]; + track_remote_netvlad_thres = + fsSettings["track_remote_netvlad_thres"]; // This is for d2featuretracker + if (enable_pca_superpoint) { + superpoint_dims = (int)fsSettings["superpoint_pca_dims"]; + } + if (enable_pca_netvlad) { + netvlad_dims = (int)fsSettings["netvlad_pca_dims"]; + track_remote_netvlad_thres = + 1.46 * track_remote_netvlad_thres - + 0.499128; // This is computed in pca_decomp.ipynb + loopdetectorconfig->loop_detection_netvlad_thres = + 1.46 * loopdetectorconfig->loop_detection_netvlad_thres - 0.499128; + } + + //superpoint configurations + cv::FileNode fs_superpoint_config = fsSettings["superpoint_config"]; + SuperPoint::SuperPointConfig superpoint_config; + superpoint_config.max_keypoints = static_cast(fsSettings["max_superpoint_cnt"]); + superpoint_config.onnx_path = static_cast(fs_superpoint_config["onnx_path"]); + superpoint_config.engine_path = static_cast(fs_superpoint_config["trt_engine_path"]); + superpoint_config.keypoint_threshold = static_cast(fs_superpoint_config["threshold"]); + cv::FileNode input_tensor_names = fs_superpoint_config["input_tensor_names"]; + for (const auto& name : input_tensor_names) { + superpoint_config.input_tensor_names.push_back(static_cast(name)); + printf("input_tensor_names: %s\n", static_cast(name).c_str()); + } + cv::FileNode output_tensor_names = fs_superpoint_config["output_tensor_names"]; + for (const auto& name : output_tensor_names) { + superpoint_config.output_tensor_names.push_back(static_cast(name)); + printf("output_tensor_names: %s\n", static_cast(name).c_str()); + } + superpoint_config.input_height = static_cast(fs_superpoint_config["input_height"]); + superpoint_config.input_width = static_cast(fs_superpoint_config["input_width"]); + superpoint_config.enable_pca = static_cast(fs_superpoint_config["enable_pca"]) !=0; + superpoint_config.pca_mean_path = static_cast(fs_superpoint_config["pca_mean_path"]); + superpoint_config.pca_comp_path = static_cast(fs_superpoint_config["pca_comp_path"]); + superpoint_config.superpoint_pca_dims = static_cast(fs_superpoint_config["superpoint_pca_dims"]); + + //print superpoint configuration all above + printf("superpoint_config.max_keypoints: %d\n", superpoint_config.max_keypoints); + printf("superpoint_config.onnx_path: %s\n", superpoint_config.onnx_path.c_str()); + printf("superpoint_config.engine_path: %s\n", superpoint_config.engine_path.c_str()); + printf("superpoint_config.keypoint_threshold: %f\n", superpoint_config.keypoint_threshold); + + loopcamconfig->superpoint_config = superpoint_config; + + + // nh.param("superpoint_thres", loopcamconfig->superpoint_thres, 0.012); + // nh.param("pca_comp_path", loopcamconfig->pca_comp, ""); + // nh.param("pca_mean_path", loopcamconfig->pca_mean, ""); + // nh.param("pca_netvlad", pca_netvlad, ""); + // nh.param("superpoint_model_path", + // loopcamconfig->superpoint_model, ""); + nh.param("netvlad_model_path", loopcamconfig->netvlad_model, ""); + loopcamconfig->cnn_enable_tensorrt = (int)fsSettings["cnn_enable_tensorrt"]; + loopcamconfig->cnn_enable_tensorrt_int8 = + (int)fsSettings["cnn_enable_tensorrt_int8"]; + if (loopcamconfig->cnn_enable_tensorrt_int8) { + loopcamconfig->netvlad_int8_calib_table_name = + (std::string)fsSettings["netvlad_int8_calib_table_name"]; + loopcamconfig->superpoint_int8_calib_table_name = + (std::string)fsSettings["superpoint_int8_calib_table_name"]; + } + + nh.param("lower_cam_as_main", loopcamconfig->right_cam_as_main, false); + nh.param("triangle_thres", loopcamconfig->TRIANGLE_THRES, 0.006); + nh.param("output_raw_superpoint_desc", + loopcamconfig->OUTPUT_RAW_SUPERPOINT_DESC, false); + nh.param("accept_min_3d_pts", loopcamconfig->ACCEPT_MIN_3D_PTS, 50); + loopcamconfig->camera_configuration = camera_configuration; + loopcamconfig->self_id = self_id; + loopcamconfig->cnn_use_onnx = (int)fsSettings["cnn_use_onnx"]; + loopcamconfig->send_img = send_img; + + //frontend thread frequency + ftconfig->lcm_thread_rate = static_cast(fsSettings["lcm_thread_freq"]); + ftconfig->stereo_frame_thread_rate = static_cast(fsSettings["image_freq"]); + ftconfig->loop_detection_thread_rate = static_cast(fsSettings["loop_detection_freq"]); + + // Feature tracker. + ftconfig->show_feature_id = (int)fsSettings["show_track_id"]; + ftconfig->long_track_frames = fsSettings["landmark_estimate_tracks"]; + ftconfig->check_essential = (int)fsSettings["check_essential"]; + ftconfig->enable_lk_optical_flow = (int)fsSettings["enable_lk_optical_flow"]; + ftconfig->lk_use_fast = (int)fsSettings["lk_use_fast"]; + ftconfig->remote_min_match_num = fsSettings["remote_min_match_num"]; + ftconfig->double_counting_common_feature = + (int)fsSettings["double_counting_common_feature"]; + ftconfig->enable_superglue_local = (int)fsSettings["enable_superglue_local"]; + ftconfig->enable_superglue_remote = + (int)fsSettings["enable_superglue_remote"]; + ftconfig->ransacReprojThreshold = fsSettings["ransacReprojThreshold"]; + ftconfig->parallex_thres = fsSettings["parallex_thres"]; + ftconfig->knn_match_ratio = fsSettings["knn_match_ratio"]; + ftconfig->frame_step = fsSettings["frame_step"]; + nh.param("long_track_thres", ftconfig->long_track_thres, 0); + nh.param("last_track_thres", ftconfig->last_track_thres, 20); + nh.param("new_feature_thres", ftconfig->new_feature_thres, 0.5); + nh.param("min_keyframe_num", ftconfig->min_keyframe_num, 2); + nh.param("superglue_model_path", ftconfig->superglue_model_path, + ""); + ftconfig->enable_search_local_aera_remote = + (int)fsSettings["enable_search_local_aera_remote"]; + ftconfig->enable_motion_prediction_local = + (int)fsSettings["enable_motion_prediction_local"]; + ftconfig->sp_track_use_lk = (int)fsSettings["sp_track_use_lk"]; + if (!fsSettings["enable_search_local_aera"].empty()) { + ftconfig->enable_search_local_aera = + (int)fsSettings["enable_search_local_aera"]; + ftconfig->search_local_max_dist = fsSettings["search_local_max_dist"]; + } else { + SPDLOG_INFO( + "[D2FrontendParams] enable_search_local_aera not found, use default\n"); + } + if (!fsSettings["feature_min_dist"].empty()) { + feature_min_dist = fsSettings["feature_min_dist"]; + } else { + SPDLOG_WARN( + "[D2FrontendParams] feature_min_dist not found, use default: {}", + feature_min_dist); + } + ftconfig->track_from_keyframe = (int)fsSettings["track_from_keyframe"]; + // Loop detector + loopdetectorconfig->enable_homography_test = + (int)fsSettings["enable_homography_test"]; + loopdetectorconfig->accept_loop_max_yaw = + (double)fsSettings["accept_loop_max_yaw"]; + loopdetectorconfig->accept_loop_max_pos = + (double)fsSettings["accept_loop_max_pos"]; + loopdetectorconfig->loop_inlier_feature_num = + fsSettings["loop_inlier_feature_num"]; + loopdetectorconfig->knn_match_ratio = fsSettings["knn_match_ratio"]; + loopdetectorconfig->gravity_check_thres = fsSettings["gravity_check_thres"]; + nh.param("enable_loop", enable_loop, true); + nh.param("is_4dof", loopdetectorconfig->is_4dof, true); + nh.param("match_index_dist", loopdetectorconfig->match_index_dist, 10); + nh.param("match_index_dist_remote", + loopdetectorconfig->match_index_dist_remote, 10); + nh.param("min_match_per_dir", loopdetectorconfig->MIN_MATCH_PRE_DIR, 15); + nh.param("inter_drone_init_frames", + loopdetectorconfig->inter_drone_init_frames, 50); + nh.param("detector_match_thres", + loopdetectorconfig->DETECTOR_MATCH_THRES, 0.9); + nh.param("odometry_consistency_threshold", + loopdetectorconfig->odometry_consistency_threshold, 2.0); + nh.param("pos_covariance_per_meter", + loopdetectorconfig->pos_covariance_per_meter, 0.01); + nh.param("yaw_covariance_per_meter", + loopdetectorconfig->yaw_covariance_per_meter, 0.003); + nh.param("loop_cov_pos", loopdetectorconfig->loop_cov_pos, 0.013); + nh.param("loop_cov_ang", loopdetectorconfig->loop_cov_ang, 2.5e-04); + nh.param("min_direction_loop", loopdetectorconfig->MIN_DIRECTION_LOOP, + 3); + pgo_mode = static_cast((int)fsSettings["pgo_mode"]); + nh.param("superglue_model_path", + loopdetectorconfig->superglue_model_path, ""); + + // Network config + nh.param("lcm_uri", _lcm_uri, "udpm://224.0.0.251:7667?ttl=1"); + nh.param("recv_msg_duration", recv_msg_duration, 0.5); + nh.param("enable_network", enable_network, true); + lazy_broadcast_keyframe = (int)fsSettings["lazy_broadcast_keyframe"]; + SPDLOG_INFO("[D2Frontend] Using lazy broadcast keyframe: {}", + lazy_broadcast_keyframe); + + if (camera_configuration == CameraConfig::STEREO_PINHOLE) { + loopdetectorconfig->MAX_DIRS = 1; + min_receive_images = 2; + } else if (camera_configuration == CameraConfig::STEREO_FISHEYE) { + loopdetectorconfig->MAX_DIRS = 4; + min_receive_images = 2; + } else if (camera_configuration == CameraConfig::PINHOLE_DEPTH) { + loopdetectorconfig->MAX_DIRS = 1; + min_receive_images = 1; + } else if (camera_configuration == CameraConfig::FOURCORNER_FISHEYE) { + loopdetectorconfig->MAX_DIRS = 4; + min_receive_images = 4; + } else { + SPDLOG_INFO("[D2Frontend] Camera configuration {} not implement yet.", + camera_configuration); + exit(-1); + } + depth_topics.emplace_back((std::string)fsSettings["depth_topic"]); + is_comp_images = (int)fsSettings["is_compressed_images"]; + generateCameraModels(fsSettings, configPath); +} + +void D2FrontendParams::generateCameraModels(cv::FileStorage& fsSettings, + std::string configPath) { + readCameraConfigs(fsSettings, configPath); + camodocal::CameraFactory cam_factory; + for (auto& cam_calib_path : camera_config_paths) { + SPDLOG_INFO("[D2Frontend] Read camera from {}", cam_calib_path); + auto cam = cam_factory.generateCameraFromYamlFile(cam_calib_path); + if (cam) { + camera_ptrs.push_back(cam); + } else { + SPDLOG_ERROR("[D2Frontend]Failed to read camera from {}", cam_calib_path); + exit(-1); + } + } + std::string photometric_calib_file = fsSettings["photometric_calib"]; + cv::Mat photometric; + if (photometric_calib_file != "") { + double avg_photometric = fsSettings["avg_photometric"]; + photometric = cv::imread(configPath + "/" + photometric_calib_file, + cv::IMREAD_GRAYSCALE); + photometric.convertTo(photometric, CV_32FC1, 1.0 / 255.0); + cv::divide(avg_photometric, photometric, photometric); + SPDLOG_INFO("[D2Frontend] Read photometric calibration from: {}", + photometric_calib_file); + } else { + SPDLOG_WARN("[D2Frontend] No photometric calibration file provided"); + } + if (enable_undistort_image) { + raw_camera_ptrs = camera_ptrs; + camera_ptrs.clear(); + for (auto cam : raw_camera_ptrs) { + auto ptr = new FisheyeUndist( + cam, 0, undistort_fov, true, FisheyeUndist::UndistortCylindrical, + width_undistort, height_undistort, photometric); + auto cylind_cam = ptr->cam_top; + camera_ptrs.push_back(cylind_cam); + undistortors.emplace_back(ptr); + // focal_length = static_cast(cylind_cam.get())->getParameters().fx(); } + } else { + // auto cam = camera_ptrs[0]; + // if (cam->modelType() == camodocal::Camera::MEI) { + // focal_length = static_cast(cam.get())->getParameters().gamma1(); + // } else if (cam->modelType() == camodocal::Camera::PINHOLE) { + // focal_length = static_cast(cam.get())->getParameters().fx(); + // } + } + SPDLOG_INFO("[D2Frontend] Focal length initialize to: {:.1f}", focal_length); +} +void D2FrontendParams::readCameraConfigs(cv::FileStorage& fsSettings, + std::string configPath) { + enable_undistort_image = loopcamconfig->enable_undistort_image = + (int)fsSettings["enable_undistort_image"]; + width_undistort = (int)fsSettings["width_undistort"]; + height_undistort = (int)fsSettings["height_undistort"]; + undistort_fov = fsSettings["undistort_fov"]; + width = (int)fsSettings["image_width"]; + height = (int)fsSettings["image_height"]; + std::string camera_seq_str = + fsSettings["camera_seq"]; // Back-right Back-left Front-left Front-right + if (camera_seq_str == "") { + this->camera_seq = std::vector{0, 1, 2, 3}; + } else { + // Camera seq from string "0123" to vector {0, 1, 2, 3} + this->camera_seq = std::vector(camera_seq_str.size()); + std::transform(camera_seq_str.begin(), camera_seq_str.end(), + this->camera_seq.begin(), [](char c) { return c - '0'; }); + } - void D2FrontendParams::readCameraConfigs(cv::FileStorage & fsSettings, std::string configPath) { - enable_undistort_image = loopcamconfig->enable_undistort_image = (int) fsSettings["enable_undistort_image"]; - width_undistort = (int) fsSettings["width_undistort"]; - height_undistort = (int) fsSettings["height_undistort"]; - undistort_fov = fsSettings["undistort_fov"]; - width = (int) fsSettings["image_width"]; - height = (int) fsSettings["image_height"]; - std::string camera_seq_str = fsSettings["camera_seq"]; // Back-right Back-left Front-left Front-right - if (camera_seq_str == "") { - this->camera_seq = std::vector{0, 1, 2, 3}; - } else { - //Camera seq from string "0123" to vector {0, 1, 2, 3} - this->camera_seq = std::vector(camera_seq_str.size()); - std::transform(camera_seq_str.begin(), camera_seq_str.end(), this->camera_seq.begin(), [](char c) { return c - '0'; }); - } - - std::string calib_file_path = fsSettings["calib_file_path"]; - if (calib_file_path != "") { - calib_file_path = configPath + "/" + calib_file_path; - ROS_INFO("Will read camera calibration from %s", calib_file_path.c_str()); - readCameraCalibrationfromFile(calib_file_path); - int camera_num = extrinsics.size(); - for (auto i = 0; i < camera_num; i++) { - char param_name[64] = {0}; - sprintf(param_name, "image%d_topic", i); - std::string topic = (std::string) fsSettings[param_name]; - image_topics.emplace_back(topic); - } - } else { - ROS_INFO("Read camera from config file"); - //Camera configurations from VINS config. - int camera_num = fsSettings["num_of_cam"]; - for (auto i = 0; i < camera_num; i++) { - std::string index = std::to_string(i); - char param_name[64] = {0}; - sprintf(param_name, "image%d_topic", i); - std::string topic = (std::string) fsSettings[param_name]; - - sprintf(param_name, "cam%d_calib", i); - std::string camera_calib_path = (std::string) fsSettings[param_name]; - camera_calib_path = configPath + "/" + camera_calib_path; - - sprintf(param_name, "body_T_cam%d", i); - cv::Mat cv_T; - fsSettings[param_name] >> cv_T; - Eigen::Matrix4d T; - cv::cv2eigen(cv_T, T); - Swarm::Pose pose(T.block<3, 3>(0, 0), T.block<3, 1>(0, 3)); - - image_topics.emplace_back(topic); - camera_config_paths.emplace_back(camera_calib_path); - extrinsics.emplace_back(pose); - - ROS_INFO("[SWARM_LOOP] Camera %d: topic: %s, calib: %s, T: %s", - i, topic.c_str(), camera_calib_path.c_str(), pose.toStr().c_str()); - } - } + std::string calib_file_path = fsSettings["calib_file_path"]; + int32_t extrinsic_parameter_type = static_cast(fsSettings["extrinsic_parameter_type"]); + if (calib_file_path != "") { + calib_file_path = configPath + "/" + calib_file_path; + SPDLOG_INFO("Will read camera calibration from {}", calib_file_path); + readCameraCalibrationfromFile(calib_file_path, extrinsic_parameter_type); + int camera_num = extrinsics.size(); + for (auto i = 0; i < camera_num; i++) { + char param_name[64] = {0}; + sprintf(param_name, "image%d_topic", i); + std::string topic = (std::string)fsSettings[param_name]; + image_topics.emplace_back(topic); } + } else { + SPDLOG_INFO("Read camera from config file"); + // Camera configurations from VINS config. + int camera_num = fsSettings["num_of_cam"]; + for (auto i = 0; i < camera_num; i++) { + std::string index = std::to_string(i); + char param_name[64] = {0}; + sprintf(param_name, "image%d_topic", i); + std::string topic = (std::string)fsSettings[param_name]; + + sprintf(param_name, "cam%d_calib", i); + std::string camera_calib_path = (std::string)fsSettings[param_name]; + camera_calib_path = configPath + "/" + camera_calib_path; + + sprintf(param_name, "body_T_cam%d", i); + cv::Mat cv_T; + fsSettings[param_name] >> cv_T; + Eigen::Matrix4d T; + cv::cv2eigen(cv_T, T); + Swarm::Pose pose(T.block<3, 3>(0, 0), T.block<3, 1>(0, 3)); - void D2FrontendParams::readCameraCalibrationfromFile(const std::string & path) { - YAML::Node config = YAML::LoadFile(path); - for (const auto& kv : config) { - std::string camera_name = kv.first.as(); - std::cout << camera_name << "\n"; - const YAML::Node& value = kv.second; - auto ret = readCameraConfig(camera_name, value); - camera_ptrs.emplace_back(ret.first); - extrinsics.emplace_back(ret.second); - } + image_topics.emplace_back(topic); + camera_config_paths.emplace_back(camera_calib_path); + extrinsics.emplace_back(pose); + + SPDLOG_INFO("Camera {}: topic: {}, calib: {}, T: {}", i, topic.c_str(), + camera_calib_path, pose.toStr()); } + } +} + +void D2FrontendParams::readCameraCalibrationfromFile(const std::string& path, int32_t extrinsic_parameter_type) { + YAML::Node config = YAML::LoadFile(path); + for (const auto& kv : config) { + std::string camera_name = kv.first.as(); + std::cout << camera_name << "\n"; + const YAML::Node& value = kv.second; + auto ret = readCameraConfig(camera_name, value, extrinsic_parameter_type); + camera_ptrs.emplace_back(ret.first); + extrinsics.emplace_back(ret.second); + } +} + +std::pair D2FrontendParams:: +readCameraConfig( + const std::string& camera_name, const YAML::Node& config, int32_t extrinsic_parameter_type) { + // In this case, we generate camera ptr. + // Now only accept omni-radtan. + camodocal::CameraPtr camera; + if (config["camera_model"].as() == "omni" && + config["distortion_model"].as() == "radtan") { + int width = config["resolution"][0].as(); + int height = config["resolution"][1].as(); + double xi = config["intrinsics"][0].as(); + double gamma1 = config["intrinsics"][1].as(); + double gamma2 = config["intrinsics"][2].as(); + double u0 = config["intrinsics"][3].as(); + double v0 = config["intrinsics"][4].as(); + double k1 = config["distortion_coeffs"][0].as(); + double k2 = config["distortion_coeffs"][1].as(); + double p1 = config["distortion_coeffs"][2].as(); + double p2 = config["distortion_coeffs"][3].as(); + SPDLOG_INFO( + "Camera {} model omni-radtan: width: {}, height: {}, xi: {}, gamma1: " + "{}, gamma2: {}, u0: {}, v0: {}, k1: {}, k2: {}, p1: {}, p2: {}", + camera_name.c_str(), width, height, xi, gamma1, gamma2, u0, v0, k1, k2, + p1, p2); + camera = camodocal::CataCameraPtr( + new camodocal::CataCamera(camera_name, width, height, xi, k1, k2, p1, + p2, gamma1, gamma2, u0, v0)); + } else if (config["camera_model"].as() == "pinhole" && + config["distortion_model"].as() == "radtan") { + int width = config["resolution"][0].as(); + int height = config["resolution"][1].as(); + double fx = config["intrinsics"][0].as(); + double fy = config["intrinsics"][1].as(); + double cx = config["intrinsics"][2].as(); + double cy = config["intrinsics"][3].as(); - std::pair readCameraConfig(const std::string & camera_name, const YAML::Node & config) { - //In this case, we generate camera ptr. - //Now only accept omni-radtan. - camodocal::CameraPtr camera; - if (config["camera_model"].as() == "omni" && - config["distortion_model"].as() == "radtan" ) { - int width = config["resolution"][0].as(); - int height = config["resolution"][1].as(); - double xi = config["intrinsics"][0].as(); - double gamma1 = config["intrinsics"][1].as(); - double gamma2 = config["intrinsics"][2].as(); - double u0 = config["intrinsics"][3].as(); - double v0 = config["intrinsics"][4].as(); - double k1 = config["distortion_coeffs"][0].as(); - double k2 = config["distortion_coeffs"][1].as(); - double p1 = config["distortion_coeffs"][2].as(); - double p2 = config["distortion_coeffs"][3].as(); - printf("Camera %s model omni-radtan\n width: %d, height: %d, xi: %f, gamma1: %f, gamma2: %f, u0: %f, v0: %f, k1: %f, k2: %f, p1: %f, p2: %f\n", - camera_name.c_str(), width, height, xi, gamma1, gamma2, u0, v0, k1, k2, p1, p2); - camera = camodocal::CataCameraPtr(new camodocal::CataCamera(camera_name, - width, height, xi, k1, k2, p1, p2, - gamma1, gamma2, u0, v0)); - } - Matrix4d T; - for (int i = 0; i < 4; i++) { - for (int j = 0; j < 4; j++) { - T(i, j) = config["T_cam_imu"][i][j].as(); - } - } - Matrix3d R = T.block<3, 3>(0, 0); - Vector3d t = T.block<3, 1>(0, 3); - Swarm::Pose pose(T.block<3, 3>(0, 0), T.block<3, 1>(0, 3)); - std::cout << "T_cam_imu:\n" << T << std::endl; - std::cout << "pose:\n" << pose.toStr() << std::endl; - - return std::make_pair(camera, pose); + double k1 = config["distortion_coeffs"][0].as(); + double k2 = config["distortion_coeffs"][1].as(); + double p1 = config["distortion_coeffs"][2].as(); + double p2 = config["distortion_coeffs"][3].as(); + SPDLOG_INFO( + "Camera {} model pinhole-radtan: width: {}, height: {}, fx: {}, fy: " + "{}, cx: {}, cy: {}, k1: {}, k2: {}, p1: {}, p2: {}", + camera_name.c_str(), width, height, fx, fy, cx, cy, k1, k2, p1, p2); + camera = camodocal::PinholeCameraPtr(new camodocal::PinholeCamera( + camera_name, width, height, k1, k2, p1, p2, fx, fy, cx, cy)); + } else { + SPDLOG_ERROR( + "Camera not supported yet, please fillin in src/d2frontend_params.cpp " + "function: readCameraConfig"); + exit(-1); + } + Matrix4d T; + for (int i = 0; i < 4; i++) { + for (int j = 0; j < 4; j++) { + T(i, j) = config["T_cam_imu"][i][j].as(); } + } + + Matrix3d R; + Vector3d t; + // OmniNxt use mode 0 thus R=R.inverse(); t=-R*t; + if (extrinsic_parameter_type == 0){ + R = T.block<3, 3>(0, 0).transpose(); + t = -R*T.block<3, 1>(0, 3); + } else { + R = T.block<3, 3>(0, 0); + t = T.block<3, 1>(0, 3); + } + Swarm::Pose pose(R, t); + std::cout << "T_cam_imu:\n" << T << std::endl; + std::cout << "pose:\n" << pose.toStr() << std::endl; + + return std::make_pair(camera, pose); +} -} \ No newline at end of file +} // namespace D2FrontEnd \ No newline at end of file diff --git a/d2frontend/src/d2frontend_spy.cpp b/d2frontend/src/d2frontend_spy.cpp index 3c17461b..4024acfb 100644 --- a/d2frontend/src/d2frontend_spy.cpp +++ b/d2frontend/src/d2frontend_spy.cpp @@ -1,95 +1,101 @@ -#include "ros/ros.h" -#include -#include "d2frontend/loop_net.h" -#include #include +#include +#include #include +#include "d2frontend/loop_net.h" +#include "ros/ros.h" + #define BACKWARD_HAS_DW 1 #include -namespace backward -{ - backward::SignalHandling sh; +namespace backward { +backward::SignalHandling sh; } -using namespace std::chrono; -using namespace D2FrontEnd; +using namespace std::chrono; +using namespace D2FrontEnd; class SwarmLoopSpy { -public: - LoopNet * loop_net = nullptr; - std::map all_images; - ros::Timer timer; -public: - SwarmLoopSpy(ros::NodeHandle& nh) { - //Init Loop Net - std::string _lcm_uri = "0.0.0.0"; - std::string camera_config_path = ""; - std::string BRIEF_PATTHER_FILE = ""; - std::string ORB_VOC = ""; - int self_id = -1; - nh.param("camera_config_path",camera_config_path, - "/home/xuhao/swarm_ws/src/VINS-Fusion-gpu/config/vi_car/cam0_mei.yaml"); - - nh.param("lcm_uri", _lcm_uri, "udpm://224.0.0.251:7667?ttl=1"); - loop_net = new LoopNet(_lcm_uri, false, false); - loop_net->frame_desc_callback = [&] (const VisualImageDescArray & img_desc) { - ROS_INFO("Received Img Desc from %d", img_desc.drone_id); - all_images[img_desc.drone_id] = img_desc; - }; - - loop_net->loopconn_callback = [&] (const LoopEdge_t & loop_conn) { - ROS_INFO("Received loop from %d to %d", loop_conn.drone_id_a, loop_conn.drone_id_b); - }; - - timer = nh.createTimer(ros::Duration(0.03), &SwarmLoopSpy::timer_callback, this); - } + public: + LoopNet *loop_net = nullptr; + std::map all_images; + ros::Timer timer; - void onLoopConnection (LoopEdge & loop_con, bool is_local = false) { - ROS_INFO("Loop conn from %d to %d", loop_con.drone_id_a, loop_con.drone_id_b); - } + public: + SwarmLoopSpy(ros::NodeHandle &nh) { + // Init Loop Net + std::string _lcm_uri = "0.0.0.0"; + std::string camera_config_path = ""; + std::string BRIEF_PATTHER_FILE = ""; + std::string ORB_VOC = ""; + int self_id = -1; + nh.param( + "camera_config_path", camera_config_path, + "/home/xuhao/swarm_ws/src/VINS-Fusion-gpu/config/vi_car/cam0_mei.yaml"); - void timer_callback(const ros::TimerEvent & e) { - for (auto &it : all_images) { - auto & img_desc = it.second; - char win_name[100] = {0}; - char frame_name[100] = {0}; - sprintf(win_name, "Drone: %d", img_desc.drone_id); - auto ret = cv::imdecode(img_desc.images[1].image, cv::IMREAD_GRAYSCALE); - auto nowPts = img_desc.images[1].landmarks2D(); - - cv::cvtColor(ret, ret, cv::COLOR_GRAY2BGR); - for (auto pt: nowPts) { - cv::circle(ret, pt, 1, cv::Scalar(255, 0, 0),1); - } - - cv::resize(ret, ret, cv::Size(), VISUALIZE_SCALE, VISUALIZE_SCALE); - - sprintf(frame_name, "Frame %ld", img_desc.frame_id); - cv::putText(ret, frame_name, cv::Point2f(10,10), cv::FONT_HERSHEY_PLAIN, 0.8, cv::Scalar(0,255,0)); - - sprintf(frame_name, "Landmark num %ld", img_desc.landmarkNum()); - cv::putText(ret, frame_name, cv::Point2f(10,20), cv::FONT_HERSHEY_PLAIN, 0.8, cv::Scalar(0,255,0)); - - cv::imshow(win_name, ret); - } - cv::waitKey(10); - } + nh.param("lcm_uri", _lcm_uri, "udpm://224.0.0.251:7667?ttl=1"); + loop_net = new LoopNet(_lcm_uri, false, false); + loop_net->frame_desc_callback = [&](const VisualImageDescArray &img_desc) { + ROS_INFO("Received Img Desc from %d", img_desc.drone_id); + all_images[img_desc.drone_id] = img_desc; + }; + loop_net->loopconn_callback = [&](const LoopEdge_t &loop_conn) { + ROS_INFO("Received loop from %d to %d", loop_conn.drone_id_a, + loop_conn.drone_id_b); + }; + + timer = nh.createTimer(ros::Duration(0.03), &SwarmLoopSpy::timer_callback, + this); + } + + void onLoopConnection(LoopEdge &loop_con, bool is_local = false) { + ROS_INFO("Loop conn from %d to %d", loop_con.drone_id_a, + loop_con.drone_id_b); + } + + void timer_callback(const ros::TimerEvent &e) { + for (auto &it : all_images) { + auto &img_desc = it.second; + char win_name[100] = {0}; + char frame_name[100] = {0}; + sprintf(win_name, "Drone: %d", img_desc.drone_id); + auto ret = cv::imdecode(img_desc.images[1].image, cv::IMREAD_GRAYSCALE); + auto nowPts = img_desc.images[1].landmarks2D(); + + cv::cvtColor(ret, ret, cv::COLOR_GRAY2BGR); + for (auto pt : nowPts) { + cv::circle(ret, pt, 1, cv::Scalar(255, 0, 0), 1); + } + + cv::resize(ret, ret, cv::Size(), VISUALIZE_SCALE, VISUALIZE_SCALE); + + sprintf(frame_name, "Frame %ld", img_desc.frame_id); + cv::putText(ret, frame_name, cv::Point2f(10, 10), cv::FONT_HERSHEY_PLAIN, + 0.8, cv::Scalar(0, 255, 0)); + + sprintf(frame_name, "Landmark num %ld", img_desc.landmarkNum()); + cv::putText(ret, frame_name, cv::Point2f(10, 20), cv::FONT_HERSHEY_PLAIN, + 0.8, cv::Scalar(0, 255, 0)); + + cv::imshow(win_name, ret); + } + cv::waitKey(10); + } }; int main(int argc, char **argv) { - ROS_INFO("SWARM_LOOP INIT"); - srand(time(NULL)); + ROS_INFO("SWARM_LOOP INIT"); + srand(time(NULL)); - ros::init(argc, argv, "swarm_loop_spy"); - ros::NodeHandle nh("swarm_loop_spy"); - SwarmLoopSpy loopnode(nh); + ros::init(argc, argv, "swarm_loop_spy"); + ros::NodeHandle nh("swarm_loop_spy"); + SwarmLoopSpy loopnode(nh); - std::thread thread([&] { - while(0 == loopnode.loop_net->lcmHandle()) { - } - }); - ros::spin(); + std::thread thread([&] { + while (0 == loopnode.loop_net->lcmHandle()) { + } + }); + ros::spin(); - return 0; + return 0; } diff --git a/d2frontend/src/d2landmark_manager.cpp b/d2frontend/src/d2landmark_manager.cpp index 8ba813da..543d2ee8 100644 --- a/d2frontend/src/d2landmark_manager.cpp +++ b/d2frontend/src/d2landmark_manager.cpp @@ -1,94 +1,123 @@ -#include #include +#include namespace D2FrontEnd { - -int LandmarkManager::addLandmark(const LandmarkPerFrame & lm) { - auto _id = count + MAX_FEATURE_NUM*params->self_id; - count ++; - LandmarkPerFrame lm_copy = lm; - lm_copy.landmark_id = _id; - landmark_db[_id] = lm_copy; - related_landmarks[lm_copy.frame_id][_id] = related_landmarks[lm_copy.frame_id][_id] + 1; - total_lm_per_frame_num ++; - return _id; + +int LandmarkManager::addLandmark(const LandmarkPerFrame &lm) { + auto _id = count + MAX_FEATURE_NUM * params->self_id; + count++; + LandmarkPerFrame lm_copy = lm; + lm_copy.landmark_id = _id; + landmark_db[_id] = lm_copy; + related_landmarks[lm_copy.frame_id][_id] = + related_landmarks[lm_copy.frame_id][_id] + 1; + total_lm_per_frame_num++; + return _id; } -void LandmarkManager::updateLandmark(const LandmarkPerFrame & lm) { - if (lm.landmark_id < 0) { - return; - } - if (landmark_db.find(lm.landmark_id) == landmark_db.end()) { - landmark_db[lm.landmark_id] = lm; - } else { - landmark_db.at(lm.landmark_id).add(lm); - } - total_lm_per_frame_num ++; - related_landmarks[lm.frame_id][lm.landmark_id] = related_landmarks[lm.frame_id][lm.landmark_id] + 1; - assert(lm.landmark_id >= 0 && "landmark id must > 0"); +void LandmarkManager::updateLandmark(const LandmarkPerFrame &lm) { + if (lm.landmark_id < 0) { + return; + } + if (landmark_db.find(lm.landmark_id) == landmark_db.end()) { + landmark_db[lm.landmark_id] = lm; + } else { + landmark_db.at(lm.landmark_id).add(lm); + } + total_lm_per_frame_num++; + related_landmarks[lm.frame_id][lm.landmark_id] = + related_landmarks[lm.frame_id][lm.landmark_id] + 1; + assert(lm.landmark_id >= 0 && "landmark id must > 0"); } -void LandmarkManager::removeLandmark(const LandmarkIdType & id) { - landmark_db.erase(id); +void LandmarkManager::removeLandmark(const LandmarkIdType &id) { + landmark_db.erase(id); } -std::vector LandmarkManager::popFrame(FrameIdType frame_id, bool pop_base) { - const Guard lock(state_lock); - //Returning margined landmarks - std::vector margined_landmarks; - if (related_landmarks.find(frame_id) == related_landmarks.end()) { - return margined_landmarks; +std::vector LandmarkManager::popFrame(FrameIdType frame_id, + bool pop_base) { + const Guard lock(state_lock); + // Returning margined landmarks + std::vector margined_landmarks; + if (related_landmarks.find(frame_id) == related_landmarks.end()) { + return margined_landmarks; + } + auto _landmark_ids = related_landmarks[frame_id]; + for (auto it : _landmark_ids) { + auto _id = it.first; + if (landmark_db.find(_id) == landmark_db.end()) { + continue; } - auto _landmark_ids = related_landmarks[frame_id]; - for (auto it : _landmark_ids) { - auto _id = it.first; - if (landmark_db.find(_id) == landmark_db.end()) { - continue; - } - auto & lm = landmark_db.at(_id); - if (pop_base) { - if (related_landmarks.find(lm.base_frame_id)!=related_landmarks.end()) { - related_landmarks.at(lm.base_frame_id).erase(lm.landmark_id); - } - if (lm.base_frame_id != frame_id) { - // printf("[D2VINS::D2LandmarkManager] popFrame base_frame_id %ld != frame_id %ld\n", lm.base_frame_id, frame_id); - } - lm.popBaseFrame(); - } - auto _size = lm.popFrame(frame_id); - total_lm_per_frame_num --; - if (_size == 0) { - //Remove this landmark. - margined_landmarks.emplace_back(lm); - removeLandmark(_id); - } + auto &lm = landmark_db.at(_id); + if (pop_base) { + if (related_landmarks.find(lm.base_frame_id) != related_landmarks.end()) { + related_landmarks.at(lm.base_frame_id).erase(lm.landmark_id); + } + if (lm.base_frame_id != frame_id) { + // printf("[D2VINS::D2LandmarkManager] popFrame base_frame_id %ld != + // frame_id %ld\n", lm.base_frame_id, frame_id); + } + lm.popBaseFrame(); } - related_landmarks.erase(frame_id); - return margined_landmarks; + auto _size = lm.popFrame(frame_id); + total_lm_per_frame_num--; + if (_size == 0) { + // Remove this landmark. + margined_landmarks.emplace_back(lm); + removeLandmark(_id); + } + } + related_landmarks.erase(frame_id); + return margined_landmarks; } - -std::vector LandmarkManager::getInitializedLandmarks(int min_tracks) const { - const Guard lock(state_lock); - std::vector lm_per_frame_vec; - for (auto it : landmark_db) { - auto & lm = it.second; - if (lm.track.size() >= min_tracks&& lm.flag >= LandmarkFlag::INITIALIZED) { - lm_per_frame_vec.push_back(lm); - } +std::vector LandmarkManager::getInitializedLandmarks( + int min_tracks) const { + const Guard lock(state_lock); + std::vector lm_per_frame_vec; + for (auto it : landmark_db) { + auto &lm = it.second; + if (lm.track.size() >= min_tracks && lm.flag >= LandmarkFlag::INITIALIZED) { + lm_per_frame_vec.push_back(lm); } - return lm_per_frame_vec; + } + return lm_per_frame_vec; } bool LandmarkManager::hasLandmark(LandmarkIdType landmark_id) const { - const Guard lock(state_lock); - return landmark_db.find(landmark_id) != landmark_db.end(); + const Guard lock(state_lock); + return landmark_db.find(landmark_id) != landmark_db.end(); } +FrameIdType LandmarkManager::getLandmarkBaseFrame( + LandmarkIdType landmark_id) const { + const Guard lock(state_lock); + return landmark_db.at(landmark_id).track[0].frame_id; +} + +std::vector LandmarkManager::findCommonLandmarkIds( + FrameIdType frame_id1, FrameIdType frame_id2) const { + auto lm_last = getRelatedLandmarks(frame_id1); + auto lm_second_last = getRelatedLandmarks(frame_id2); + std::set common_lm; + std::set_intersection(lm_last.begin(), lm_last.end(), lm_second_last.begin(), + lm_second_last.end(), + std::inserter(common_lm, common_lm.begin())); + return std::vector(common_lm.begin(), common_lm.end()); +} -FrameIdType LandmarkManager::getLandmarkBaseFrame(LandmarkIdType landmark_id) const { - const Guard lock(state_lock); - return landmark_db.at(landmark_id).track[0].frame_id; +std::vector> +LandmarkManager::findCommonLandmarkPerFrames(FrameIdType frame_id1, + FrameIdType frame_id2) const { + auto lm_common = findCommonLandmarkIds(frame_id1, frame_id2); + std::vector> ret; + for (auto lm_id : lm_common) { + auto &lm = landmark_db.at(lm_id); + LandmarkPerFrame lm1 = lm.at(frame_id1); + LandmarkPerFrame lm2 = lm.at(frame_id2); + ret.emplace_back(lm1, lm2); + } + return ret; } -} \ No newline at end of file +} // namespace D2FrontEnd \ No newline at end of file diff --git a/d2frontend/src/loop_cam.cpp b/d2frontend/src/loop_cam.cpp index c194acb4..e4010335 100644 --- a/d2frontend/src/loop_cam.cpp +++ b/d2frontend/src/loop_cam.cpp @@ -1,572 +1,640 @@ -#include #include + +#include + #include -#include #include "opencv2/features2d.hpp" -#include +#include #include #include -#include +#include + + +#include #include +#include +#include +#include "d2frontend/CNN/superpoint.h" using namespace std::chrono; double TRIANGLE_THRES; namespace D2FrontEnd { -LoopCam::LoopCam(LoopCamConfig config, ros::NodeHandle &nh) : - camera_configuration(config.camera_configuration), - self_id(config.self_id), - _config(config) -{ - int img_width = config.enable_undistort_image ? params->width_undistort: params->width; - int img_height = config.enable_undistort_image ? params->height_undistort: params->height; - - if (config.cnn_use_onnx) { - printf("[D2FrontEnd::LoopCam] Init CNNs using onnx\n"); - netvlad_onnx = new MobileNetVLADONNX(config.netvlad_model, img_width, img_height, config.cnn_enable_tensorrt, - config.cnn_enable_tensorrt_fp16, config.cnn_enable_tensorrt_int8, config.netvlad_int8_calib_table_name); - superpoint_onnx = new SuperPointONNX(config.superpoint_model, ((int)(params->feature_min_dist/2)), config.pca_comp, - config.pca_mean, img_width, img_height, config.superpoint_thres, config.superpoint_max_num, - config.cnn_enable_tensorrt, config.cnn_enable_tensorrt_fp16, config.cnn_enable_tensorrt_int8, - config.superpoint_int8_calib_table_name); - } - undistortors = params->undistortors; - cams = params->camera_ptrs; - printf("[D2FrontEnd::LoopCam] Deepnet ready\n"); - if (_config.OUTPUT_RAW_SUPERPOINT_DESC) { - fsp.open(params->OUTPUT_PATH+"superpoint.csv", std::fstream::app); +LoopCam::LoopCam(LoopCamConfig config, ros::NodeHandle &nh) + : camera_configuration(config.camera_configuration), + self_id(config.self_id), + _config(config) { + int img_width = + config.enable_undistort_image ? params->width_undistort : params->width; + int img_height = + config.enable_undistort_image ? params->height_undistort : params->height; + + if (config.cnn_use_onnx) { + SPDLOG_INFO("Init CNNs using onnx"); + netvlad_onnx = new MobileNetVLADONNX( + config.netvlad_model, img_width, img_height, config.cnn_enable_tensorrt, + config.cnn_enable_tensorrt_fp16, config.cnn_enable_tensorrt_int8, + config.netvlad_int8_calib_table_name); + + SuperPoint::SuperPointConfig sp_config = config.superpoint_config; + superpoint_ptr = std::make_unique(sp_config); + if (superpoint_ptr->build()){ + SPDLOG_INFO("SuperPoint build success"); + } else { + SPDLOG_ERROR("SuperPoint build failed"); } + + + // superpoint_onnx = new SuperPointONNX( + // config.superpoint_model, ((int)(params->feature_min_dist / 2)), + // config.pca_comp, config.pca_mean, img_width, img_height, + // config.superpoint_thres, config.superpoint_max_num, + // config.cnn_enable_tensorrt, config.cnn_enable_tensorrt_fp16, + // config.cnn_enable_tensorrt_int8, + // config.superpoint_int8_calib_table_name); + + + } + undistortors = params->undistortors; + cams = params->camera_ptrs; + SPDLOG_INFO("Deepnet ready"); + if (_config.OUTPUT_RAW_SUPERPOINT_DESC) { + fsp.open(params->OUTPUT_PATH + "superpoint.csv", std::fstream::app); + } } -void LoopCam::encodeImage(const cv::Mat &_img, VisualImageDesc &_img_desc) -{ - std::vector jpg_params; - jpg_params.push_back(cv::IMWRITE_JPEG_QUALITY); - jpg_params.push_back(params->JPG_QUALITY); - - cv::imencode(".jpg", _img, _img_desc.image, jpg_params); - _img_desc.image_width = _img.cols; - _img_desc.image_width = _img.rows; - // std::cout << "IMENCODE Cost " << duration_cast(high_resolution_clock::now() - start).count()/1000.0 << "ms" << std::endl; - // std::cout << "JPG SIZE" << _img_desc.image.size() << std::endl; +void LoopCam::encodeImage(const cv::Mat &_img, VisualImageDesc &_img_desc) { + std::vector jpg_params; + jpg_params.push_back(cv::IMWRITE_JPEG_QUALITY); + jpg_params.push_back(params->JPG_QUALITY); + + cv::imencode(".jpg", _img, _img_desc.image, jpg_params); + _img_desc.image_width = _img.cols; + _img_desc.image_width = _img.rows; + // std::cout << "IMENCODE Cost " << + // duration_cast(high_resolution_clock::now() - + // start).count()/1000.0 << "ms" << std::endl; std::cout << "JPG SIZE" << + // _img_desc.image.size() << std::endl; } -double triangulatePoint(Eigen::Quaterniond q0, Eigen::Vector3d t0, Eigen::Quaterniond q1, Eigen::Vector3d t1, - Eigen::Vector2d point0, Eigen::Vector2d point1, Eigen::Vector3d &point_3d) -{ - Eigen::Matrix3d R0 = q0.toRotationMatrix(); - Eigen::Matrix3d R1 = q1.toRotationMatrix(); - - // std::cout << "RO" << R0 << "T0" << t0.transpose() << std::endl; - // std::cout << "R1" << R1 << "T1" << t1.transpose() << std::endl; - - Eigen::Matrix Pose0; - Pose0.leftCols<3>() = R0.transpose(); - Pose0.rightCols<1>() = -R0.transpose() * t0; - - Eigen::Matrix Pose1; - Pose1.leftCols<3>() = R1.transpose(); - Pose1.rightCols<1>() = -R1.transpose() * t1; - - Eigen::Matrix4d design_matrix = Eigen::Matrix4d::Zero(); - design_matrix.row(0) = point0[0] * Pose0.row(2) - Pose0.row(0); - design_matrix.row(1) = point0[1] * Pose0.row(2) - Pose0.row(1); - design_matrix.row(2) = point1[0] * Pose1.row(2) - Pose1.row(0); - design_matrix.row(3) = point1[1] * Pose1.row(2) - Pose1.row(1); - Eigen::Vector4d triangulated_point; - triangulated_point = - design_matrix.jacobiSvd(Eigen::ComputeFullV).matrixV().rightCols<1>(); - point_3d(0) = triangulated_point(0) / triangulated_point(3); - point_3d(1) = triangulated_point(1) / triangulated_point(3); - point_3d(2) = triangulated_point(2) / triangulated_point(3); - - Eigen::MatrixXd pts(4, 1); - pts << point_3d.x(), point_3d.y(), point_3d.z(), 1; - Eigen::MatrixXd errs = design_matrix*pts; - return errs.norm()/ errs.rows(); +double triangulatePoint(Eigen::Quaterniond q0, Eigen::Vector3d t0, + Eigen::Quaterniond q1, Eigen::Vector3d t1, + Eigen::Vector2d point0, Eigen::Vector2d point1, + Eigen::Vector3d &point_3d) { + Eigen::Matrix3d R0 = q0.toRotationMatrix(); + Eigen::Matrix3d R1 = q1.toRotationMatrix(); + + // std::cout << "RO" << R0 << "T0" << t0.transpose() << std::endl; + // std::cout << "R1" << R1 << "T1" << t1.transpose() << std::endl; + + Eigen::Matrix Pose0; + Pose0.leftCols<3>() = R0.transpose(); + Pose0.rightCols<1>() = -R0.transpose() * t0; + + Eigen::Matrix Pose1; + Pose1.leftCols<3>() = R1.transpose(); + Pose1.rightCols<1>() = -R1.transpose() * t1; + + Eigen::Matrix4d design_matrix = Eigen::Matrix4d::Zero(); + design_matrix.row(0) = point0[0] * Pose0.row(2) - Pose0.row(0); + design_matrix.row(1) = point0[1] * Pose0.row(2) - Pose0.row(1); + design_matrix.row(2) = point1[0] * Pose1.row(2) - Pose1.row(0); + design_matrix.row(3) = point1[1] * Pose1.row(2) - Pose1.row(1); + Eigen::Vector4d triangulated_point; + triangulated_point = + design_matrix.jacobiSvd(Eigen::ComputeFullV).matrixV().rightCols<1>(); + point_3d(0) = triangulated_point(0) / triangulated_point(3); + point_3d(1) = triangulated_point(1) / triangulated_point(3); + point_3d(2) = triangulated_point(2) / triangulated_point(3); + + Eigen::MatrixXd pts(4, 1); + pts << point_3d.x(), point_3d.y(), point_3d.z(), 1; + Eigen::MatrixXd errs = design_matrix * pts; + return errs.norm() / errs.rows(); } template -void reduceVector(std::vector &v, std::vector status) -{ - int j = 0; - for (int i = 0; i < int(v.size()); i++) - if (status[i]) - v[j++] = v[i]; - v.resize(j); +void reduceVector(std::vector &v, std::vector status) { + int j = 0; + for (int i = 0; i < int(v.size()); i++) + if (status[i]) v[j++] = v[i]; + v.resize(j); } -cv::Mat drawMatches(std::vector pts1, std::vector pts2, std::vector _matches, const cv::Mat & up, const cv::Mat & down) { - std::vector kps1; - std::vector kps2; +cv::Mat drawMatches(std::vector pts1, + std::vector pts2, + std::vector _matches, const cv::Mat &up, + const cv::Mat &down) { + std::vector kps1; + std::vector kps2; - for (auto pt : pts1) { - cv::KeyPoint kp; - kp.pt = pt; - kps1.push_back(kp); - } + for (auto pt : pts1) { + cv::KeyPoint kp; + kp.pt = pt; + kps1.push_back(kp); + } - for (auto pt : pts2) { - cv::KeyPoint kp; - kp.pt = pt; - kps2.push_back(kp); - } + for (auto pt : pts2) { + cv::KeyPoint kp; + kp.pt = pt; + kps2.push_back(kp); + } - cv::Mat _show; + cv::Mat _show; - cv::drawMatches(up, kps1, down, kps2, _matches, _show); + cv::drawMatches(up, kps1, down, kps2, _matches, _show); - return _show; + return _show; } -void matchLocalFeatures(std::vector & pts_up, std::vector & pts_down, - std::vector & _desc_up, std::vector & _desc_down, - std::vector & ids_up, std::vector & ids_down) { - // printf("matchLocalFeatures %ld %ld: ", pts_up.size(), pts_down.size()); - const cv::Mat desc_up( _desc_up.size()/params->superpoint_dims, params->superpoint_dims, CV_32F, _desc_up.data()); - const cv::Mat desc_down( _desc_down.size()/params->superpoint_dims, params->superpoint_dims, CV_32F, _desc_down.data()); - - cv::BFMatcher bfmatcher(cv::NORM_L2, true); - - std::vector _matches; - bfmatcher.match(desc_up, desc_down, _matches); - - std::vector _pts_up, _pts_down; - std::vector ids; - for (auto match : _matches) { - if (match.distance < ACCEPT_SP_MATCH_DISTANCE || true) - { - int now_id = match.queryIdx; - int old_id = match.trainIdx; - _pts_up.push_back(pts_up[now_id]); - _pts_down.push_back(pts_down[old_id]); - ids_up.push_back(now_id); - ids_down.push_back(old_id); - } else { - std::cout << "Giveup match dis" << match.distance << std::endl; - } +void matchLocalFeatures(std::vector &pts_up, + std::vector &pts_down, + std::vector &_desc_up, + std::vector &_desc_down, + std::vector &ids_up, std::vector &ids_down) { + // printf("matchLocalFeatures %ld %ld: ", pts_up.size(), pts_down.size()); + const cv::Mat desc_up(_desc_up.size() / params->superpoint_dims, + params->superpoint_dims, CV_32F, _desc_up.data()); + const cv::Mat desc_down(_desc_down.size() / params->superpoint_dims, + params->superpoint_dims, CV_32F, _desc_down.data()); + + cv::BFMatcher bfmatcher(cv::NORM_L2, true); + + std::vector _matches; + bfmatcher.match(desc_up, desc_down, _matches); + + std::vector _pts_up, _pts_down; + std::vector ids; + for (auto match : _matches) { + if (match.distance < ACCEPT_SP_MATCH_DISTANCE || true) { + int now_id = match.queryIdx; + int old_id = match.trainIdx; + _pts_up.push_back(pts_up[now_id]); + _pts_down.push_back(pts_down[old_id]); + ids_up.push_back(now_id); + ids_down.push_back(old_id); + } else { + std::cout << "Giveup match dis" << match.distance << std::endl; } + } - std::vector status; + std::vector status; - pts_up = std::vector(_pts_up); - pts_down = std::vector(_pts_down); + pts_up = std::vector(_pts_up); + pts_down = std::vector(_pts_down); } -VisualImageDescArray LoopCam::processStereoframe(const StereoFrame & msg) { - VisualImageDescArray visual_array; - visual_array.stamp = msg.stamp.toSec(); - - cv::Mat _show, tmp; - TicToc tt; - static int t_count = 0; - static double tt_sum = 0; - - if (camera_configuration == CameraConfig::FOURCORNER_FISHEYE) { - visual_array.images.resize(4); - } - - for (unsigned int i = 0; i < msg.left_images.size(); i ++) { - if (camera_configuration == CameraConfig::PINHOLE_DEPTH) { - visual_array.images.push_back(generateGrayDepthImageDescriptor(msg, i, tmp)); - } else if (camera_configuration == CameraConfig::STEREO_PINHOLE) { - auto _imgs = generateStereoImageDescriptor(msg, i, tmp); - if (_config.stereo_as_depth_cam) { - if (_config.right_cam_as_main) { - visual_array.images.push_back(_imgs[1]); - } else { - visual_array.images.push_back(_imgs[0]); - } - } else { - visual_array.images.push_back(_imgs[0]); - visual_array.images.push_back(_imgs[1]); - } - } else if (camera_configuration == CameraConfig::FOURCORNER_FISHEYE) { - auto seq = params->camera_seq[i]; - visual_array.images[seq] = generateImageDescriptor(msg, i, tmp); - } - - if (_show.cols == 0) { - _show = tmp; +VisualImageDescArray LoopCam::processStereoframe(const StereoFrame &msg) { + VisualImageDescArray visual_array; + visual_array.stamp = msg.stamp.toSec(); + + cv::Mat _show, tmp; + TicToc tt; + static int t_count = 0; + static double tt_sum = 0; + + if (camera_configuration == CameraConfig::FOURCORNER_FISHEYE) { + visual_array.images.resize(4); + } + + for (unsigned int i = 0; i < msg.left_images.size(); i++) { + if (camera_configuration == CameraConfig::PINHOLE_DEPTH) { + visual_array.images.push_back( + generateGrayDepthImageDescriptor(msg, i, tmp)); + } else if (camera_configuration == CameraConfig::STEREO_PINHOLE) { + auto _imgs = generateStereoImageDescriptor(msg, i, tmp); + if (_config.stereo_as_depth_cam) { + if (_config.right_cam_as_main) { + visual_array.images.push_back(_imgs[1]); } else { - cv::hconcat(_show, tmp, _show); + visual_array.images.push_back(_imgs[0]); } + } else { + visual_array.images.push_back(_imgs[0]); + visual_array.images.push_back(_imgs[1]); + } + } else if (camera_configuration == CameraConfig::FOURCORNER_FISHEYE) { + auto seq = params->camera_seq[i]; + visual_array.images[seq] = generateImageDescriptor(msg, i, tmp); } - tt_sum+= tt.toc(); - t_count+= 1; - printf("[D2Frontend::LoopCam] KF Count %d loop_cam cost avg %.1fms cur %.1fms\n", kf_count, tt_sum/t_count, tt.toc()); - - visual_array.frame_id = msg.keyframe_id; - visual_array.pose_drone = msg.pose_drone; - visual_array.drone_id = self_id; - - if (_config.show && !_show.empty()) { - char text[100] = {0}; - char PATH[100] = {0}; - sprintf(text, "FEATURES@Drone%d", self_id); - sprintf(PATH, "loop/features%d.png", kf_count); - cv::imshow(text, _show); - cv::imwrite(params->OUTPUT_PATH+PATH, _show); - cv::waitKey(10); + if (_show.cols == 0) { + _show = tmp; + } else { + cv::hconcat(_show, tmp, _show); } - kf_count ++; - visual_array.sync_landmark_ids(); - return visual_array; + } + + tt_sum += tt.toc(); + t_count += 1; + spdlog::debug("KF Count {} loop_cam cost avg {:.1f}ms cur {:.1f}ms", kf_count, + tt_sum / t_count, tt.toc()); + + visual_array.frame_id = msg.keyframe_id; + visual_array.pose_drone = msg.pose_drone; + visual_array.drone_id = self_id; + + if (_config.show && !_show.empty()) { + char text[100] = {0}; + char PATH[100] = {0}; + sprintf(text, "FEATURES@Drone%d", self_id); + sprintf(PATH, "loop/features%d.png", kf_count); + cv::imshow(text, _show); + cv::imwrite(params->OUTPUT_PATH + PATH, _show); + cv::waitKey(10); + } + kf_count++; + visual_array.sync_landmark_ids(); + return visual_array; } -VisualImageDesc LoopCam::generateImageDescriptor(const StereoFrame & msg, int vcam_id, cv::Mat &_show) { - if (vcam_id > msg.left_images.size()) { - ROS_WARN("Flatten images too few"); - VisualImageDesc ides; - ides.stamp = msg.stamp.toSec(); - return ides; - } - cv::Mat undist = msg.left_images[vcam_id]; - TicToc tt; - if (_config.enable_undistort_image) { - undist = cv::Mat(undistortors[vcam_id]->undist_id_cuda(undist, 0, true)); +VisualImageDesc LoopCam::generateImageDescriptor(const StereoFrame &msg, + int vcam_id, cv::Mat &_show) { + if (vcam_id > msg.left_images.size()) { + SPDLOG_WARN("Flatten images too few"); + VisualImageDesc ides; + ides.stamp = msg.stamp.toSec(); + return ides; + } + cv::Mat undist = msg.left_images[vcam_id]; + TicToc tt; + if (_config.enable_undistort_image) { + undist = cv::Mat(undistortors[vcam_id]->undist_id_cuda(undist, 0, true)); + } + if (params->enable_perf_output) { + SPDLOG_INFO("[D2Frontend::LoopCam] undist image cost {:.1f}ms", tt.toc()); + } + VisualImageDesc vframe = extractorImgDescDeepnet( + msg.stamp, undist, msg.left_camera_indices[vcam_id], + msg.left_camera_ids[vcam_id], false); + + if (vframe.image_desc.size() == 0) { + SPDLOG_WARN("Failed on deepnet: vframe.image_desc.size() == 0."); + cv::Mat _img; + // return ides; + } + + auto start = high_resolution_clock::now(); + // std::cout << "Downsample and encode Cost " << + // duration_cast(high_resolution_clock::now() - + // start).count()/1000.0 << "ms" << std::endl; + + vframe.extrinsic = msg.left_extrisincs[vcam_id]; + vframe.pose_drone = msg.pose_drone; + vframe.frame_id = msg.keyframe_id; + if (params->debug_plot_superpoint_features || + params->ftconfig->enable_lk_optical_flow || params->show) { + vframe.raw_image = undist; + } + + auto image_left = undist; + auto pts_up = vframe.landmarks2D(); + std::vector ids_up, ids_down; + if (_config.send_img) { + encodeImage(image_left, vframe); + } + if (_config.show) { + cv::Mat img_up = image_left; + if (!_config.send_img) { + encodeImage(img_up, vframe); } - if (params->enable_perf_output) { - printf("[D2Frontend::LoopCam] undist image cost %.1fms\n", tt.toc()); - } - VisualImageDesc vframe = extractorImgDescDeepnet(msg.stamp, undist, msg.left_camera_indices[vcam_id], msg.left_camera_ids[vcam_id], false); - - if (vframe.image_desc.size() == 0) - { - ROS_WARN("Failed on deepnet: vframe.image_desc.size() == 0."); - cv::Mat _img; - // return ides; + cv::cvtColor(img_up, img_up, cv::COLOR_GRAY2BGR); + for (unsigned int i = 0; i < vframe.landmarkNum(); i++) { + if (vframe.landmarks[i].depth_mea) { + auto pt = vframe.landmarks[i].pt2d; + auto dep = vframe.landmarks[i].depth; + cv::circle(img_up, pt, 3, cv::Scalar(0, 255, 0), 1); + char idtext[100] = {}; + sprintf(idtext, "%3.2f", dep); + cv::putText(img_up, idtext, pt - cv::Point2f(5, 0), + cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 255, 255), 1); + } } - - auto start = high_resolution_clock::now(); - // std::cout << "Downsample and encode Cost " << duration_cast(high_resolution_clock::now() - start).count()/1000.0 << "ms" << std::endl; - - vframe.extrinsic = msg.left_extrisincs[vcam_id]; - vframe.pose_drone = msg.pose_drone; - vframe.frame_id = msg.keyframe_id; - if (params->debug_plot_superpoint_features || params->ftconfig->enable_lk_optical_flow || params->show) { - vframe.raw_image = undist; - } - - auto image_left = undist; - auto pts_up = vframe.landmarks2D(); - std::vector ids_up, ids_down; - if (_config.send_img) { - encodeImage(image_left, vframe); - } - if (_config.show) { - cv::Mat img_up = image_left; - if (!_config.send_img) { - encodeImage(img_up, vframe); - } - cv::cvtColor(img_up, img_up, cv::COLOR_GRAY2BGR); - for (unsigned int i = 0; i < vframe.landmarkNum(); i++ ) { - if (vframe.landmarks[i].depth_mea) { - auto pt = vframe.landmarks[i].pt2d; - auto dep = vframe.landmarks[i].depth; - cv::circle(img_up, pt, 3, cv::Scalar(0, 255, 0), 1); - char idtext[100] = {}; - sprintf(idtext, "%3.2f", dep); - cv::putText(img_up, idtext, pt - cv::Point2f(5, 0), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 255, 255), 1); - } - } - _show = img_up; - char text[100] = {0}; - sprintf(text, "Frame %d: %ld Features %d", kf_count, msg.keyframe_id, pts_up.size()); - cv::putText(_show, text, cv::Point2f(20, 30), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 255, 0), 1.5); - } - return vframe; + _show = img_up; + char text[100] = {0}; + sprintf(text, "Frame %d: %ld Features %d", kf_count, msg.keyframe_id, + pts_up.size()); + cv::putText(_show, text, cv::Point2f(20, 30), cv::FONT_HERSHEY_SIMPLEX, 0.5, + cv::Scalar(0, 255, 0), 1.5); + } + return vframe; } -VisualImageDesc LoopCam::generateGrayDepthImageDescriptor(const StereoFrame & msg, int vcam_id, cv::Mat & _show) -{ - if (vcam_id > msg.left_images.size()) { - ROS_WARN("Flatten images too few"); - VisualImageDesc ides; - ides.stamp = msg.stamp.toSec(); - return ides; +VisualImageDesc LoopCam::generateGrayDepthImageDescriptor( + const StereoFrame &msg, int vcam_id, cv::Mat &_show) { + if (vcam_id > msg.left_images.size()) { + SPDLOG_WARN("Flatten images too few"); + VisualImageDesc ides; + ides.stamp = msg.stamp.toSec(); + return ides; + } + + VisualImageDesc vframe = extractorImgDescDeepnet( + msg.stamp, msg.left_images[vcam_id], msg.left_camera_indices[vcam_id], + msg.left_camera_ids[vcam_id], false); + + if (vframe.image_desc.size() == 0) { + SPDLOG_WARN("Failed on deepnet: vframe.image_desc.size() == 0."); + cv::Mat _img; + // return ides; + } + + auto start = high_resolution_clock::now(); + // std::cout << "Downsample and encode Cost " << + // duration_cast(high_resolution_clock::now() - + // start).count()/1000.0 << "ms" << std::endl; + + vframe.extrinsic = msg.left_extrisincs[vcam_id]; + vframe.pose_drone = msg.pose_drone; + vframe.frame_id = msg.keyframe_id; + if (params->debug_plot_superpoint_features || + params->ftconfig->enable_lk_optical_flow || params->show) { + vframe.raw_image = msg.left_images[vcam_id]; + vframe.raw_depth_image = msg.depth_images[vcam_id]; + } + + auto image_left = msg.left_images[vcam_id]; + auto pts_up = vframe.landmarks2D(); + std::vector ids_up, ids_down; + Swarm::Pose pose_drone(msg.pose_drone); + Swarm::Pose pose_cam = pose_drone * Swarm::Pose(msg.left_extrisincs[vcam_id]); + + std::vector desc_new; + + int count_3d = 0; + for (unsigned int i = 0; i < pts_up.size(); i++) { + cv::Point2f pt_up = pts_up[i]; + if (pt_up.x < 0 || pt_up.x >= params->width || pt_up.y < 0 || + pt_up.y >= params->height) { + continue; } - - VisualImageDesc vframe = extractorImgDescDeepnet(msg.stamp, msg.left_images[vcam_id], msg.left_camera_indices[vcam_id], msg.left_camera_ids[vcam_id], false); - if (vframe.image_desc.size() == 0) - { - ROS_WARN("Failed on deepnet: vframe.image_desc.size() == 0."); - cv::Mat _img; - // return ides; + auto dep = msg.depth_images[vcam_id].at(pt_up) / 1000.0; + if (dep > _config.DEPTH_NEAR_THRES && dep < _config.DEPTH_FAR_THRES) { + Eigen::Vector3d pt_up3d, pt_down3d; + cams.at(vcam_id)->liftProjective(Vector2d(pt_up.x, pt_up.y), pt_up3d); + + Eigen::Vector3d pt2d_norm(pt_up3d.x() / pt_up3d.z(), + pt_up3d.y() / pt_up3d.z(), 1); + auto pt3dcam = pt2d_norm * dep; + Eigen::Vector3d pt3d = pose_cam * pt3dcam; + // printf("landmark raw depth %f pt3dcam %f %f %f pt2d_norm %f %f distance + // %f\n", dep, + // pt3dcam.x(), pt3dcam.y(), pt3dcam.z(), pt2d_norm.x(), + // pt2d_norm.y(), pt3dcam.norm()); + + vframe.landmarks[i].pt3d = pt3d; + vframe.landmarks[i].flag = LandmarkFlag::UNINITIALIZED; + vframe.landmarks[i].depth = pt3dcam.norm(); + vframe.landmarks[i].depth_mea = true; + count_3d++; } + } - auto start = high_resolution_clock::now(); - // std::cout << "Downsample and encode Cost " << duration_cast(high_resolution_clock::now() - start).count()/1000.0 << "ms" << std::endl; + // ROS_INFO("Image 2d kpts: %ld 3d : %d desc size %ld", + // ides.landmarks_2d.size(), count_3d, ides.feature_descriptor.size()); - vframe.extrinsic = msg.left_extrisincs[vcam_id]; - vframe.pose_drone = msg.pose_drone; - vframe.frame_id = msg.keyframe_id; - if (params->debug_plot_superpoint_features || params->ftconfig->enable_lk_optical_flow || params->show) { - vframe.raw_image = msg.left_images[vcam_id]; - vframe.raw_depth_image = msg.depth_images[vcam_id]; - } + if (_config.send_img) { + encodeImage(image_left, vframe); + } - auto image_left = msg.left_images[vcam_id]; - auto pts_up = vframe.landmarks2D(); - std::vector ids_up, ids_down; - Swarm::Pose pose_drone(msg.pose_drone); - Swarm::Pose pose_cam = pose_drone * Swarm::Pose(msg.left_extrisincs[vcam_id]); - - std::vector desc_new; + if (_config.show) { + cv::Mat img_up = image_left; - int count_3d = 0; - for (unsigned int i = 0; i < pts_up.size(); i++) - { - cv::Point2f pt_up = pts_up[i]; - if (pt_up.x < 0 || pt_up.x >= params->width || pt_up.y < 0 || pt_up.y >= params->height) { - continue; - } - - auto dep = msg.depth_images[vcam_id].at(pt_up)/1000.0; - if (dep > _config.DEPTH_NEAR_THRES && dep < _config.DEPTH_FAR_THRES) { - Eigen::Vector3d pt_up3d, pt_down3d; - cams.at(vcam_id)->liftProjective(Vector2d(pt_up.x, pt_up.y), pt_up3d); - - Eigen::Vector3d pt2d_norm(pt_up3d.x()/pt_up3d.z(), pt_up3d.y()/pt_up3d.z(), 1); - auto pt3dcam = pt2d_norm*dep; - Eigen::Vector3d pt3d = pose_cam * pt3dcam; - // printf("landmark raw depth %f pt3dcam %f %f %f pt2d_norm %f %f distance %f\n", dep, - // pt3dcam.x(), pt3dcam.y(), pt3dcam.z(), pt2d_norm.x(), pt2d_norm.y(), pt3dcam.norm()); - - vframe.landmarks[i].pt3d = pt3d; - vframe.landmarks[i].flag = LandmarkFlag::UNINITIALIZED; - vframe.landmarks[i].depth = pt3dcam.norm(); - vframe.landmarks[i].depth_mea = true; - count_3d ++; - } + if (!_config.send_img) { + encodeImage(img_up, vframe); } - // ROS_INFO("Image 2d kpts: %ld 3d : %d desc size %ld", ides.landmarks_2d.size(), count_3d, ides.feature_descriptor.size()); - - if (_config.send_img) { - encodeImage(image_left, vframe); + cv::cvtColor(img_up, img_up, cv::COLOR_GRAY2BGR); + + for (unsigned int i = 0; i < vframe.landmarkNum(); i++) { + if (vframe.landmarks[i].depth_mea) { + auto pt = vframe.landmarks[i].pt2d; + auto dep = vframe.landmarks[i].depth; + cv::circle(img_up, pt, 3, cv::Scalar(0, 255, 0), 1); + char idtext[100] = {}; + sprintf(idtext, "%3.2f", dep); + cv::putText(img_up, idtext, pt - cv::Point2f(5, 0), + cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 255, 255), 1); + } } - if (_config.show) { - cv::Mat img_up = image_left; + _show = img_up; + char text[100] = {0}; + sprintf(text, "Frame %d: %ld Features %d/%d", kf_count, msg.keyframe_id, + count_3d, pts_up.size()); + cv::putText(_show, text, cv::Point2f(20, 30), cv::FONT_HERSHEY_SIMPLEX, 0.5, + cv::Scalar(0, 255, 0), 1.5); + } - if (!_config.send_img) { - encodeImage(img_up, vframe); - } - - cv::cvtColor(img_up, img_up, cv::COLOR_GRAY2BGR); - - for (unsigned int i = 0; i < vframe.landmarkNum(); i++ ) { - if (vframe.landmarks[i].depth_mea) { - auto pt = vframe.landmarks[i].pt2d; - auto dep = vframe.landmarks[i].depth; - cv::circle(img_up, pt, 3, cv::Scalar(0, 255, 0), 1); - char idtext[100] = {}; - sprintf(idtext, "%3.2f", dep); - cv::putText(img_up, idtext, pt - cv::Point2f(5, 0), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 255, 255), 1); - } - } - - _show = img_up; - char text[100] = {0}; - sprintf(text, "Frame %d: %ld Features %d/%d", kf_count, msg.keyframe_id, count_3d, pts_up.size()); - cv::putText(_show, text, cv::Point2f(20, 30), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 255, 0), 1.5); - } - - return vframe; + return vframe; } -std::vector LoopCam::generateStereoImageDescriptor(const StereoFrame & msg, int vcam_id, cv::Mat & _show) -{ - //This function currently only support pinhole-like stereo camera. - auto vframe0 = extractorImgDescDeepnet(msg.stamp, msg.left_images[vcam_id], msg.left_camera_indices[vcam_id], - msg.left_camera_ids[vcam_id], _config.right_cam_as_main); - auto vframe1 = extractorImgDescDeepnet(msg.stamp, msg.right_images[vcam_id], msg.right_camera_indices[vcam_id], - msg.right_camera_ids[vcam_id], !_config.right_cam_as_main); - - if (vframe0.image_desc.size() == 0 && vframe1.image_desc.size() == 0) - { - ROS_WARN("Failed on deepnet: vframe0.image_desc.size() == 0 && vframe1.image_desc.size() == 0"); - // cv::Mat _img; - // return ides; +std::vector LoopCam::generateStereoImageDescriptor( + const StereoFrame &msg, int vcam_id, cv::Mat &_show) { + // This function currently only support pinhole-like stereo camera. + auto vframe0 = extractorImgDescDeepnet( + msg.stamp, msg.left_images[vcam_id], msg.left_camera_indices[vcam_id], + msg.left_camera_ids[vcam_id], _config.right_cam_as_main); + auto vframe1 = extractorImgDescDeepnet( + msg.stamp, msg.right_images[vcam_id], msg.right_camera_indices[vcam_id], + msg.right_camera_ids[vcam_id], !_config.right_cam_as_main); + + if (vframe0.image_desc.size() == 0 && vframe1.image_desc.size() == 0) { + SPDLOG_WARN( + "Failed on deepnet: vframe0.image_desc.size() == 0 && " + "vframe1.image_desc.size() == 0"); + // cv::Mat _img; + // return ides; + } + + auto start = high_resolution_clock::now(); + // std::cout << "Downsample and encode Cost " << + // duration_cast(high_resolution_clock::now() - + // start).count()/1000.0 << "ms" << std::endl; + + vframe0.extrinsic = msg.left_extrisincs[vcam_id]; + vframe0.pose_drone = msg.pose_drone; + vframe0.frame_id = msg.keyframe_id; + + vframe1.extrinsic = msg.right_extrisincs[vcam_id]; + vframe1.pose_drone = msg.pose_drone; + vframe1.frame_id = msg.keyframe_id; + + auto image_left = msg.left_images[vcam_id]; + auto image_right = msg.right_images[vcam_id]; + + auto pts_up = vframe0.landmarks2D(); + auto pts_down = vframe1.landmarks2D(); + std::vector ids_up, ids_down; + + int count_3d = 0; + if (_config.stereo_as_depth_cam) { + if (vframe0.landmarkNum() > _config.ACCEPT_MIN_3D_PTS) { + matchLocalFeatures(pts_up, pts_down, vframe0.landmark_descriptor, + vframe1.landmark_descriptor, ids_up, ids_down); } - - auto start = high_resolution_clock::now(); - // std::cout << "Downsample and encode Cost " << duration_cast(high_resolution_clock::now() - start).count()/1000.0 << "ms" << std::endl; - - vframe0.extrinsic = msg.left_extrisincs[vcam_id]; - vframe0.pose_drone = msg.pose_drone; - vframe0.frame_id = msg.keyframe_id; - - vframe1.extrinsic = msg.right_extrisincs[vcam_id]; - vframe1.pose_drone = msg.pose_drone; - vframe1.frame_id = msg.keyframe_id; - - auto image_left = msg.left_images[vcam_id]; - auto image_right = msg.right_images[vcam_id]; - - auto pts_up = vframe0.landmarks2D(); - auto pts_down = vframe1.landmarks2D(); - std::vector ids_up, ids_down; - - int count_3d = 0; - if (_config.stereo_as_depth_cam) { - if (vframe0.landmarkNum() > _config.ACCEPT_MIN_3D_PTS) { - matchLocalFeatures(pts_up, pts_down, vframe0.landmark_descriptor, vframe1.landmark_descriptor, ids_up, ids_down); - } - Swarm::Pose pose_drone(msg.pose_drone); - Swarm::Pose pose_up = pose_drone * Swarm::Pose(msg.left_extrisincs[vcam_id]); - Swarm::Pose pose_down = pose_drone * Swarm::Pose(msg.right_extrisincs[vcam_id]); - for (unsigned int i = 0; i < pts_up.size(); i++) { - auto pt_up = pts_up[i]; - auto pt_down = pts_down[i]; - - Eigen::Vector3d pt_up3d, pt_down3d; - //TODO: This may not work for stereo fisheye. Pending to update. - cams.at(msg.left_camera_indices[vcam_id])->liftProjective(Eigen::Vector2d(pt_up.x, pt_up.y), pt_up3d); - cams.at(msg.right_camera_indices[vcam_id])->liftProjective(Eigen::Vector2d(pt_down.x, pt_down.y), pt_down3d); - - Eigen::Vector2d pt_up_norm(pt_up3d.x()/pt_up3d.z(), pt_up3d.y()/pt_up3d.z()); - Eigen::Vector2d pt_down_norm(pt_down3d.x()/pt_down3d.z(), pt_down3d.y()/pt_down3d.z()); - - Eigen::Vector3d point_3d; - double err = triangulatePoint(pose_up.att(), pose_up.pos(), pose_down.att(), pose_down.pos(), - pt_up_norm, pt_down_norm, point_3d); - - auto pt_cam = pose_up.att().inverse() * (point_3d - pose_up.pos()); - - if (err > TRIANGLE_THRES) { - continue; - } - - int idx = ids_up[i]; - int idx_down = ids_down[i]; - vframe0.landmarks[idx].pt3d = point_3d; - vframe0.landmarks[idx].depth_mea = true; - vframe0.landmarks[idx].depth = pt_cam.norm(); - vframe0.landmarks[idx].flag = LandmarkFlag::UNINITIALIZED; - - auto pt_cam2 = pose_down.att().inverse() * (point_3d - pose_down.pos()); - vframe1.landmarks[idx_down].pt3d = point_3d; - vframe0.landmarks[idx].depth_mea = true; - vframe0.landmarks[idx].depth = pt_cam2.norm(); - vframe1.landmarks[idx_down].flag = LandmarkFlag::UNINITIALIZED; - count_3d ++; - } + Swarm::Pose pose_drone(msg.pose_drone); + Swarm::Pose pose_up = + pose_drone * Swarm::Pose(msg.left_extrisincs[vcam_id]); + Swarm::Pose pose_down = + pose_drone * Swarm::Pose(msg.right_extrisincs[vcam_id]); + for (unsigned int i = 0; i < pts_up.size(); i++) { + auto pt_up = pts_up[i]; + auto pt_down = pts_down[i]; + + Eigen::Vector3d pt_up3d, pt_down3d; + // TODO: This may not work for stereo fisheye. Pending to update. + cams.at(msg.left_camera_indices[vcam_id]) + ->liftProjective(Eigen::Vector2d(pt_up.x, pt_up.y), pt_up3d); + cams.at(msg.right_camera_indices[vcam_id]) + ->liftProjective(Eigen::Vector2d(pt_down.x, pt_down.y), pt_down3d); + + Eigen::Vector2d pt_up_norm(pt_up3d.x() / pt_up3d.z(), + pt_up3d.y() / pt_up3d.z()); + Eigen::Vector2d pt_down_norm(pt_down3d.x() / pt_down3d.z(), + pt_down3d.y() / pt_down3d.z()); + + Eigen::Vector3d point_3d; + double err = + triangulatePoint(pose_up.att(), pose_up.pos(), pose_down.att(), + pose_down.pos(), pt_up_norm, pt_down_norm, point_3d); + + auto pt_cam = pose_up.att().inverse() * (point_3d - pose_up.pos()); + + if (err > TRIANGLE_THRES) { + continue; + } + + int idx = ids_up[i]; + int idx_down = ids_down[i]; + vframe0.landmarks[idx].pt3d = point_3d; + vframe0.landmarks[idx].depth_mea = true; + vframe0.landmarks[idx].depth = pt_cam.norm(); + vframe0.landmarks[idx].flag = LandmarkFlag::UNINITIALIZED; + + auto pt_cam2 = pose_down.att().inverse() * (point_3d - pose_down.pos()); + vframe1.landmarks[idx_down].pt3d = point_3d; + vframe0.landmarks[idx].depth_mea = true; + vframe0.landmarks[idx].depth = pt_cam2.norm(); + vframe1.landmarks[idx_down].flag = LandmarkFlag::UNINITIALIZED; + count_3d++; } - - if (_config.send_img) { - encodeImage(image_left, vframe0); - encodeImage(image_right, vframe1); + } + + if (_config.send_img) { + encodeImage(image_left, vframe0); + encodeImage(image_right, vframe1); + } + + if (_config.show) { + cv::Mat img_up = image_left; + cv::Mat img_down = image_right; + if (!_config.send_img) { + encodeImage(img_up, vframe0); + encodeImage(img_down, vframe1); } + cv::cvtColor(img_up, img_up, cv::COLOR_GRAY2BGR); + cv::cvtColor(img_down, img_down, cv::COLOR_GRAY2BGR); - if (_config.show) { - cv::Mat img_up = image_left; - cv::Mat img_down = image_right; - if (!_config.send_img) { - encodeImage(img_up, vframe0); - encodeImage(img_down, vframe1); - } - cv::cvtColor(img_up, img_up, cv::COLOR_GRAY2BGR); - cv::cvtColor(img_down, img_down, cv::COLOR_GRAY2BGR); - - for (auto pt : pts_down) - { - cv::circle(img_down, pt, 1, cv::Scalar(255, 0, 0), -1); - } + for (auto pt : pts_down) { + cv::circle(img_down, pt, 1, cv::Scalar(255, 0, 0), -1); + } - for (auto _pt : vframe0.landmarks2D()) { - cv::Point2f pt(_pt.x, _pt.y); - cv::circle(img_up, pt, 3, cv::Scalar(0, 0, 255), 1); - } + for (auto _pt : vframe0.landmarks2D()) { + cv::Point2f pt(_pt.x, _pt.y); + cv::circle(img_up, pt, 3, cv::Scalar(0, 0, 255), 1); + } - cv::vconcat(img_up, img_down, _show); - if (_config.stereo_as_depth_cam) { - for (unsigned int i = 0; i < pts_up.size(); i++) - { - int idx = ids_up[i]; - if (vframe0.landmarks[idx].flag) { - char title[100] = {0}; - auto pt = pts_up[i]; - cv::circle(_show, pt, 3, cv::Scalar(0, 255, 0), 1); - cv::arrowedLine(_show, pts_up[i], pts_down[i], cv::Scalar(255, 255, 0), 1); - } - } + cv::vconcat(img_up, img_down, _show); + if (_config.stereo_as_depth_cam) { + for (unsigned int i = 0; i < pts_up.size(); i++) { + int idx = ids_up[i]; + if (vframe0.landmarks[idx].flag) { + char title[100] = {0}; + auto pt = pts_up[i]; + cv::circle(_show, pt, 3, cv::Scalar(0, 255, 0), 1); + cv::arrowedLine(_show, pts_up[i], pts_down[i], + cv::Scalar(255, 255, 0), 1); } - - char text[100] = {0}; - sprintf(text, "Frame %ld Features %d/%d", msg.keyframe_id, count_3d, pts_up.size()); - cv::putText(_show, text, cv::Point2f(20, 30), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 255, 0), 1.5); + } } - if (params->debug_plot_superpoint_features || params->show || params->ftconfig->enable_lk_optical_flow) { - vframe1.raw_image = msg.right_images[vcam_id]; - vframe0.raw_image = msg.left_images[vcam_id]; - } - std::vector ret{vframe0, vframe1}; - return ret; + char text[100] = {0}; + sprintf(text, "Frame %ld Features %d/%d", msg.keyframe_id, count_3d, + pts_up.size()); + cv::putText(_show, text, cv::Point2f(20, 30), cv::FONT_HERSHEY_SIMPLEX, 0.5, + cv::Scalar(0, 255, 0), 1.5); + } + + if (params->debug_plot_superpoint_features || params->show || + params->ftconfig->enable_lk_optical_flow) { + vframe1.raw_image = msg.right_images[vcam_id]; + vframe0.raw_image = msg.left_images[vcam_id]; + } + std::vector ret{vframe0, vframe1}; + return ret; } -VisualImageDesc LoopCam::extractorImgDescDeepnet(ros::Time stamp, cv::Mat img, int camera_index, - int camera_id, bool superpoint_mode) -{ - auto start = high_resolution_clock::now(); - - VisualImageDesc vframe; - vframe.stamp = stamp.toSec(); - vframe.camera_index = camera_index; - vframe.camera_id = camera_id; - vframe.drone_id = self_id; - - if (camera_configuration == CameraConfig::STEREO_FISHEYE) { - cv::Mat roi = img(cv::Rect(0, img.rows*3/4, img.cols, img.rows/4)); - roi.setTo(cv::Scalar(0, 0, 0)); +VisualImageDesc LoopCam::extractorImgDescDeepnet(ros::Time stamp, cv::Mat img, + int camera_index, + int camera_id, + bool superpoint_mode) { + auto start = high_resolution_clock::now(); + + VisualImageDesc vframe; + vframe.stamp = stamp.toSec(); + vframe.camera_index = camera_index; + vframe.camera_id = camera_id; + vframe.drone_id = self_id; + + if (camera_configuration == CameraConfig::STEREO_FISHEYE) { + cv::Mat roi = img(cv::Rect(0, img.rows * 3 / 4, img.cols, img.rows / 4)); + roi.setTo(cv::Scalar(0, 0, 0)); + } + std::vector landmarks_2d; + if (_config.superpoint_max_num > 0) { + // We only inference when superpoint max num > 0 + // otherwise, d2vins only uses LK optical flow feature. + superpoint_ptr->infer(img, landmarks_2d, vframe.landmark_descriptor, + vframe.landmark_scores); + } + + if (!superpoint_mode) { + if (_config.cnn_use_onnx) { + vframe.image_desc = netvlad_onnx->inference(img); } - std::vector landmarks_2d; - if (_config.superpoint_max_num > 0) { - //We only inference when superpoint max num > 0 - //otherwise, d2vins only uses LK optical flow feature. - if (_config.cnn_use_onnx) { - superpoint_onnx->inference(img, landmarks_2d, vframe.landmark_descriptor, vframe.landmark_scores); - } + } + + for (unsigned int i = 0; i < landmarks_2d.size(); i++) { + auto pt_up = landmarks_2d[i]; + Eigen::Vector3d pt_up3d; + cams.at(camera_index) + ->liftProjective(Eigen::Vector2d(pt_up.x, pt_up.y), pt_up3d); + LandmarkPerFrame lm; + lm.pt2d = pt_up; + pt_up3d.normalize(); + if (pt_up3d.hasNaN()) { + SPDLOG_WARN("NaN detected!!! This will inference landmark_descriptor\n"); + continue; } - - if (!superpoint_mode) { - if (_config.cnn_use_onnx) { - vframe.image_desc = netvlad_onnx->inference(img); - } + lm.pt3d_norm = pt_up3d; + lm.camera_index = camera_index; + lm.camera_id = camera_id; + lm.stamp = vframe.stamp; + lm.stamp_discover = vframe.stamp; + lm.color = extractColor(img, pt_up); + vframe.landmarks.emplace_back(lm); + if (_config.OUTPUT_RAW_SUPERPOINT_DESC) { + for (int j = 0; j < params->superpoint_dims; j++) { + fsp << vframe.landmark_descriptor[i * params->superpoint_dims + j] + << " "; + } + fsp << std::endl; } + } - for (unsigned int i = 0; i < landmarks_2d.size(); i++) - { - auto pt_up = landmarks_2d[i]; - Eigen::Vector3d pt_up3d; - cams.at(camera_index)->liftProjective(Eigen::Vector2d(pt_up.x, pt_up.y), pt_up3d); - LandmarkPerFrame lm; - lm.pt2d = pt_up; - pt_up3d.normalize(); - if (pt_up3d.hasNaN()) { - ROS_WARN("NaN detected!!! This will inference landmark_descriptor\n"); - continue; - } - lm.pt3d_norm = pt_up3d; - lm.camera_index = camera_index; - lm.camera_id = camera_id; - lm.stamp = vframe.stamp; - lm.stamp_discover = vframe.stamp; - lm.color = extractColor(img, pt_up); - vframe.landmarks.emplace_back(lm); - if (_config.OUTPUT_RAW_SUPERPOINT_DESC) { - for (int j = 0; j < params->superpoint_dims; j ++) { - fsp << vframe.landmark_descriptor[i*params->superpoint_dims + j] << " "; - } - fsp << std::endl; - } - } - - return vframe; + return vframe; } -} \ No newline at end of file +} // namespace D2FrontEnd \ No newline at end of file diff --git a/d2frontend/src/loop_detector.cpp b/d2frontend/src/loop_detector.cpp index 4a2c5090..eb4b3bbd 100644 --- a/d2frontend/src/loop_detector.cpp +++ b/d2frontend/src/loop_detector.cpp @@ -1,19 +1,21 @@ -#include -#include #include -#include -#include #include -#include -#include -#include -#include -#include +#include +#include #include -#include #include +#include + +#include +#include +#include +#include +#include +#include +#include +#include -using namespace std::chrono; +using namespace std::chrono; using namespace D2Common; #define USE_FUNDMENTAL @@ -21,718 +23,850 @@ using namespace D2Common; namespace D2FrontEnd { -void LoopDetector::processImageArray(VisualImageDescArray & image_array) { - //Lock frame_mutex with Guard - std::lock_guard guard(frame_mutex); - TicToc tt; - static double t_sum = 0; - static int t_count = 0; - - auto start = high_resolution_clock::now(); - - if (t0 < 0) { - t0 = image_array.stamp; - } - - printf("[LoopDetector] processImageArray %ld from drone %d images: %d landmark: %d lazy: %d matched_to %d@D%d\n", image_array.frame_id, - image_array.drone_id, image_array.images.size(), image_array.spLandmarkNum(), image_array.is_lazy_frame, image_array.matched_frame, image_array.matched_drone); - - if (image_array.images.size() == 0) { - ROS_WARN("[LoopDetector] FlattenDesc must carry more than zero images"); - return; - } - - ego_motion_traj.push(ros::Time(image_array.stamp), image_array.pose_drone); - - int drone_id = image_array.drone_id; - int images_num = image_array.images.size(); - bool is_matched_frame = image_array.isMatchedFrame(); - bool is_lazy_frame = image_array.is_lazy_frame; - if (is_matched_frame && image_array.matched_drone != self_id) { - if(!image_array.is_lazy_frame) { - //Will be cache to databse - addImageArrayToDatabase(image_array, false); - printf("[LoopDetector@%d] Add KF %ld from drone %d images: %d landmark: %d lazy: %d matched_to %d", - self_id, image_array.frame_id, drone_id, image_array.images.size(), image_array.spLandmarkNum(), image_array.is_lazy_frame, image_array.matched_frame); +void LoopDetector::processImageArray(VisualImageDescArray &image_array) { + // Lock frame_mutex with Guard + std::lock_guard guard(frame_mutex); + TicToc tt; + static double t_sum = 0; + static int t_count = 0; + + auto start = high_resolution_clock::now(); + + if (t0 < 0) { + t0 = image_array.stamp; + } + + SPDLOG_INFO( + "[LoopDetector] processImageArray {} from {} images: {} landmark: {} " + "lazy: {} matched_to {}@D{}", + image_array.frame_id, image_array.drone_id, image_array.images.size(), + image_array.spLandmarkNum(), image_array.is_lazy_frame, + image_array.matched_frame, image_array.matched_drone); + + if (image_array.images.size() == 0) { + SPDLOG_WARN("[LoopDetector] FlattenDesc must carry more than zero images"); + return; + } + + ego_motion_traj.push(ros::Time(image_array.stamp), image_array.pose_drone); + + int drone_id = image_array.drone_id; + int images_num = image_array.images.size(); + bool is_matched_frame = image_array.isMatchedFrame(); + bool is_lazy_frame = image_array.is_lazy_frame; + if (is_matched_frame && image_array.matched_drone != self_id) { + if (!image_array.is_lazy_frame) { + // Will be cache to databse + addImageArrayToDatabase(image_array, false); + printf( + "[LoopDetector@%d] Add KF %ld from drone %d images: %d landmark: %d " + "lazy: %d matched_to %d", + self_id, image_array.frame_id, drone_id, image_array.images.size(), + image_array.spLandmarkNum(), image_array.is_lazy_frame, + image_array.matched_frame); + } + // printf("[LoopDetector@%d] Frame %ld matched to drone %ld, giveup\n", + // self_id, image_array.frame_id, image_array.matched_drone); + return; + } + + if (drone_id != this->self_id && databaseSize() == 0) { + ROS_INFO("[LoopDetector] Empty local database, will giveup remote image"); + return; + } + + bool new_node = all_nodes.find(image_array.drone_id) == all_nodes.end(); + all_nodes.insert(image_array.drone_id); + + int dir_count = 0; + for (auto &img : image_array.images) { + if (img.spLandmarkNum() > 0 || image_array.is_lazy_frame) { + dir_count++; + } + } + if (dir_count < _config.MIN_DIRECTION_LOOP) { + ROS_INFO( + "[LoopDetector@%d] Give up image_array %ld with less than %d(%d) " + "available images", + self_id, image_array.frame_id, _config.MIN_DIRECTION_LOOP, dir_count); + return; + } + + if (image_array.spLandmarkNum() >= _config.loop_inlier_feature_num || + is_lazy_frame) { + // Initialize images for visualization + if (params->show) { + std::vector imgs; + for (unsigned int i = 0; i < images_num; i++) { + auto &img_des = image_array.images[i]; + if (!img_des.raw_image.empty()) { + imgs.emplace_back(img_des.raw_image); + } else if (img_des.image.size() != 0) { + imgs.emplace_back(decode_image(img_des)); + } else { + // imgs[i] = cv::Mat(height, width, CV_8UC3, cv::Scalar(255, 255, + // 255)); + imgs.emplace_back( + cv::Mat(params->height, params->width, CV_8U, cv::Scalar(255))); } - // printf("[LoopDetector@%d] Frame %ld matched to drone %ld, giveup\n", self_id, image_array.frame_id, image_array.matched_drone); - return; - } - - if (drone_id!= this->self_id && databaseSize() == 0) { - ROS_INFO("[LoopDetector] Empty local database, will giveup remote image"); - return; + if (params->camera_configuration == STEREO_PINHOLE) { + break; + } + } + msgid2cvimgs[image_array.frame_id] = imgs; } - bool new_node = all_nodes.find(image_array.drone_id) == all_nodes.end(); - all_nodes.insert(image_array.drone_id); - - int dir_count = 0; - for (auto & img : image_array.images) { - if (img.spLandmarkNum() > 0 || image_array.is_lazy_frame) { - dir_count ++; + bool success = false; + VisualImageDescArray _old_fisheye_img; + int camera_index = 1; + int camera_index_old = -1; + if (is_matched_frame) { + if (!hasFrame(image_array.matched_frame)) { + success = false; + printf( + "[LoopDetector] frame %ld is matched to local frame %ld but not in " + "db\n", + image_array.frame_id, image_array.matched_frame); + } else { + // printf("[LoopDetector] frame %ld is matched to local frame %ld in + // db\n", image_array.frame_id, image_array.matched_frame); + success = true; + const std::lock_guard lock(keyframe_database_mutex); + _old_fisheye_img = keyframe_database.at(image_array.matched_frame); + camera_index = 0; // TODO: this is a hack + camera_index_old = 0; + if (is_lazy_frame) { + // In this case, it's a keyframe that has been broadcasted and should + // be recorded in database + printf( + "[LoopDetector] frame %ld is matched to local frame %ld in db " + "and we find it in cache\n", + image_array.frame_id, image_array.matched_frame); + if (keyframe_database.find(image_array.frame_id) == + keyframe_database.end()) { + ROS_WARN( + "[LoopDetector] Lazy frame %ld is matched to local frame %ld " + "in db, but is not in cache", + image_array.frame_id, image_array.matched_frame); + success = false; + } else { + printf("[LoopDetector] frame %ld is found in database\n", + image_array.frame_id); + image_array = keyframe_database.at(image_array.frame_id); + } } + } + } else { + if (databaseSize() > _config.match_index_dist || + drone_id != self_id && + databaseSize() > _config.match_index_dist_remote) { + success = queryImageArrayFromDatabase(image_array, _old_fisheye_img, + camera_index, camera_index_old); + auto stop = high_resolution_clock::now(); + } } - if (dir_count < _config.MIN_DIRECTION_LOOP) { - ROS_INFO("[LoopDetector@%d] Give up image_array %ld with less than %d(%d) available images", - self_id, image_array.frame_id, _config.MIN_DIRECTION_LOOP, dir_count); - return; - } - - if (image_array.spLandmarkNum() >= _config.loop_inlier_feature_num || is_lazy_frame) { - //Initialize images for visualization - if (params->show) { - std::vector imgs; - for (unsigned int i = 0; i < images_num; i++) { - auto & img_des = image_array.images[i]; - if (!img_des.raw_image.empty()) { - imgs.emplace_back(img_des.raw_image); - } else if (img_des.image.size() != 0) { - imgs.emplace_back(decode_image(img_des)); - } else { - // imgs[i] = cv::Mat(height, width, CV_8UC3, cv::Scalar(255, 255, 255)); - imgs.emplace_back(cv::Mat(params->height, params->width, CV_8U, cv::Scalar(255))); - } - if (params->camera_configuration == STEREO_PINHOLE) { - break; - } - } - msgid2cvimgs[image_array.frame_id] = imgs; + if (success) { + if (!is_matched_frame && is_lazy_frame) { + // In this case, we need to send the matched frame to the drone + _old_fisheye_img.matched_drone = image_array.drone_id; + _old_fisheye_img.matched_frame = image_array.frame_id; + printf( + "[LoopDetector@%d] Lazy frame %d is matched with %d try to " + "broadcast this frame\n", + self_id, image_array.frame_id, _old_fisheye_img.frame_id); + if (broadcast_keyframe_cb) { + broadcast_keyframe_cb(_old_fisheye_img); } - - bool success = false; - VisualImageDescArray _old_fisheye_img; - int camera_index = 1; - int camera_index_old = -1; - if (is_matched_frame) { - if (!hasFrame(image_array.matched_frame)) { - success = false; - printf("[LoopDetector] frame %ld is matched to local frame %ld but not in db\n", image_array.frame_id, image_array.matched_frame); - } else { - // printf("[LoopDetector] frame %ld is matched to local frame %ld in db\n", image_array.frame_id, image_array.matched_frame); - success = true; - const std::lock_guard lock(keyframe_database_mutex); - _old_fisheye_img = keyframe_database.at(image_array.matched_frame); - camera_index = 0; //TODO: this is a hack - camera_index_old = 0; - if (is_lazy_frame) { - //In this case, it's a keyframe that has been broadcasted and should be recorded in database - printf("[LoopDetector] frame %ld is matched to local frame %ld in db and we find it in cache\n", - image_array.frame_id, image_array.matched_frame); - if (keyframe_database.find(image_array.frame_id) == keyframe_database.end()) { - ROS_WARN("[LoopDetector] Lazy frame %ld is matched to local frame %ld in db, but is not in cache", image_array.frame_id, image_array.matched_frame); - success = false; - } else { - printf("[LoopDetector] frame %ld is found in database\n", image_array.frame_id); - image_array = keyframe_database.at(image_array.frame_id); - } - } - } + } else { + printf("Compute loop connection %ld and %ld\n", image_array.frame_id, + _old_fisheye_img.frame_id); + swarm_msgs::LoopEdge ret; + if (_old_fisheye_img.drone_id == self_id) { + success = computeLoop(_old_fisheye_img, image_array, camera_index_old, + camera_index, ret); + } else if (image_array.drone_id == self_id) { + success = computeLoop(image_array, _old_fisheye_img, camera_index, + camera_index_old, ret); } else { - if (databaseSize() > _config.match_index_dist || drone_id != self_id && databaseSize() > _config.match_index_dist_remote) { - success = queryImageArrayFromDatabase(image_array, _old_fisheye_img, camera_index, camera_index_old); - auto stop = high_resolution_clock::now(); - } + ROS_WARN("[LoopDetector%d] Will not compute loop, drone id is %d", + self_id, image_array.drone_id); } if (success) { - if (!is_matched_frame && is_lazy_frame) { - //In this case, we need to send the matched frame to the drone - _old_fisheye_img.matched_drone = image_array.drone_id; - _old_fisheye_img.matched_frame = image_array.frame_id; - printf("[LoopDetector@%d] Lazy frame %d is matched with %d try to broadcast this frame\n", - self_id, image_array.frame_id, _old_fisheye_img.frame_id); - if (broadcast_keyframe_cb) { - broadcast_keyframe_cb(_old_fisheye_img); - } - } else { - printf("Compute loop connection %ld and %ld\n", image_array.frame_id, _old_fisheye_img.frame_id); - swarm_msgs::LoopEdge ret; - if (_old_fisheye_img.drone_id == self_id) { - success = computeLoop(_old_fisheye_img, image_array, camera_index_old, camera_index, ret); - } else if (image_array.drone_id == self_id) { - success = computeLoop(image_array, _old_fisheye_img, camera_index, camera_index_old, ret); - } else { - ROS_WARN("[LoopDetector%d] Will not compute loop, drone id is %d", self_id, image_array.drone_id); - } - if (success) { - onLoopConnection(ret); - } - } - } else { - if (params->verbose) - printf("[LoopDetector@%d] No matched image for frame %ld\n", self_id, image_array.frame_id); - } - if (!is_lazy_frame && (!image_array.prevent_adding_db || new_node)) { - if (image_array.drone_id == self_id) { - //Only add local frame to database - addImageArrayToDatabase(image_array, true); - } else { - addImageArrayToDatabase(image_array, false); - } + onLoopConnection(ret); } - } - - t_sum += tt.toc(); - t_count += 1; - if (params->verbose || params->enable_perf_output) - printf("[LoopDetector] Full LoopDetect avg %.1fms cur %.1fms\n", t_sum/t_count, tt.toc()); + } + } else { + if (params->verbose) + printf("[LoopDetector@%d] No matched image for frame %ld\n", self_id, + image_array.frame_id); + } + if (!is_lazy_frame && (!image_array.prevent_adding_db || new_node)) { + if (image_array.drone_id == self_id) { + // Only add local frame to database + addImageArrayToDatabase(image_array, true); + } else { + addImageArrayToDatabase(image_array, false); + } + } + } + + t_sum += tt.toc(); + t_count += 1; + if (params->verbose || params->enable_perf_output) + printf("[LoopDetector] Full LoopDetect avg %.1fms cur %.1fms\n", + t_sum / t_count, tt.toc()); } +cv::Mat LoopDetector::decode_image(const VisualImageDesc &_img_desc) { + auto start = high_resolution_clock::now(); + // auto ret = cv::imdecode(_img_desc.image, cv::IMREAD_GRAYSCALE); + auto ret = cv::imdecode(_img_desc.image, cv::IMREAD_UNCHANGED); + // std::cout << "IMDECODE Cost " << + // duration_cast(high_resolution_clock::now() - + // start).count()/1000.0 << "ms" << std::endl; -cv::Mat LoopDetector::decode_image(const VisualImageDesc & _img_desc) { - - auto start = high_resolution_clock::now(); - // auto ret = cv::imdecode(_img_desc.image, cv::IMREAD_GRAYSCALE); - auto ret = cv::imdecode(_img_desc.image, cv::IMREAD_UNCHANGED); - // std::cout << "IMDECODE Cost " << duration_cast(high_resolution_clock::now() - start).count()/1000.0 << "ms" << std::endl; - - return ret; + return ret; } -int LoopDetector::addImageArrayToDatabase(VisualImageDescArray & new_fisheye_desc, bool add_to_faiss) { - if (add_to_faiss) { - for (size_t i = 0; i < new_fisheye_desc.images.size(); i++) { - auto & img_desc = new_fisheye_desc.images[i]; - if (img_desc.spLandmarkNum() > 0 && img_desc.image_desc.size() > 0) { - int index = addImageDescToDatabase(img_desc); - index_to_frame_id[index] = new_fisheye_desc.frame_id; - imgid2dir[index] = i; - // ROS_INFO("[LoopDetector] Add keyframe from %d(dir %d) to local keyframe database index: %d", img_desc.drone_id, i, index); - } - if (params->camera_configuration == CameraConfig::PINHOLE_DEPTH) { - break; - } - const std::lock_guard lock(keyframe_database_mutex); - } - } - keyframe_database[new_fisheye_desc.frame_id] = new_fisheye_desc; - printf("[LoopDetector] Add KF %ld with %d images from %d to local keyframe database. Total frames: %ld\n", - new_fisheye_desc.frame_id, new_fisheye_desc.images.size(), new_fisheye_desc.drone_id, keyframe_database.size()); - // new_fisheye_desc.printSize(); - return new_fisheye_desc.frame_id; +int LoopDetector::addImageArrayToDatabase( + VisualImageDescArray &new_fisheye_desc, bool add_to_faiss) { + if (add_to_faiss) { + for (size_t i = 0; i < new_fisheye_desc.images.size(); i++) { + auto &img_desc = new_fisheye_desc.images[i]; + if (img_desc.spLandmarkNum() > 0 && img_desc.image_desc.size() > 0) { + int index = addImageDescToDatabase(img_desc); + index_to_frame_id[index] = new_fisheye_desc.frame_id; + imgid2dir[index] = i; + } + if (params->camera_configuration == CameraConfig::PINHOLE_DEPTH) { + break; + } + const std::lock_guard lock(keyframe_database_mutex); + } + } + keyframe_database[new_fisheye_desc.frame_id] = new_fisheye_desc; + SPDLOG_INFO( + "[LoopDetector] Add KF {} with {} images from {} to local keyframe " + "database. Total frames: {}", + new_fisheye_desc.frame_id, new_fisheye_desc.images.size(), + new_fisheye_desc.drone_id, keyframe_database.size()); + // new_fisheye_desc.printSize(); + return new_fisheye_desc.frame_id; } -int LoopDetector::addImageDescToDatabase(VisualImageDesc & img_desc_a) { - if (img_desc_a.drone_id == self_id) { - local_index.add(1, img_desc_a.image_desc.data()); - return local_index.ntotal - 1; - } else { - remote_index.add(1, img_desc_a.image_desc.data()); - return remote_index.ntotal - 1 + REMOTE_MAGIN_NUMBER; - } - return -1; +int LoopDetector::addImageDescToDatabase(VisualImageDesc &img_desc_a) { + if (img_desc_a.drone_id == self_id) { + local_index.add(1, img_desc_a.image_desc.data()); + return local_index.ntotal - 1; + } else { + remote_index.add(1, img_desc_a.image_desc.data()); + return remote_index.ntotal - 1 + REMOTE_MAGIN_NUMBER; + } + return -1; } - -int LoopDetector::queryFrameIndexFromDatabase(const VisualImageDesc & img_desc, double & similarity) { - double thres = _config.loop_detection_netvlad_thres; - int ret = -1; - if (img_desc.drone_id == self_id) { - //Then this is self drone - double similarity_local, similarity_remote; - int ret_remote = queryIndexFromDatabase(img_desc, remote_index, true, thres, _config.match_index_dist, similarity_remote); - int ret_local = queryIndexFromDatabase(img_desc, local_index, false, thres, _config.match_index_dist, similarity_local); - if (ret_remote >=0 && ret_local >= 0) { - if (similarity_local > similarity_remote) { - similarity = similarity_local; - return ret_local; - } else { - similarity = similarity_remote; - return similarity_remote; - } - } else if (ret_remote >=0) { - similarity = similarity_remote; - return similarity_remote; - } else if (ret_local >= 0) { - similarity = similarity_local; - return ret_local; - } - } else { - ret = queryIndexFromDatabase(img_desc, local_index, false, thres, _config.match_index_dist_remote, similarity); - } - return ret; +int LoopDetector::queryFrameIndexFromDatabase(const VisualImageDesc &img_desc, + double &similarity) { + double thres = _config.loop_detection_netvlad_thres; + int ret = -1; + if (img_desc.drone_id == self_id) { + // Then this is self drone + double similarity_local, similarity_remote; + int ret_remote = + queryIndexFromDatabase(img_desc, remote_index, true, thres, + _config.match_index_dist, similarity_remote); + int ret_local = + queryIndexFromDatabase(img_desc, local_index, false, thres, + _config.match_index_dist, similarity_local); + if (ret_remote >= 0 && ret_local >= 0) { + if (similarity_local > similarity_remote) { + similarity = similarity_local; + return ret_local; + } else { + similarity = similarity_remote; + return similarity_remote; + } + } else if (ret_remote >= 0) { + similarity = similarity_remote; + return similarity_remote; + } else if (ret_local >= 0) { + similarity = similarity_local; + return ret_local; + } + } else { + ret = queryIndexFromDatabase(img_desc, local_index, false, thres, + _config.match_index_dist_remote, similarity); + } + return ret; } -int LoopDetector::queryIndexFromDatabase(const VisualImageDesc & img_desc, faiss::IndexFlatIP & index, bool remote_db, - double thres, int max_index, double & similarity) { - float similiarity[1024] = {0}; - faiss::idx_t labels[1024]; - - int index_offset = 0; - if (remote_db) { - index_offset = REMOTE_MAGIN_NUMBER; - } - for (int i = 0; i < 1000; i++) { - labels[i] = -1; - } - int search_num = std::min(SEARCH_NEAREST_NUM + max_index, (int) index.ntotal); - if (search_num <= 0) { - return -1; - } - index.search(1, img_desc.image_desc.data(), search_num, similiarity, labels); - int return_frame_id = -1, return_drone_id = -1; - int k = -1; - for (int i = 0; i < search_num; i++) { - if (labels[i] < 0) { - continue; - } - if (index_to_frame_id.find(labels[i] + index_offset) == index_to_frame_id.end()) { - ROS_WARN("[LoopDetector] Can't find image %d; skipping", labels[i] + index_offset); - continue; - } - // int return_frame_id = index_to_frame_id.at(labels[i] + index_offset); - return_frame_id = labels[i] + index_offset; - const std::lock_guard lock(keyframe_database_mutex); - return_drone_id = keyframe_database.at(index_to_frame_id.at(return_frame_id)).drone_id; - // ROS_INFO("Return Label %d/%d/%d from %d, distance %f/%f", labels[i] + index_offset, index.ntotal, index.ntotal - max_index , return_drone_id, similiarity[i], thres); - if (labels[i] <= index.ntotal - max_index && similiarity[i] > thres) { - //Is same id, max index make sense - k = i; - thres = similarity = similiarity[i]; - return return_frame_id; - } - } - // ROS_INFO("Database return %ld on drone %d, radius %f frame_id %d", labels[k] + index_offset, return_drone_id, similiarity[k], return_frame_id); +int LoopDetector::queryIndexFromDatabase(const VisualImageDesc &img_desc, + faiss::IndexFlatIP &index, + bool remote_db, double thres, + int max_index, double &similarity) { + float similiarity[1024] = {0}; + faiss::idx_t labels[1024]; + + int index_offset = 0; + if (remote_db) { + index_offset = REMOTE_MAGIN_NUMBER; + } + for (int i = 0; i < 1000; i++) { + labels[i] = -1; + } + int search_num = std::min(SEARCH_NEAREST_NUM + max_index, (int)index.ntotal); + if (search_num <= 0) { return -1; + } + index.search(1, img_desc.image_desc.data(), search_num, similiarity, labels); + int return_frame_id = -1, return_drone_id = -1; + int k = -1; + for (int i = 0; i < search_num; i++) { + if (labels[i] < 0) { + continue; + } + if (index_to_frame_id.find(labels[i] + index_offset) == + index_to_frame_id.end()) { + ROS_WARN("[LoopDetector] Can't find image %d; skipping", + labels[i] + index_offset); + continue; + } + // int return_frame_id = index_to_frame_id.at(labels[i] + index_offset); + return_frame_id = labels[i] + index_offset; + const std::lock_guard lock(keyframe_database_mutex); + return_drone_id = + keyframe_database.at(index_to_frame_id.at(return_frame_id)).drone_id; + // ROS_INFO("Return Label %d/%d/%d from %d, distance %f/%f", labels[i] + + // index_offset, index.ntotal, index.ntotal - max_index , return_drone_id, + // similiarity[i], thres); + if (labels[i] <= index.ntotal - max_index && similiarity[i] > thres) { + // Is same id, max index make sense + k = i; + thres = similarity = similiarity[i]; + return return_frame_id; + } + } + // ROS_INFO("Database return %ld on drone %d, radius %f frame_id %d", + // labels[k] + index_offset, return_drone_id, similiarity[k], + // return_frame_id); + return -1; } - -bool LoopDetector::queryImageArrayFromDatabase(const VisualImageDescArray & img_desc_a, - VisualImageDescArray & ret, int & camera_index_new, int & camera_index_old) { - double best_similarity = -1; - int best_image_index = -1; - //Strict use camera_index 1 now +bool LoopDetector::queryImageArrayFromDatabase( + const VisualImageDescArray &img_desc_a, VisualImageDescArray &ret, + int &camera_index_new, int &camera_index_old) { + double best_similarity = -1; + int best_image_index = -1; + // Strict use camera_index 1 now + camera_index_new = 0; + if (loop_cam->getCameraConfiguration() == CameraConfig::STEREO_FISHEYE) { + camera_index_new = 1; + } else if (loop_cam->getCameraConfiguration() == + CameraConfig::FOURCORNER_FISHEYE) { + // If four coner fishe, use camera 2. + camera_index_new = 2; + } else if (loop_cam->getCameraConfiguration() == + CameraConfig::STEREO_PINHOLE || + loop_cam->getCameraConfiguration() == + CameraConfig::PINHOLE_DEPTH) { camera_index_new = 0; - if (loop_cam->getCameraConfiguration() == CameraConfig::STEREO_FISHEYE) { - camera_index_new = 1; - } else if (loop_cam->getCameraConfiguration() == CameraConfig::FOURCORNER_FISHEYE) { - //If four coner fishe, use camera 2. - camera_index_new = 2; - } else if (loop_cam->getCameraConfiguration() == CameraConfig::STEREO_PINHOLE || - loop_cam->getCameraConfiguration() == CameraConfig::PINHOLE_DEPTH) { - camera_index_new = 0; - } else { - ROS_ERROR("[LoopDetector] Camera configuration %d not support yet in queryImageArrayFromDatabase", loop_cam->getCameraConfiguration()); - exit(-1); - } - - if (img_desc_a.images[camera_index_new].spLandmarkNum() > 0 || img_desc_a.is_lazy_frame) { - double similarity = -1; - int index = queryFrameIndexFromDatabase(img_desc_a.images.at(camera_index_new), similarity); - if (index != -1 && similarity > best_similarity) { - best_image_index = index; - best_similarity = similarity; - } - - if (best_image_index != -1) { - const std::lock_guard lock(keyframe_database_mutex); - int frame_id = index_to_frame_id[best_image_index]; - camera_index_old = imgid2dir[best_image_index]; - printf("[LoopDetector] Query image for %ld: ret frame_id %d index %d drone %d with camera %d similarity %f\n", - img_desc_a.frame_id, frame_id, best_image_index, keyframe_database.at(frame_id).drone_id, camera_index_old, best_similarity); - ret = keyframe_database.at(frame_id); - return true; - } - } - - camera_index_old = -1; - ret.frame_id = -1; - return false; + } else { + ROS_ERROR( + "[LoopDetector] Camera configuration %d not support yet in " + "queryImageArrayFromDatabase", + loop_cam->getCameraConfiguration()); + exit(-1); + } + + if (img_desc_a.images[camera_index_new].spLandmarkNum() > 0 || + img_desc_a.is_lazy_frame) { + double similarity = -1; + int index = queryFrameIndexFromDatabase( + img_desc_a.images.at(camera_index_new), similarity); + if (index != -1 && similarity > best_similarity) { + best_image_index = index; + best_similarity = similarity; + } + + if (best_image_index != -1) { + const std::lock_guard lock(keyframe_database_mutex); + int frame_id = index_to_frame_id[best_image_index]; + camera_index_old = imgid2dir[best_image_index]; + printf( + "[LoopDetector] Query image for %ld: ret frame_id %d index %d drone " + "%d with camera %d similarity %f\n", + img_desc_a.frame_id, frame_id, best_image_index, + keyframe_database.at(frame_id).drone_id, camera_index_old, + best_similarity); + ret = keyframe_database.at(frame_id); + return true; + } + } + + camera_index_old = -1; + ret.frame_id = -1; + return false; } - int LoopDetector::databaseSize() const { - return local_index.ntotal + remote_index.ntotal; + return local_index.ntotal + remote_index.ntotal; } - -bool LoopDetector::checkLoopOdometryConsistency(LoopEdge & loop_conn) const { - if (loop_conn.drone_id_a != loop_conn.drone_id_b || _config.DEBUG_NO_REJECT) { - //Is inter_loop, odometry consistency check is disabled. - return true; - } - - Swarm::LoopEdge edge(loop_conn); - auto odom = ego_motion_traj.get_relative_pose_by_appro_ts(edge.ts_a, edge.ts_b); - Eigen::Matrix6d cov_vec = odom.second + edge.getCovariance(); - auto dp = Swarm::Pose::DeltaPose(edge.relative_pose, odom.first); - auto md = Swarm::computeSquaredMahalanobisDistance(dp.log_map(), cov_vec); - if (md > _config.odometry_consistency_threshold) { - printf("[LoopDetector] LoopEdge-Odometry consistency check failed %.1f, odom %s loop %s dp %s.\n", - md, odom.first.toStr().c_str(), edge.relative_pose.toStr().c_str(), dp.toStr().c_str()); - return false; - } - - printf("[LoopDetector] LoopEdge-Odometry consistency OK %.1f odom %s loop %s dp %s.\n", md, - odom.first.toStr().c_str(), edge.relative_pose.toStr().c_str(), dp.toStr().c_str()); +bool LoopDetector::checkLoopOdometryConsistency(LoopEdge &loop_conn) const { + if (loop_conn.drone_id_a != loop_conn.drone_id_b || _config.DEBUG_NO_REJECT) { + // Is inter_loop, odometry consistency check is disabled. return true; + } + + Swarm::LoopEdge edge(loop_conn); + auto odom = + ego_motion_traj.get_relative_pose_by_appro_ts(edge.ts_a, edge.ts_b); + Eigen::Matrix6d cov_vec = odom.second + edge.getCovariance(); + auto dp = Swarm::Pose::DeltaPose(edge.relative_pose, odom.first); + auto md = Swarm::computeSquaredMahalanobisDistance(dp.log_map(), cov_vec); + if (md > _config.odometry_consistency_threshold) { + printf( + "[LoopDetector] LoopEdge-Odometry consistency check failed %.1f, odom " + "%s loop %s dp %s.\n", + md, odom.first.toStr().c_str(), edge.relative_pose.toStr().c_str(), + dp.toStr().c_str()); + return false; + } + + printf( + "[LoopDetector] LoopEdge-Odometry consistency OK %.1f odom %s loop %s dp " + "%s.\n", + md, odom.first.toStr().c_str(), edge.relative_pose.toStr().c_str(), + dp.toStr().c_str()); + return true; } -//Note! here the norms are both projected to main dir's unit sphere. -//index2dirindex store the dir and the index of the point -bool LoopDetector::computeCorrespondFeaturesOnImageArray(const VisualImageDescArray & frame_array_a, - const VisualImageDescArray & frame_array_b, int main_dir_a, int main_dir_b, - std::vector &lm_pos_a, std::vector &lm_norm_3d_b, std::vector &cam_indices, std::vector> &index2dirindex_a, +// Note! here the norms are both projected to main dir's unit sphere. +// index2dirindex store the dir and the index of the point +bool LoopDetector::computeCorrespondFeaturesOnImageArray( + const VisualImageDescArray &frame_array_a, + const VisualImageDescArray &frame_array_b, int main_dir_a, int main_dir_b, + std::vector &lm_pos_a, std::vector &lm_norm_3d_b, + std::vector &cam_indices, + std::vector> &index2dirindex_a, std::vector> &index2dirindex_b) { - std::vector dirs_a; - std::vector dirs_b; - - if (params->camera_configuration == STEREO_PINHOLE) { - dirs_a = {0, 1}; - dirs_b = {0, 1}; - } else if (params->camera_configuration == PINHOLE_DEPTH) { - dirs_a = {0, 1}; - dirs_b = {0}; - } else if (params->camera_configuration == STEREO_FISHEYE || params->camera_configuration == FOURCORNER_FISHEYE) { - for (int _dir_a = main_dir_a; _dir_a < main_dir_a + _config.MAX_DIRS; _dir_a ++) { - int dir_a = _dir_a % _config.MAX_DIRS; - int dir_b = ((main_dir_b - main_dir_a + _config.MAX_DIRS) % _config.MAX_DIRS + _dir_a)% _config.MAX_DIRS; - if (dir_a < frame_array_a.images.size() && dir_b < frame_array_b.images.size()) { - if (frame_array_b.images[dir_b].spLandmarkNum() > 0 && frame_array_a.images[dir_a].spLandmarkNum() > 0) { - dirs_a.push_back(dir_a); - dirs_b.push_back(dir_b); - } - } - } - } - - Swarm::Pose extrinsic_a(frame_array_a.images[main_dir_a].extrinsic); - Swarm::Pose extrinsic_b(frame_array_b.images[main_dir_b].extrinsic); - Eigen::Quaterniond main_quat_new = extrinsic_a.att(); - Eigen::Quaterniond main_quat_old = extrinsic_b.att(); - - int matched_dir_count = 0; - for (size_t i = 0; i < dirs_a.size(); i++) { - int dir_a = dirs_a[i]; - int dir_b = dirs_b[i]; - std::vector _lm_norm_3d_b; - std::vector _lm_pos_a; - std::vector _idx_a; - std::vector _idx_b; - std::vector _camera_indices; - - if (dir_a < frame_array_a.images.size() && dir_b < frame_array_b.images.size() && dir_a >= 0 && dir_b >= 0) { - bool succ = computeCorrespondFeatures(frame_array_a.images[dir_a],frame_array_b.images[dir_b], - _lm_pos_a, _idx_a, _lm_norm_3d_b, _idx_b, _camera_indices); - // ROS_INFO("[LoopDetector] computeCorrespondFeatures on camera_index %d:%d gives %d common features", dir_b, dir_a, _lm_pos_a.size()); - if (!succ) { - continue; - } - } - - if ( _lm_pos_a.size() >= _config.MIN_MATCH_PRE_DIR ) { - matched_dir_count ++; - } - - Swarm::Pose _extrinsic_a(frame_array_a.images[dir_a].extrinsic); - Swarm::Pose _extrinsic_b(frame_array_b.images[dir_b].extrinsic); - - for (size_t id = 0; id < _lm_norm_3d_b.size(); id++) { - index2dirindex_a.push_back(std::make_pair(dir_a, _idx_a[id])); - index2dirindex_b.push_back(std::make_pair(dir_b, _idx_b[id])); - } - lm_pos_a.insert(lm_pos_a.end(), _lm_pos_a.begin(), _lm_pos_a.end()); - lm_norm_3d_b.insert(lm_norm_3d_b.end(), _lm_norm_3d_b.begin(), _lm_norm_3d_b.end()); - cam_indices.insert(cam_indices.end(), _camera_indices.begin(), _camera_indices.end()); - } - - if(lm_norm_3d_b.size() > _config.loop_inlier_feature_num && matched_dir_count >= _config.MIN_DIRECTION_LOOP) { - return true; - } else { - ROS_WARN("[LoopDetector::computeCorrImageArray@%d] Failed: features %d/%d dirs %d/%d", - self_id, lm_norm_3d_b.size(), _config.loop_inlier_feature_num, - matched_dir_count, _config.MIN_DIRECTION_LOOP); - return false; - } -} - -bool LoopDetector::computeCorrespondFeatures(const VisualImageDesc & img_desc_a, const VisualImageDesc & img_desc_b, - std::vector &lm_pos_a, std::vector &idx_a, std::vector &lm_norm_3d_b, - std::vector &idx_b, std::vector &cam_indices) { - std::vector _matches; - auto & _a_lms = img_desc_a.landmarks; - auto & _b_lms = img_desc_b.landmarks; - - if (_config.enable_superglue) { - auto kpts_a = img_desc_a.landmarks2D(true, true); - auto kpts_b = img_desc_b.landmarks2D(true, true); - auto & desc0 = img_desc_a.landmark_descriptor; - auto & desc1 = img_desc_b.landmark_descriptor; - auto & scores0 = img_desc_a.landmark_scores; - auto & scores1 = img_desc_b.landmark_scores; - _matches = superglue->inference(kpts_a, kpts_b, desc0, desc1, scores0, scores1); - } else{ - assert(img_desc_a.spLandmarkNum() * params->superpoint_dims == img_desc_a.landmark_descriptor.size() && "Desciptor size of new img desc must equal to to landmarks*256!!!"); - assert(img_desc_b.spLandmarkNum() * params->superpoint_dims == img_desc_b.landmark_descriptor.size() && "Desciptor size of old img desc must equal to to landmarks*256!!!"); - cv::Mat descriptors_a( img_desc_a.spLandmarkNum(), params->superpoint_dims, CV_32F); - memcpy(descriptors_a.data, img_desc_a.landmark_descriptor.data(), img_desc_a.landmark_descriptor.size()*sizeof(float)); - cv::Mat descriptors_b( img_desc_b.spLandmarkNum(), params->superpoint_dims, CV_32F); - memcpy(descriptors_b.data, img_desc_b.landmark_descriptor.data(), img_desc_b.landmark_descriptor.size()*sizeof(float)); - if (_config.enable_knn_match) { - _matches = matchKNN(descriptors_a, descriptors_b, _config.knn_match_ratio); - } else { - cv::BFMatcher bfmatcher(cv::NORM_L2, true); - bfmatcher.match(descriptors_a, descriptors_b, _matches); + std::vector dirs_a; + std::vector dirs_b; + + if (params->camera_configuration == STEREO_PINHOLE) { + dirs_a = {0}; + dirs_b = {0}; + } else if (params->camera_configuration == PINHOLE_DEPTH) { + dirs_a = {0, 1}; + dirs_b = {0}; + } else if (params->camera_configuration == STEREO_FISHEYE || + params->camera_configuration == FOURCORNER_FISHEYE) { + for (int _dir_a = main_dir_a; _dir_a < main_dir_a + _config.MAX_DIRS; + _dir_a++) { + int dir_a = _dir_a % _config.MAX_DIRS; + int dir_b = + ((main_dir_b - main_dir_a + _config.MAX_DIRS) % _config.MAX_DIRS + + _dir_a) % + _config.MAX_DIRS; + if (dir_a < frame_array_a.images.size() && + dir_b < frame_array_b.images.size()) { + if (frame_array_b.images[dir_b].spLandmarkNum() > 0 && + frame_array_a.images[dir_a].spLandmarkNum() > 0) { + dirs_a.push_back(dir_a); + dirs_b.push_back(dir_b); } - - } - Point2fVector lm_b_2d, lm_a_2d; - std::lock_guard guard(landmark_mutex); - for (auto match : _matches) { - int index_a = match.queryIdx; - int index_b = match.trainIdx; - auto landmark_id = _a_lms[index_a].landmark_id; - if (landmark_db.find(landmark_id) == landmark_db.end()) { - continue; - } - if (landmark_db.at(landmark_id).flag == LandmarkFlag::UNINITIALIZED || - landmark_db.at(landmark_id).flag == LandmarkFlag::OUTLIER) { - // ROS_WARN("Landmark %ld is not estimated or is outlier tracks %ld multi %ld", landmark_id, - // landmark_db.at(landmark_id).track.size(), landmark_db.at(landmark_id).isMultiCamera()); - continue; - } - Vector3d pt3d_norm_b = _b_lms[index_b].pt3d_norm; - lm_a_2d.push_back(_a_lms[index_a].pt2d); - lm_b_2d.push_back(_b_lms[index_b].pt2d); - idx_a.push_back(index_a); - idx_b.push_back(index_b); - lm_pos_a.push_back(landmark_db.at(landmark_id).position); - lm_norm_3d_b.push_back(pt3d_norm_b); - cam_indices.push_back(img_desc_b.camera_index); - } - - if (lm_b_2d.size() < 4) { - return false; - } - if (_config.enable_homography_test && !_config.enable_superglue) { - std::vector mask; - cv::findHomography(lm_b_2d, lm_a_2d, cv::RANSAC, 10.0, mask); - reduceVector(idx_a, mask); - reduceVector(idx_b, mask); - reduceVector(lm_pos_a, mask); - reduceVector(lm_norm_3d_b, mask); - } + } + } + } + + Swarm::Pose extrinsic_a(frame_array_a.images[main_dir_a].extrinsic); + Swarm::Pose extrinsic_b(frame_array_b.images[main_dir_b].extrinsic); + Eigen::Quaterniond main_quat_new = extrinsic_a.att(); + Eigen::Quaterniond main_quat_old = extrinsic_b.att(); + + int matched_dir_count = 0; + for (size_t i = 0; i < dirs_a.size(); i++) { + int dir_a = dirs_a[i]; + int dir_b = dirs_b[i]; + std::vector _lm_norm_3d_b; + std::vector _lm_pos_a; + std::vector _idx_a; + std::vector _idx_b; + std::vector _camera_indices; + + if (dir_a < frame_array_a.images.size() && + dir_b < frame_array_b.images.size() && dir_a >= 0 && dir_b >= 0) { + bool succ = computeCorrespondFeatures( + frame_array_a.images[dir_a], frame_array_b.images[dir_b], _lm_pos_a, + _idx_a, _lm_norm_3d_b, _idx_b, _camera_indices); + SPDLOG_INFO( + "[LoopDetector] computeCorrespondFeatures on camera_index {}:{} " + "gives {} common features", + dir_b, dir_a, _lm_pos_a.size()); + if (!succ) { + continue; + } + } + + if (_lm_pos_a.size() >= _config.MIN_MATCH_PRE_DIR) { + matched_dir_count++; + } + + Swarm::Pose _extrinsic_a(frame_array_a.images[dir_a].extrinsic); + Swarm::Pose _extrinsic_b(frame_array_b.images[dir_b].extrinsic); + + for (size_t id = 0; id < _lm_norm_3d_b.size(); id++) { + index2dirindex_a.push_back(std::make_pair(dir_a, _idx_a[id])); + index2dirindex_b.push_back(std::make_pair(dir_b, _idx_b[id])); + } + lm_pos_a.insert(lm_pos_a.end(), _lm_pos_a.begin(), _lm_pos_a.end()); + lm_norm_3d_b.insert(lm_norm_3d_b.end(), _lm_norm_3d_b.begin(), + _lm_norm_3d_b.end()); + cam_indices.insert(cam_indices.end(), _camera_indices.begin(), + _camera_indices.end()); + } + + if (lm_norm_3d_b.size() > _config.loop_inlier_feature_num && + matched_dir_count >= _config.MIN_DIRECTION_LOOP) { return true; + } else { + SPDLOG_WARN( + "[LoopDetector::computeCorrImageArray@{}] Failed: features {}/{} dirs " + "{}/{}", + self_id, lm_norm_3d_b.size(), _config.loop_inlier_feature_num, + matched_dir_count, _config.MIN_DIRECTION_LOOP); + return false; + } } -//Require 3d points of frame a and 2d point of frame b -bool LoopDetector::computeLoop(const VisualImageDescArray & frame_array_a, const VisualImageDescArray & frame_array_b, - int main_dir_a, int main_dir_b, LoopEdge & ret) { - - if (frame_array_a.spLandmarkNum() < _config.loop_inlier_feature_num) { - return false; - } - //Recover imformation - - assert(frame_array_a.drone_id == self_id && "frame_array_a must from self drone to provide more 2d points!"); - - bool success = false; - - double t_b = frame_array_b.stamp - t0; - double t_a = frame_array_a.stamp - t0; - printf("[LoopDetector::computeLoop@%d] Compute loop drone b %d(d%d,dir %d)->a %d(d%d,dir %d) t %.1f->%.1f(%.1f)s landmarks %d:%d.\n", - self_id, frame_array_b.frame_id, frame_array_b.drone_id, main_dir_b, frame_array_a.frame_id, frame_array_a.drone_id, main_dir_a, - t_b, t_a, t_a - t_b, frame_array_b.spLandmarkNum(), frame_array_a.spLandmarkNum()); - - std::vector lm_pos_a; - std::vector lm_norm_3d_b; - std::vector dirs_a; - Swarm::Pose DP_old_to_new; - std::vector inliers; - std::vector camera_indices; - std::vector> index2dirindex_a, index2dirindex_b; - - success = computeCorrespondFeaturesOnImageArray(frame_array_a, frame_array_b, - main_dir_a, main_dir_b, lm_pos_a, lm_norm_3d_b, camera_indices, index2dirindex_a, index2dirindex_b); - - if(success) { - std::vector extrinsics; - for (auto & img: frame_array_b.images) { - extrinsics.push_back(img.extrinsic); - } - success = computeRelativePosePnPnonCentral(lm_pos_a, lm_norm_3d_b, - extrinsics, camera_indices, frame_array_a.pose_drone, frame_array_b.pose_drone, DP_old_to_new, inliers, _config.is_4dof); - if (!success) { - printf("[LoopDetector::computeLoop@%d] Compute relative pose failed!\n", self_id); - return false; - } - - //setup return loop - ret.relative_pose = DP_old_to_new.toROS(); - ret.drone_id_a = frame_array_b.drone_id; - ret.ts_a = ros::Time(frame_array_b.stamp); - - ret.drone_id_b = frame_array_a.drone_id; - ret.ts_b = ros::Time(frame_array_a.stamp); - - ret.self_pose_a = toROSPose(frame_array_b.pose_drone); - ret.self_pose_b = toROSPose(frame_array_a.pose_drone); - - ret.keyframe_id_a = frame_array_b.frame_id; - ret.keyframe_id_b = frame_array_a.frame_id; - - ret.pos_cov.x = _config.loop_cov_pos; - ret.pos_cov.y = _config.loop_cov_pos; - ret.pos_cov.z = _config.loop_cov_pos; - - ret.ang_cov.x = _config.loop_cov_ang; - ret.ang_cov.y = _config.loop_cov_ang; - ret.ang_cov.z = _config.loop_cov_ang; - - ret.pnp_inlier_num = inliers.size(); - ret.id = self_id*MAX_LOOP_ID + loop_count; - - if (checkLoopOdometryConsistency(ret)) { - loop_count ++; - printf("[LoopDetector] Loop %ld Detected %d->%d dt %3.3fs DPose %s inliers %d. Will publish\n", - ret.id, ret.drone_id_a, ret.drone_id_b, (ret.ts_b - ret.ts_a).toSec(), - DP_old_to_new.toStr().c_str(), ret.pnp_inlier_num); - - int new_d_id = frame_array_a.drone_id; - int old_d_id = frame_array_b.drone_id; - inter_drone_loop_count[new_d_id][old_d_id] = inter_drone_loop_count[new_d_id][old_d_id] +1; - inter_drone_loop_count[old_d_id][new_d_id] = inter_drone_loop_count[old_d_id][new_d_id] +1; - } else { - success = false; - printf("[LoopDetector] Loop not consistency with odometry, give up.\n"); - } - } - - if (params->show) { - drawMatched(frame_array_a, frame_array_b, main_dir_a, main_dir_b, success, inliers, DP_old_to_new, index2dirindex_a, index2dirindex_b); - } - - return success; +bool LoopDetector::computeCorrespondFeatures( + const VisualImageDesc &img_desc_a, const VisualImageDesc &img_desc_b, + std::vector &lm_pos_a, std::vector &idx_a, + std::vector &lm_norm_3d_b, std::vector &idx_b, + std::vector &cam_indices) { + std::vector _matches; + auto &_a_lms = img_desc_a.landmarks; + auto &_b_lms = img_desc_b.landmarks; + + if (_config.enable_superglue) { + auto kpts_a = img_desc_a.landmarks2D(true, true); + auto kpts_b = img_desc_b.landmarks2D(true, true); + auto &desc0 = img_desc_a.landmark_descriptor; + auto &desc1 = img_desc_b.landmark_descriptor; + auto &scores0 = img_desc_a.landmark_scores; + auto &scores1 = img_desc_b.landmark_scores; + _matches = + superglue->inference(kpts_a, kpts_b, desc0, desc1, scores0, scores1); + } else { + assert(img_desc_a.spLandmarkNum() * params->superpoint_dims == + img_desc_a.landmark_descriptor.size() && + "Desciptor size of new img desc must equal to to landmarks*256!!!"); + assert(img_desc_b.spLandmarkNum() * params->superpoint_dims == + img_desc_b.landmark_descriptor.size() && + "Desciptor size of old img desc must equal to to landmarks*256!!!"); + cv::Mat descriptors_a(img_desc_a.spLandmarkNum(), params->superpoint_dims, + CV_32F); + memcpy(descriptors_a.data, img_desc_a.landmark_descriptor.data(), + img_desc_a.landmark_descriptor.size() * sizeof(float)); + cv::Mat descriptors_b(img_desc_b.spLandmarkNum(), params->superpoint_dims, + CV_32F); + memcpy(descriptors_b.data, img_desc_b.landmark_descriptor.data(), + img_desc_b.landmark_descriptor.size() * sizeof(float)); + if (_config.enable_knn_match) { + _matches = + matchKNN(descriptors_a, descriptors_b, _config.knn_match_ratio); + } else { + cv::BFMatcher bfmatcher(cv::NORM_L2, true); + bfmatcher.match(descriptors_a, descriptors_b, _matches); + } + } + Point2fVector lm_b_2d, lm_a_2d; + std::lock_guard guard(landmark_mutex); + for (auto match : _matches) { + int index_a = match.queryIdx; + int index_b = match.trainIdx; + auto landmark_id = _a_lms[index_a].landmark_id; + if (landmark_db.find(landmark_id) == landmark_db.end()) { + continue; + } + if (landmark_db.at(landmark_id).flag == LandmarkFlag::UNINITIALIZED || + landmark_db.at(landmark_id).flag == LandmarkFlag::OUTLIER) { + // ROS_WARN("Landmark %ld is not estimated or is outlier tracks %ld multi + // %ld", landmark_id, + // landmark_db.at(landmark_id).track.size(), + // landmark_db.at(landmark_id).isMultiCamera()); + continue; + } + Vector3d pt3d_norm_b = _b_lms[index_b].pt3d_norm; + lm_a_2d.push_back(_a_lms[index_a].pt2d); + lm_b_2d.push_back(_b_lms[index_b].pt2d); + idx_a.push_back(index_a); + idx_b.push_back(index_b); + lm_pos_a.push_back(landmark_db.at(landmark_id).position); + lm_norm_3d_b.push_back(pt3d_norm_b); + cam_indices.push_back(img_desc_b.camera_index); + } + + if (lm_b_2d.size() < 4) { + return false; + } + if (_config.enable_homography_test && !_config.enable_superglue) { + std::vector mask; + cv::findHomography(lm_b_2d, lm_a_2d, cv::RANSAC, 10.0, mask); + reduceVector(idx_a, mask); + reduceVector(idx_b, mask); + reduceVector(lm_pos_a, mask); + reduceVector(lm_norm_3d_b, mask); + } + return true; } -void LoopDetector::drawMatched(const VisualImageDescArray & frame_array_a, - const VisualImageDescArray & frame_array_b, int main_dir_a, int main_dir_b, - bool success, std::vector inliers, Swarm::Pose DP_b_to_a, - std::vector> index2dirindex_a, - std::vector> index2dirindex_b) { - cv::Mat show; - char title[100] = {0}; - std::vector _matched_imgs; - auto & imgs_a = msgid2cvimgs[frame_array_a.frame_id]; - auto & imgs_b = msgid2cvimgs[frame_array_b.frame_id]; - _matched_imgs.resize(imgs_b.size()); - for (size_t i = 0; i < imgs_b.size(); i ++) { - int dir_a = ((-main_dir_b + main_dir_a + _config.MAX_DIRS) % _config.MAX_DIRS + i)% _config.MAX_DIRS; - if (!imgs_b[i].empty() && !imgs_a[dir_a].empty()) { - cv::vconcat(imgs_b[i], imgs_a[dir_a], _matched_imgs[i]); - if (_matched_imgs[i].channels() != 3) { - cv::cvtColor(_matched_imgs[i], _matched_imgs[i], cv::COLOR_GRAY2BGR); - } - } - } - std::set inlier_set(inliers.begin(), inliers.end()); - - for (int i = 0; i < index2dirindex_a.size(); i ++) { - int old_pt_id = index2dirindex_b[i].second; - int old_dir_id = index2dirindex_b[i].first; - - int new_pt_id = index2dirindex_a[i].second; - int new_dir_id = index2dirindex_a[i].first; - auto pt_old = frame_array_b.images[old_dir_id].landmarks[old_pt_id].pt2d; - auto pt_new = frame_array_a.images[new_dir_id].landmarks[new_pt_id].pt2d; - if (_matched_imgs[old_dir_id].empty()) { - continue; - } - cv::Scalar color(rand() % 255, rand() % 255, rand() % 255); - int line_thickness = 2; - if (inlier_set.find(i) == inlier_set.end()) { - color = cv::Scalar(0, 0, 0); - line_thickness = 1; - } - cv::line(_matched_imgs[old_dir_id], pt_old, pt_new + cv::Point2f(0, imgs_b[old_dir_id].rows), color, line_thickness); - cv::circle(_matched_imgs[old_dir_id], pt_old, 3, color, 1); - cv::circle(_matched_imgs[new_dir_id], pt_new + cv::Point2f(0, imgs_b[old_dir_id].rows), 3, color, 1); - } - - - show = _matched_imgs[0]; - for (size_t i = 1; i < _matched_imgs.size(); i ++) { - if (_matched_imgs[i].empty()) continue; - cv::line(_matched_imgs[i], cv::Point2f(0, 0), cv::Point2f(0, _matched_imgs[i].rows), cv::Scalar(255, 255, 0), 2); - cv::hconcat(show, _matched_imgs[i], show); - } - - double dt = (frame_array_a.stamp - frame_array_b.stamp); +// Require 3d points of frame a and 2d point of frame b +bool LoopDetector::computeLoop(const VisualImageDescArray &frame_array_a, + const VisualImageDescArray &frame_array_b, + int main_dir_a, int main_dir_b, LoopEdge &ret) { + if (frame_array_a.spLandmarkNum() < _config.loop_inlier_feature_num) { + return false; + } + // Recover imformation + + assert(frame_array_a.drone_id == self_id && + "frame_array_a must from self drone to provide more 2d points!"); + + bool success = false; + + double t_b = frame_array_b.stamp - t0; + double t_a = frame_array_a.stamp - t0; + printf( + "[LoopDetector::computeLoop@%d] Compute loop drone b %d(d%d,dir %d)->a " + "%d(d%d,dir %d) t %.1f->%.1f(%.1f)s landmarks %d:%d.\n", + self_id, frame_array_b.frame_id, frame_array_b.drone_id, main_dir_b, + frame_array_a.frame_id, frame_array_a.drone_id, main_dir_a, t_b, t_a, + t_a - t_b, frame_array_b.spLandmarkNum(), frame_array_a.spLandmarkNum()); + + std::vector lm_pos_a; + std::vector lm_norm_3d_b; + std::vector dirs_a; + Swarm::Pose DP_old_to_new; + std::vector inliers; + std::vector camera_indices; + std::vector> index2dirindex_a, index2dirindex_b; + + success = computeCorrespondFeaturesOnImageArray( + frame_array_a, frame_array_b, main_dir_a, main_dir_b, lm_pos_a, + lm_norm_3d_b, camera_indices, index2dirindex_a, index2dirindex_b); + + if (success) { + std::vector extrinsics; + for (auto &img : frame_array_b.images) { + extrinsics.push_back(img.extrinsic); + } + success = computeRelativePosePnPnonCentral( + lm_pos_a, lm_norm_3d_b, extrinsics, camera_indices, + frame_array_a.pose_drone, frame_array_b.pose_drone, DP_old_to_new, + inliers, _config.is_4dof); if (success) { - auto ypr = DP_b_to_a.rpy()*180/M_PI; - sprintf(title, "Loop: %d->%d dt %3.1fs LM %d/%d", - frame_array_b.drone_id, frame_array_a.drone_id, dt, inliers.size(), index2dirindex_a.size()); - cv::putText(show, title, cv::Point2f(20, 30), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(0, 255, 0), 2); - - sprintf(title, "%s", DP_b_to_a.toStr().c_str()); - cv::putText(show, title, cv::Point2f(20, 60), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(0, 255, 0), 2); - sprintf(title, "%d<->%d", frame_array_b.frame_id, frame_array_a.frame_id); - cv::putText(show, title, cv::Point2f(20, 90), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(0, 255, 0), 2); - sprintf(title, "Ego A: %s", frame_array_a.pose_drone.toStr().c_str()); - cv::putText(show, title, cv::Point2f(20, 120), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(0, 255, 0), 2); - sprintf(title, "Ego B: %s", frame_array_b.pose_drone.toStr().c_str()); - cv::putText(show, title, cv::Point2f(20, 150), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(0, 255, 0), 2); + // setup return loop + ret.relative_pose = DP_old_to_new.toROS(); + ret.drone_id_a = frame_array_b.drone_id; + ret.ts_a = ros::Time(frame_array_b.stamp); + + ret.drone_id_b = frame_array_a.drone_id; + ret.ts_b = ros::Time(frame_array_a.stamp); + + ret.self_pose_a = toROSPose(frame_array_b.pose_drone); + ret.self_pose_b = toROSPose(frame_array_a.pose_drone); + + ret.keyframe_id_a = frame_array_b.frame_id; + ret.keyframe_id_b = frame_array_a.frame_id; + + ret.pos_cov.x = _config.loop_cov_pos; + ret.pos_cov.y = _config.loop_cov_pos; + ret.pos_cov.z = _config.loop_cov_pos; + + ret.ang_cov.x = _config.loop_cov_ang; + ret.ang_cov.y = _config.loop_cov_ang; + ret.ang_cov.z = _config.loop_cov_ang; + + ret.pnp_inlier_num = inliers.size(); + ret.id = self_id * MAX_LOOP_ID + loop_count; + + if (checkLoopOdometryConsistency(ret)) { + loop_count++; + SPDLOG_INFO( + "[LoopDetector] Loop {} Detected {}->{} dt {:.3f}s DPose {} " + "inliers {}. Will publish\n", + ret.id, ret.drone_id_a, ret.drone_id_b, + (ret.ts_b - ret.ts_a).toSec(), DP_old_to_new.toStr().c_str(), + ret.pnp_inlier_num); + + int new_d_id = frame_array_a.drone_id; + int old_d_id = frame_array_b.drone_id; + inter_drone_loop_count[new_d_id][old_d_id] = + inter_drone_loop_count[new_d_id][old_d_id] + 1; + inter_drone_loop_count[old_d_id][new_d_id] = + inter_drone_loop_count[old_d_id][new_d_id] + 1; + } else { + success = false; + SPDLOG_INFO( + "[LoopDetector] Loop not consistency with odometry, give up.\n"); + } + } + } + + if (params->show) { + drawMatched(frame_array_a, frame_array_b, main_dir_a, main_dir_b, success, + inliers, DP_old_to_new, index2dirindex_a, index2dirindex_b); + } + + return success; +} - } else { - sprintf(title, "FAILED %d->%d dt %3.1fs LM %d/%d", frame_array_b.drone_id, frame_array_a.drone_id, dt, inliers.size(), +void LoopDetector::drawMatched( + const VisualImageDescArray &frame_array_a, + const VisualImageDescArray &frame_array_b, int main_dir_a, int main_dir_b, + bool success, std::vector inliers, Swarm::Pose DP_b_to_a, + std::vector> index2dirindex_a, + std::vector> index2dirindex_b) { + cv::Mat show; + char title[100] = {0}; + std::vector _matched_imgs; + auto &imgs_a = msgid2cvimgs[frame_array_a.frame_id]; + auto &imgs_b = msgid2cvimgs[frame_array_b.frame_id]; + _matched_imgs.resize(imgs_b.size()); + for (size_t i = 0; i < imgs_b.size(); i++) { + int dir_a = + ((-main_dir_b + main_dir_a + _config.MAX_DIRS) % _config.MAX_DIRS + i) % + _config.MAX_DIRS; + if (!imgs_b[i].empty() && !imgs_a[dir_a].empty()) { + cv::vconcat(imgs_b[i], imgs_a[dir_a], _matched_imgs[i]); + if (_matched_imgs[i].channels() != 3) { + cv::cvtColor(_matched_imgs[i], _matched_imgs[i], cv::COLOR_GRAY2BGR); + } + } + } + std::set inlier_set(inliers.begin(), inliers.end()); + + for (int i = 0; i < index2dirindex_a.size(); i++) { + int old_pt_id = index2dirindex_b[i].second; + int old_dir_id = index2dirindex_b[i].first; + + int new_pt_id = index2dirindex_a[i].second; + int new_dir_id = index2dirindex_a[i].first; + auto pt_old = frame_array_b.images[old_dir_id].landmarks[old_pt_id].pt2d; + auto pt_new = frame_array_a.images[new_dir_id].landmarks[new_pt_id].pt2d; + if (_matched_imgs[old_dir_id].empty()) { + continue; + } + cv::Scalar color(rand() % 255, rand() % 255, rand() % 255); + int line_thickness = 2; + if (inlier_set.find(i) == inlier_set.end()) { + color = cv::Scalar(0, 0, 0); + line_thickness = 1; + } + cv::line(_matched_imgs[old_dir_id], pt_old, + pt_new + cv::Point2f(0, imgs_b[old_dir_id].rows), color, + line_thickness); + cv::circle(_matched_imgs[old_dir_id], pt_old, 3, color, 1); + cv::circle(_matched_imgs[new_dir_id], + pt_new + cv::Point2f(0, imgs_b[old_dir_id].rows), 3, color, 1); + } + + show = _matched_imgs[0]; + for (size_t i = 1; i < _matched_imgs.size(); i++) { + if (_matched_imgs[i].empty()) continue; + cv::line(_matched_imgs[i], cv::Point2f(0, 0), + cv::Point2f(0, _matched_imgs[i].rows), cv::Scalar(255, 255, 0), 2); + cv::hconcat(show, _matched_imgs[i], show); + } + + double dt = (frame_array_a.stamp - frame_array_b.stamp); + if (success) { + auto ypr = DP_b_to_a.rpy() * 180 / M_PI; + sprintf(title, "Loop: %d->%d dt %3.1fs LM %d/%d", frame_array_b.drone_id, + frame_array_a.drone_id, dt, inliers.size(), index2dirindex_a.size()); - cv::putText(show, title, cv::Point2f(20, 30), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(0, 0, 255), 2); - } - - // cv::resize(show, show, cv::Size(), 2, 2); - static int loop_match_count = 0; - loop_match_count ++; - char PATH[100] = {0}; - - if (!show.empty() && success) { - sprintf(PATH, "loop/match%d.png", loop_match_count); - cv::imwrite(params->OUTPUT_PATH+PATH, show); - } - cv::imshow("Matches", show); - cv::waitKey(10); + cv::putText(show, title, cv::Point2f(20, 30), cv::FONT_HERSHEY_SIMPLEX, 1, + cv::Scalar(0, 255, 0), 2); + + sprintf(title, "%s", DP_b_to_a.toStr().c_str()); + cv::putText(show, title, cv::Point2f(20, 60), cv::FONT_HERSHEY_SIMPLEX, 1, + cv::Scalar(0, 255, 0), 2); + sprintf(title, "%d<->%d", frame_array_b.frame_id, frame_array_a.frame_id); + cv::putText(show, title, cv::Point2f(20, 90), cv::FONT_HERSHEY_SIMPLEX, 1, + cv::Scalar(0, 255, 0), 2); + sprintf(title, "Ego A: %s", frame_array_a.pose_drone.toStr().c_str()); + cv::putText(show, title, cv::Point2f(20, 120), cv::FONT_HERSHEY_SIMPLEX, 1, + cv::Scalar(0, 255, 0), 2); + sprintf(title, "Ego B: %s", frame_array_b.pose_drone.toStr().c_str()); + cv::putText(show, title, cv::Point2f(20, 150), cv::FONT_HERSHEY_SIMPLEX, 1, + cv::Scalar(0, 255, 0), 2); + + } else { + sprintf(title, "FAILED %d->%d dt %3.1fs LM %d/%d", frame_array_b.drone_id, + frame_array_a.drone_id, dt, inliers.size(), + index2dirindex_a.size()); + cv::putText(show, title, cv::Point2f(20, 30), cv::FONT_HERSHEY_SIMPLEX, 1, + cv::Scalar(0, 0, 255), 2); + } + + // cv::resize(show, show, cv::Size(), 2, 2); + static int loop_match_count = 0; + loop_match_count++; + char PATH[100] = {0}; + + if (!show.empty() && success) { + sprintf(PATH, "loop/match%d.png", loop_match_count); + cv::imwrite(params->OUTPUT_PATH + PATH, show); + } + cv::imshow("Matches", show); + cv::waitKey(10); } -void LoopDetector::onLoopConnection(LoopEdge & loop_conn) { - on_loop_cb(loop_conn); +void LoopDetector::onLoopConnection(LoopEdge &loop_conn) { + on_loop_cb(loop_conn); } -void LoopDetector::updatebyLandmarkDB(const std::map & vins_landmark_db) { - std::lock_guard guard(landmark_mutex); - for (auto it : vins_landmark_db) { - auto landmark_id = it.first; - if (landmark_db.find(landmark_id) == landmark_db.end()) { - landmark_db[landmark_id] = it.second; - } else { - if (it.second.flag == LandmarkFlag::INITIALIZED || it.second.flag == LandmarkFlag::ESTIMATED) { - landmark_db[landmark_id] = it.second; - } - } +void LoopDetector::updatebyLandmarkDB( + const std::map &vins_landmark_db) { + std::lock_guard guard(landmark_mutex); + for (auto it : vins_landmark_db) { + auto landmark_id = it.first; + if (landmark_db.find(landmark_id) == landmark_db.end()) { + landmark_db[landmark_id] = it.second; + } else { + if (it.second.flag == LandmarkFlag::INITIALIZED || + it.second.flag == LandmarkFlag::ESTIMATED) { + landmark_db[landmark_id] = it.second; + } } + } } -void LoopDetector::updatebySldWin(const std::vector sld_win) { - const std::lock_guard lock(keyframe_database_mutex); - for (auto frame : sld_win) { - auto frame_id = frame->frame_id; - if (keyframe_database.find(frame_id) != keyframe_database.end()) { - keyframe_database.at(frame_id).pose_drone = frame->odom.pose(); - } +void LoopDetector::updatebySldWin(const std::vector sld_win) { + const std::lock_guard lock(keyframe_database_mutex); + for (auto frame : sld_win) { + auto frame_id = frame->frame_id; + if (keyframe_database.find(frame_id) != keyframe_database.end()) { + keyframe_database.at(frame_id).pose_drone = frame->odom.pose(); } + } } bool LoopDetector::hasFrame(FrameIdType frame_id) { - const std::lock_guard lock(keyframe_database_mutex); - return keyframe_database.find(frame_id) != keyframe_database.end(); + const std::lock_guard lock(keyframe_database_mutex); + return keyframe_database.find(frame_id) != keyframe_database.end(); } -LoopDetector::LoopDetector(int _self_id, const LoopDetectorConfig & config): - self_id(_self_id), - _config(config), - local_index(params->netvlad_dims), - remote_index(params->netvlad_dims), - ego_motion_traj(_self_id, true, _config.pos_covariance_per_meter, _config.yaw_covariance_per_meter) { - if (_config.enable_superglue) { - superglue = new SuperGlueOnnx(_config.superglue_model_path); - } +LoopDetector::LoopDetector(int _self_id, const LoopDetectorConfig &config) + : self_id(_self_id), + _config(config), + local_index(params->netvlad_dims), + remote_index(params->netvlad_dims), + ego_motion_traj(_self_id, true, _config.pos_covariance_per_meter, + _config.yaw_covariance_per_meter) { + if (_config.enable_superglue) { + superglue = new SuperGlueOnnx(_config.superglue_model_path); + } } -} \ No newline at end of file +} // namespace D2FrontEnd \ No newline at end of file diff --git a/d2frontend/src/loop_net.cpp b/d2frontend/src/loop_net.cpp index c2ead3e9..0c3e409b 100644 --- a/d2frontend/src/loop_net.cpp +++ b/d2frontend/src/loop_net.cpp @@ -1,406 +1,483 @@ #include -#include -#include "d2frontend/loop_detector.h" +#include + #include +#include "d2frontend/loop_detector.h" +#include "spdlog/spdlog.h" + namespace D2FrontEnd { void LoopNet::setupNetwork(std::string _lcm_uri) { - if (!lcm.good()) { - ROS_ERROR("LCM %s failed", _lcm_uri.c_str()); - exit(-1); - } - lcm.subscribe("VIOKF_HEADER", &LoopNet::onImgDescHeaderRecevied, this); - lcm.subscribe("VIOKF_LANDMARKS", &LoopNet::onLandmarkRecevied, this); - lcm.subscribe("VIOKF_IMG_ARRAY", &LoopNet::onImgArrayRecevied, this); - lcm.subscribe("SWARM_LOOP_CONN", &LoopNet::onLoopConnectionRecevied, this); - - srand((unsigned)time(NULL)); - msg_recv_rate_callback = [&](int drone_id, float rate) {}; + if (!lcm.good()) { + ROS_ERROR("LCM %s failed", _lcm_uri.c_str()); + exit(-1); + } + lcm.subscribe("VIOKF_HEADER", &LoopNet::onImgDescHeaderRecevied, this); + lcm.subscribe("VIOKF_LANDMARKS", &LoopNet::onLandmarkRecevied, this); + lcm.subscribe("VIOKF_IMG_ARRAY", &LoopNet::onImgArrayRecevied, this); + lcm.subscribe("SWARM_LOOP_CONN", &LoopNet::onLoopConnectionRecevied, this); + + srand((unsigned)time(NULL)); + msg_recv_rate_callback = [&](int drone_id, float rate) {}; } -void LoopNet::broadcastVisualImageDescArray(VisualImageDescArray & image_array, bool force_features) { - bool need_send_features = force_features || !params->lazy_broadcast_keyframe; //TODO: need to consider for D2VINS. - bool need_send_netvlad = true; - bool only_match_relationship = false; - if (sent_image_arrays.find(image_array.frame_id) != sent_image_arrays.end()) { - printf("[LoopNet@%d] image array %ld already sent, will only send the matching relationship\n", params->self_id, image_array.frame_id); - need_send_features = false; - need_send_netvlad = false; - only_match_relationship = true; - } - ImageArrayDescriptor_t fisheye_desc = image_array.toLCM(need_send_features, compress_int8_desc, need_send_netvlad); - if (need_send_features) { - //Only label the image array as sent if we are sending the features. - sent_image_arrays.insert(image_array.frame_id); +void LoopNet::broadcastVisualImageDescArray(VisualImageDescArray& image_array, + bool force_features) { + bool need_send_features = + force_features || + !params->lazy_broadcast_keyframe; // TODO: need to consider for D2VINS. + bool need_send_netvlad = true; + bool only_match_relationship = false; + if (sent_image_arrays.find(image_array.frame_id) != sent_image_arrays.end()) { + printf( + "[LoopNet@%d] image array %ld already sent, will only send the " + "matching relationship\n", + params->self_id, image_array.frame_id); + need_send_features = false; + need_send_netvlad = false; + only_match_relationship = true; + } + ImageArrayDescriptor_t fisheye_desc = image_array.toLCM( + need_send_features, compress_int8_desc, need_send_netvlad); + if (need_send_features) { + // Only label the image array as sent if we are sending the features. + sent_image_arrays.insert(image_array.frame_id); + } + spdlog::debug( + "[LoopNet@{}] broadcast image array: {} lazy: {} size {} " + "need_send_features {}", + params->self_id, fisheye_desc.frame_id, params->lazy_broadcast_keyframe, + fisheye_desc.getEncodedSize(), need_send_features); + if (send_whole_img_desc) { + sent_message.insert(fisheye_desc.msg_id); + lcm.publish("VIOKF_IMG_ARRAY", &fisheye_desc); + if (params->print_network_status) { + int feature_num = fisheye_desc.landmark_num; + int byte_sent = fisheye_desc.getEncodedSize(); + sum_byte_sent += byte_sent; + count_img_desc_sent++; + sum_features += feature_num; + printf( + "[SWARM_LOOP](%d) BD KF %d LM: %d size %d avgsize %.0f sumkB %.0f " + "avgLM %.0f force_features:%d\n", + count_img_desc_sent, fisheye_desc.frame_id, feature_num, byte_sent, + ceil(sum_byte_sent / count_img_desc_sent), sum_byte_sent / 1000, + ceil(sum_features / count_img_desc_sent), force_features); } - printf("[LoopNet@%d] broadcast image array: %ld lazy: %d size %d need_send_features %d\n", params->self_id, fisheye_desc.frame_id, - params->lazy_broadcast_keyframe, fisheye_desc.getEncodedSize(), need_send_features); - if (send_whole_img_desc) { - sent_message.insert(fisheye_desc.msg_id); - lcm.publish("VIOKF_IMG_ARRAY", &fisheye_desc); - if (params->print_network_status) { - int feature_num = fisheye_desc.landmark_num; - int byte_sent = fisheye_desc.getEncodedSize(); - sum_byte_sent+= byte_sent; - count_img_desc_sent ++; - sum_features += feature_num; - printf("[SWARM_LOOP](%d) BD KF %d LM: %d size %d avgsize %.0f sumkB %.0f avgLM %.0f force_features:%d\n", count_img_desc_sent, - fisheye_desc.frame_id, feature_num, byte_sent, ceil(sum_byte_sent/count_img_desc_sent), sum_byte_sent/1000, ceil(sum_features/count_img_desc_sent), force_features); - } + } else { + if (!need_send_features && + params->camera_configuration == CameraConfig::STEREO_PINHOLE) { + auto& img = fisheye_desc.images[0]; + img.header.is_keyframe = fisheye_desc.is_keyframe; + broadcastImgDesc(img, fisheye_desc.sld_win_status, need_send_features); } else { - if (!need_send_features && params->camera_configuration == CameraConfig::STEREO_PINHOLE) { - auto & img = fisheye_desc.images[0]; - img.header.is_keyframe = fisheye_desc.is_keyframe; - broadcastImgDesc(img, fisheye_desc.sld_win_status, need_send_features); - } else { - for (auto & img : fisheye_desc.images) { - if (img.landmark_num > 0 || !need_send_features) { - img.header.is_keyframe = fisheye_desc.is_keyframe; - broadcastImgDesc(img, fisheye_desc.sld_win_status, need_send_features); - if (only_match_relationship) { - break; - } - } - } + for (auto& img : fisheye_desc.images) { + if (img.landmark_num > 0 || !need_send_features) { + img.header.is_keyframe = fisheye_desc.is_keyframe; + broadcastImgDesc(img, fisheye_desc.sld_win_status, + need_send_features); + if (only_match_relationship) { + break; + } } + } } + } } -void LoopNet::broadcastImgDesc(ImageDescriptor_t & img_des, const SlidingWindow_t & sld_status, bool need_send_features) { - int64_t msg_id = rand() + img_des.header.timestamp.nsec; - img_des.header.msg_id = msg_id; - sent_message.insert(img_des.header.msg_id); - - int byte_sent = 0; - int feature_num = img_des.landmark_num; - - ImageDescriptorHeader_t & img_desc_header = img_des.header; - img_desc_header.sld_win_status = sld_status; - img_desc_header.feature_num = feature_num; - img_desc_header.timestamp_sent = toLCMTime(ros::Time::now()); - - byte_sent += img_desc_header.getEncodedSize(); - lcm.publish("VIOKF_HEADER", &img_desc_header); - // printf("[LoopNet] Header id %ld msg_id %ld desc_size %ld:%ld\n", img_desc_header.frame_id, img_desc_header.msg_id, - // img_desc_header.image_desc_size_int8, img_desc_header.image_desc_size); - if (need_send_features) { - LandmarkDescriptorPacket_t * lm_pack = new LandmarkDescriptorPacket_t(); - lm_pack->desc_len = 0; - lm_pack->desc_len_int8 = 0; - for (size_t i = 0; i < img_des.landmark_num; i++ ) { - if (img_des.landmarks[i].type == LandmarkType::SuperPointLandmark) { - lm_pack->landmarks.emplace_back(img_des.landmarks[i].compact); - if (img_des.landmark_descriptor_int8.size() > 0) { - lm_pack->desc_len_int8 += params->superpoint_dims; - lm_pack->landmark_descriptor_int8.insert(lm_pack->landmark_descriptor_int8.end(), - img_des.landmark_descriptor_int8.data() + i *params->superpoint_dims, - img_des.landmark_descriptor_int8.data() + (i+1)*params->superpoint_dims); - lm_pack->desc_len = 0; - } else { - lm_pack->desc_len += params->superpoint_dims; - lm_pack->landmark_descriptor.insert(lm_pack->landmark_descriptor.end(), - img_des.landmark_descriptor.data() + i *params->superpoint_dims, - img_des.landmark_descriptor.data() + (i+1)*params->superpoint_dims); - lm_pack->desc_len_int8 = 0; - } - - if (lm_pack->landmarks.size() > pack_landmark_num || i == img_des.landmark_num - 1) { - lm_pack->msg_id = rand() + img_des.header.timestamp.nsec; - lm_pack->header_id = img_des.header.msg_id; - lm_pack->landmark_num = lm_pack->landmarks.size(); - sent_message.insert(msg_id); - byte_sent += lm_pack->getEncodedSize(); - if (params->print_network_status) { - // printf("[LoopNet] BD LMPack LM size %d(%dx%d) superpoint_dims %d lm.landmark_descriptor_int8 %d\n", lm_pack->getEncodedSize(), - // lm_pack->landmarks.size(), lm_pack->landmarks[0].getEncodedSize(), params->superpoint_dims, lm_pack->landmark_descriptor_int8.size()); - } - // lm_pack->timestamp_sent = toLCMTime(ros::Time::now()); - lcm.publish("VIOKF_LANDMARKS", lm_pack); - delete lm_pack; - if (i != img_des.landmark_num - 1) { - lm_pack = new LandmarkDescriptorPacket_t(); - lm_pack->desc_len = 0; - lm_pack->desc_len_int8 = 0; - } - } - } +void LoopNet::broadcastImgDesc(ImageDescriptor_t& img_des, + const SlidingWindow_t& sld_status, + bool need_send_features) { + int64_t msg_id = rand() + img_des.header.timestamp.nsec; + img_des.header.msg_id = msg_id; + sent_message.insert(img_des.header.msg_id); + + int byte_sent = 0; + int feature_num = img_des.landmark_num; + + ImageDescriptorHeader_t& img_desc_header = img_des.header; + img_desc_header.sld_win_status = sld_status; + img_desc_header.feature_num = feature_num; + img_desc_header.timestamp_sent = toLCMTime(ros::Time::now()); + + byte_sent += img_desc_header.getEncodedSize(); + lcm.publish("VIOKF_HEADER", &img_desc_header); + // printf("[LoopNet] Header id %ld msg_id %ld desc_size %ld:%ld\n", + // img_desc_header.frame_id, img_desc_header.msg_id, + // img_desc_header.image_desc_size_int8, img_desc_header.image_desc_size); + if (need_send_features) { + LandmarkDescriptorPacket_t* lm_pack = new LandmarkDescriptorPacket_t(); + lm_pack->desc_len = 0; + lm_pack->desc_len_int8 = 0; + for (size_t i = 0; i < img_des.landmark_num; i++) { + if (img_des.landmarks[i].type == LandmarkType::SuperPointLandmark) { + lm_pack->landmarks.emplace_back(img_des.landmarks[i].compact); + if (img_des.landmark_descriptor_int8.size() > 0) { + lm_pack->desc_len_int8 += params->superpoint_dims; + lm_pack->landmark_descriptor_int8.insert( + lm_pack->landmark_descriptor_int8.end(), + img_des.landmark_descriptor_int8.data() + + i * params->superpoint_dims, + img_des.landmark_descriptor_int8.data() + + (i + 1) * params->superpoint_dims); + lm_pack->desc_len = 0; + } else { + lm_pack->desc_len += params->superpoint_dims; + lm_pack->landmark_descriptor.insert( + lm_pack->landmark_descriptor.end(), + img_des.landmark_descriptor.data() + i * params->superpoint_dims, + img_des.landmark_descriptor.data() + + (i + 1) * params->superpoint_dims); + lm_pack->desc_len_int8 = 0; } - } - sum_byte_sent+= byte_sent; - sum_features+=feature_num; - count_img_desc_sent ++; - if (params->print_network_status) { - printf("[SWARM_LOOP](%d) BD KF %d@%d LM: %d size %d header %d avgsize %.0f sumkB %.0f avgLM %.0f need_send_features: %d\n", count_img_desc_sent, - img_desc_header.frame_id, img_desc_header.camera_index, feature_num, byte_sent, img_desc_header.getEncodedSize(), - ceil(sum_byte_sent/count_img_desc_sent), sum_byte_sent/1024, ceil(sum_features/count_img_desc_sent), need_send_features); + if (lm_pack->landmarks.size() > pack_landmark_num || + i == img_des.landmark_num - 1) { + lm_pack->msg_id = rand() + img_des.header.timestamp.nsec; + lm_pack->header_id = img_des.header.msg_id; + lm_pack->landmark_num = lm_pack->landmarks.size(); + sent_message.insert(msg_id); + byte_sent += lm_pack->getEncodedSize(); + if (params->print_network_status) { + // printf("[LoopNet] BD LMPack LM size %d(%dx%d) superpoint_dims %d + // lm.landmark_descriptor_int8 %d\n", lm_pack->getEncodedSize(), + // lm_pack->landmarks.size(), + // lm_pack->landmarks[0].getEncodedSize(), + // params->superpoint_dims, + // lm_pack->landmark_descriptor_int8.size()); + } + // lm_pack->timestamp_sent = toLCMTime(ros::Time::now()); + lcm.publish("VIOKF_LANDMARKS", lm_pack); + delete lm_pack; + if (i != img_des.landmark_num - 1) { + lm_pack = new LandmarkDescriptorPacket_t(); + lm_pack->desc_len = 0; + lm_pack->desc_len_int8 = 0; + } + } + } } + } + + sum_byte_sent += byte_sent; + sum_features += feature_num; + count_img_desc_sent++; + if (params->print_network_status) { + printf( + "[SWARM_LOOP](%d) BD KF %d@%d LM: %d size %d header %d avgsize %.0f " + "sumkB %.0f avgLM %.0f need_send_features: %d\n", + count_img_desc_sent, img_desc_header.frame_id, + img_desc_header.camera_index, feature_num, byte_sent, + img_desc_header.getEncodedSize(), + ceil(sum_byte_sent / count_img_desc_sent), sum_byte_sent / 1024, + ceil(sum_features / count_img_desc_sent), need_send_features); + } } -void LoopNet::broadcastLoopConnection(swarm_msgs::LoopEdge & loop_conn) { - auto _loop_conn = toLCMLoopEdge(loop_conn); - sent_message.insert(_loop_conn.id); - lcm.publish("SWARM_LOOP_CONN", &_loop_conn); +void LoopNet::broadcastLoopConnection(swarm_msgs::LoopEdge& loop_conn) { + auto _loop_conn = toLCMLoopEdge(loop_conn); + sent_message.insert(_loop_conn.id); + lcm.publish("SWARM_LOOP_CONN", &_loop_conn); } void LoopNet::onImgArrayRecevied(const lcm::ReceiveBuffer* rbuf, - const std::string& chan, - const ImageArrayDescriptor_t* msg) { - std::lock_guard Guard(recv_lock); - if (sent_message.find(msg->msg_id) == sent_message.end()) { - frame_desc_callback(*msg); - } + const std::string& chan, + const ImageArrayDescriptor_t* msg) { + std::lock_guard Guard(recv_lock); + if (sent_message.find(msg->msg_id) == sent_message.end()) { + frame_desc_callback(*msg); + } } void LoopNet::onLandmarkRecevied(const lcm::ReceiveBuffer* rbuf, - const std::string& chan, - const LandmarkDescriptorPacket_t* msg) { - std::lock_guard Guard(recv_lock); - if(msgBlocked(msg->header_id)) { - return; - } - updateRecvImgDescTs(msg->header_id, false); - if (received_images.find(msg->header_id) == received_images.end()) { - //May happen when the image is not received yet - return; - } - - auto & tmp = received_images[msg->header_id]; - for (auto landmark: msg->landmarks) { - tmp.landmarks.emplace_back(Landmark_t()); - auto & new_lm = tmp.landmarks.back(); - new_lm.compact = landmark; - new_lm.camera_id = tmp.header.camera_id; - new_lm.camera_index = tmp.header.camera_index; - new_lm.frame_id = tmp.header.frame_id; - new_lm.drone_id = tmp.header.drone_id; - new_lm.type = LandmarkType::SuperPointLandmark; - new_lm.timestamp = tmp.header.timestamp; - new_lm.cur_td = tmp.header.cur_td; - } - // printf("[LoopNet] Recv LMPack: msg_id %ld attached to frame %ldc%d current lms %ld\n", msg->header_id, tmp.header.frame_id, tmp.header.camera_index, - // tmp.landmarks.size()); - - if (msg->landmark_descriptor_int8.size() > 0) { - tmp.landmark_descriptor_int8.insert(tmp.landmark_descriptor_int8.end(), msg->landmark_descriptor_int8.begin(), msg->landmark_descriptor_int8.end()); - tmp.landmark_descriptor_size_int8 = tmp.landmark_descriptor_int8.size(); - tmp.landmark_descriptor_size = 0; - // printf("Adding landmark_descriptor_int8 %ld to image_desc %ld: %ld\n", msg->landmark_descriptor_int8.size(), tmp.frame_id, tmp.landmark_descriptor_size_int8); - } else { - tmp.landmark_descriptor.insert(tmp.landmark_descriptor.end(), msg->landmark_descriptor.begin(), msg->landmark_descriptor.end()); - tmp.landmark_descriptor_size = tmp.landmark_descriptor.size(); - tmp.landmark_descriptor_size_int8 = 0; - } - scanRecvPackets(); + const std::string& chan, + const LandmarkDescriptorPacket_t* msg) { + std::lock_guard Guard(recv_lock); + if (msgBlocked(msg->header_id)) { + return; + } + updateRecvImgDescTs(msg->header_id, false); + if (received_images.find(msg->header_id) == received_images.end()) { + // May happen when the image is not received yet + return; + } + + auto& tmp = received_images[msg->header_id]; + for (auto landmark : msg->landmarks) { + tmp.landmarks.emplace_back(Landmark_t()); + auto& new_lm = tmp.landmarks.back(); + new_lm.compact = landmark; + new_lm.camera_id = tmp.header.camera_id; + new_lm.camera_index = tmp.header.camera_index; + new_lm.frame_id = tmp.header.frame_id; + new_lm.drone_id = tmp.header.drone_id; + new_lm.type = LandmarkType::SuperPointLandmark; + new_lm.timestamp = tmp.header.timestamp; + new_lm.cur_td = tmp.header.cur_td; + } + // printf("[LoopNet] Recv LMPack: msg_id %ld attached to frame %ldc%d current + // lms %ld\n", msg->header_id, tmp.header.frame_id, tmp.header.camera_index, + // tmp.landmarks.size()); + + if (msg->landmark_descriptor_int8.size() > 0) { + tmp.landmark_descriptor_int8.insert(tmp.landmark_descriptor_int8.end(), + msg->landmark_descriptor_int8.begin(), + msg->landmark_descriptor_int8.end()); + tmp.landmark_descriptor_size_int8 = tmp.landmark_descriptor_int8.size(); + tmp.landmark_descriptor_size = 0; + // printf("Adding landmark_descriptor_int8 %ld to image_desc %ld: %ld\n", + // msg->landmark_descriptor_int8.size(), tmp.frame_id, + // tmp.landmark_descriptor_size_int8); + } else { + tmp.landmark_descriptor.insert(tmp.landmark_descriptor.end(), + msg->landmark_descriptor.begin(), + msg->landmark_descriptor.end()); + tmp.landmark_descriptor_size = tmp.landmark_descriptor.size(); + tmp.landmark_descriptor_size_int8 = 0; + } + scanRecvPackets(); } -void LoopNet::processRecvImageDesc(const ImageDescriptor_t & image, const SlidingWindow_t & sld_win_status) { - std::lock_guard Guard(recv_lock); - int64_t frame_id = image.header.frame_id; - if (received_image_arrays.find(frame_id) == received_image_arrays.end()) { - ImageArrayDescriptor_t frame_desc; - if (params->camera_configuration == CameraConfig::STEREO_FISHEYE) { - frame_desc.image_num = 4; - } else if (params->camera_configuration == CameraConfig::STEREO_PINHOLE) { - if (image.header.is_lazy_frame) { - frame_desc.image_num = 1; - } else { - frame_desc.image_num = 2; - } - } else if (params->camera_configuration == CameraConfig::PINHOLE_DEPTH) { - frame_desc.image_num = 1; - } else if (params->camera_configuration == CameraConfig::FOURCORNER_FISHEYE) { - frame_desc.image_num = 4; - } +void LoopNet::processRecvImageDesc(const ImageDescriptor_t& image, + const SlidingWindow_t& sld_win_status) { + std::lock_guard Guard(recv_lock); + int64_t frame_id = image.header.frame_id; + if (received_image_arrays.find(frame_id) == received_image_arrays.end()) { + ImageArrayDescriptor_t frame_desc; + if (params->camera_configuration == CameraConfig::STEREO_FISHEYE) { + frame_desc.image_num = 4; + } else if (params->camera_configuration == CameraConfig::STEREO_PINHOLE) { + if (image.header.is_lazy_frame) { + frame_desc.image_num = 1; + } else { + frame_desc.image_num = 2; + } + } else if (params->camera_configuration == CameraConfig::PINHOLE_DEPTH) { + frame_desc.image_num = 1; + } else if (params->camera_configuration == + CameraConfig::FOURCORNER_FISHEYE) { + frame_desc.image_num = 4; + } - frame_desc.timestamp = image.header.timestamp; - for (size_t i = 0; i < frame_desc.image_num; i ++) { - if (i != image.header.camera_index) { - auto img_desc = generate_null_img_desc(); - frame_desc.images.push_back(img_desc); - } else { - frame_desc.images.push_back(image); - } - } + frame_desc.timestamp = image.header.timestamp; + for (size_t i = 0; i < frame_desc.image_num; i++) { + if (i != image.header.camera_index) { + auto img_desc = generate_null_img_desc(); + frame_desc.images.push_back(img_desc); + } else { + frame_desc.images.push_back(image); + } + } - frame_desc.msg_id = image.header.frame_id; - frame_desc.pose_drone = image.header.pose_drone; - frame_desc.landmark_num = 0; - frame_desc.drone_id = image.header.drone_id; - frame_desc.frame_id = image.header.frame_id; - frame_desc.is_lazy_frame = image.header.is_lazy_frame; - frame_desc.matched_frame = image.header.matched_frame; - frame_desc.matched_drone = image.header.matched_drone; - frame_desc.sld_win_status = sld_win_status; - frame_desc.reference_frame_id = image.header.reference_frame_id; - frame_desc.Ba.x = 0; - frame_desc.Ba.y = 0; - frame_desc.Ba.z = 0; - frame_desc.Bg.x = 0; - frame_desc.Bg.y = 0; - frame_desc.Bg.z = 0; - frame_desc.is_keyframe = image.header.is_keyframe; - frame_desc.cur_td = image.header.cur_td; - received_image_arrays[image.header.frame_id] = frame_desc; - frame_header_recv_time[image.header.frame_id] = msg_header_recv_time[image.header.msg_id]; - active_receving_image_array_idx.insert(image.header.frame_id); - if (params->print_network_status) { - printf("[LoopNet::processRecvImageDesc] Create frame %dc%d from D%d \n", frame_id, - image.header.camera_index, frame_desc.drone_id); - } - } else { - auto & frame_desc = received_image_arrays[frame_id]; - frame_desc.images[image.header.camera_index] = image; - if (params->print_network_status) { - printf("[LoopNet::processRecvImageDesc] Adding subframe %dc%d D%d to current\n", frame_id, - image.header.camera_index, frame_desc.drone_id); - } + frame_desc.msg_id = image.header.frame_id; + frame_desc.pose_drone = image.header.pose_drone; + frame_desc.landmark_num = 0; + frame_desc.drone_id = image.header.drone_id; + frame_desc.frame_id = image.header.frame_id; + frame_desc.is_lazy_frame = image.header.is_lazy_frame; + frame_desc.matched_frame = image.header.matched_frame; + frame_desc.matched_drone = image.header.matched_drone; + frame_desc.sld_win_status = sld_win_status; + frame_desc.reference_frame_id = image.header.reference_frame_id; + frame_desc.Ba.x = 0; + frame_desc.Ba.y = 0; + frame_desc.Ba.z = 0; + frame_desc.Bg.x = 0; + frame_desc.Bg.y = 0; + frame_desc.Bg.z = 0; + frame_desc.is_keyframe = image.header.is_keyframe; + frame_desc.cur_td = image.header.cur_td; + received_image_arrays[image.header.frame_id] = frame_desc; + frame_header_recv_time[image.header.frame_id] = + msg_header_recv_time[image.header.msg_id]; + active_receving_image_array_idx.insert(image.header.frame_id); + if (params->print_network_status) { + printf("[LoopNet::processRecvImageDesc] Create frame %dc%d from D%d \n", + frame_id, image.header.camera_index, frame_desc.drone_id); + } + } else { + auto& frame_desc = received_image_arrays[frame_id]; + frame_desc.images[image.header.camera_index] = image; + if (params->print_network_status) { + printf( + "[LoopNet::processRecvImageDesc] Adding subframe %dc%d D%d to " + "current\n", + frame_id, image.header.camera_index, frame_desc.drone_id); } + } } void LoopNet::onLoopConnectionRecevied(const lcm::ReceiveBuffer* rbuf, - const std::string& chan, - const LoopEdge_t* msg) { - - if (sent_message.find(msg->id) != sent_message.end()) { - // ROS_INFO("Receive self sent Loop message"); - return; - } - ROS_INFO("Received Loop %d->%d from LCM!!!", msg->drone_id_a, msg->drone_id_b); - loopconn_callback(*msg); + const std::string& chan, + const LoopEdge_t* msg) { + if (sent_message.find(msg->id) != sent_message.end()) { + // ROS_INFO("Receive self sent Loop message"); + return; + } + ROS_INFO("Received Loop %d->%d from LCM!!!", msg->drone_id_a, + msg->drone_id_b); + loopconn_callback(*msg); } - void LoopNet::onImgDescHeaderRecevied(const lcm::ReceiveBuffer* rbuf, - const std::string& chan, - const ImageDescriptorHeader_t* msg) { - std::lock_guard Guard(recv_lock); + const std::string& chan, + const ImageDescriptorHeader_t* msg) { + std::lock_guard Guard(recv_lock); + + if (msgBlocked(msg->msg_id)) { + return; + } + if (msg->matched_drone >= 0 && params->print_network_status) { + printf( + "[LoopNet@%d] Received ImageHeader %ld from %d matched to frame %ld " + "msg_id\n", + params->self_id, msg->frame_id, msg->drone_id, msg->matched_frame, + msg->msg_id); + } + + if (params->print_network_status) { + double delay = (ros::Time::now() - toROSTime(msg->timestamp_sent)).toSec(); + printf( + "[LoopNet]RecvImageHeader %ldc%ld lazy %d from D%d delay %.1fms msg_id " + "%ld: feature num %d gdesc %d:%d\n", + msg->frame_id, msg->camera_index, msg->is_lazy_frame, msg->drone_id, + delay * 1000.0, msg->msg_id, msg->feature_num, + msg->image_desc_size_int8, msg->image_desc_size); + } + updateRecvImgDescTs(msg->msg_id, true); + + if (received_images.find(msg->msg_id) == received_images.end()) { + ImageDescriptor_t tmp; + tmp.landmark_descriptor_size_int8 = 0; + tmp.landmark_descriptor_size = 0; + tmp.landmark_scores_size = 0; + active_receving_image_msg_idx.insert(msg->msg_id); + received_images[msg->msg_id] = tmp; + } + received_sld_win_status[msg->msg_id] = msg->sld_win_status; + received_images[msg->msg_id].header = *msg; + received_images[msg->msg_id].landmark_num = msg->feature_num; +} - if(msgBlocked(msg->msg_id)) { - return; +void LoopNet::scanRecvPackets() { + std::lock_guard Guard(recv_lock); + double tnow = ros::Time::now().toSec(); + std::vector finish_recv_image_id; + static double sum_feature_num = 0; + static double sum_feature_num_all = 0; + static int sum_packets = 0; + // Processing per view + for (auto msg_id : active_receving_image_msg_idx) { + auto& _frame = received_images[msg_id]; + if (tnow - msg_header_recv_time[msg_id] > recv_period || + _frame.landmark_num == _frame.landmarks.size() || + _frame.header.is_lazy_frame) { + sum_feature_num_all += _frame.landmark_num; + sum_feature_num += _frame.landmarks.size(); + float cur_recv_rate = + ((float)_frame.landmarks.size()) / ((float)_frame.landmark_num); + if (params->print_network_status) { + printf( + "[LoopNet](%d) frame %ldc%d from D%d msg_id %ld , LM %d/%d recv " + "duration: %.3fs recv_rate avg %.1f cur %.1f", + sum_packets, _frame.header.frame_id, _frame.header.camera_index, + _frame.header.drone_id, msg_id, _frame.landmarks.size(), + _frame.landmark_num, tnow - msg_header_recv_time[msg_id], + sum_feature_num / sum_feature_num_all * 100, cur_recv_rate * 100); + printf(" gdesc_size %d/%d lm_desc_size %d/%d\n", + _frame.header.image_desc_size_int8, + _frame.header.image_desc_size, + _frame.landmark_descriptor_size_int8, + _frame.landmark_descriptor_size); + } + _frame.landmark_num = _frame.landmarks.size(); + finish_recv_image_id.push_back(msg_id); + images_finish_recv.insert(msg_id); + + sum_packets += 1; + msg_recv_rate_callback(_frame.header.drone_id, cur_recv_rate); } - if (msg->matched_drone >= 0 && params->print_network_status) { - printf("[LoopNet@%d] Received ImageHeader %ld from %d matched to frame %ld msg_id\n", params->self_id, msg->frame_id, msg->drone_id, msg->matched_frame, msg->msg_id); + } + + for (auto msg_id : finish_recv_image_id) { + auto& msg = received_images[msg_id]; + // Processed recevied message + msg.landmark_num = msg.landmarks.size(); + if (msg.landmarks.size() > 0 || msg.header.is_lazy_frame) { + this->processRecvImageDesc(msg, received_sld_win_status[msg_id]); } - - if (params->print_network_status) { - double delay = (ros::Time::now() - toROSTime(msg->timestamp_sent)).toSec(); - printf("[LoopNet]RecvImageHeader %ldc%ld lazy %d from D%d delay %.1fms msg_id %ld: feature num %d gdesc %d:%d\n", - msg->frame_id, msg->camera_index, msg->is_lazy_frame, msg->drone_id, - delay*1000.0, msg->msg_id, msg->feature_num, msg->image_desc_size_int8, msg->image_desc_size); + received_images.erase(msg_id); + received_sld_win_status.erase(msg_id); + blacklist.insert(msg_id); + active_receving_image_msg_idx.erase(msg_id); + } + + // Scan finish image array + std::vector finish_recv_image_array_idx; + for (auto image_array_idx : active_receving_image_array_idx) { + int count_images = 0; + auto& frame_desc = received_image_arrays[image_array_idx]; + for (size_t i = 0; i < frame_desc.images.size(); i++) { + if ((frame_desc.images[i].landmark_num > 0 || frame_desc.is_lazy_frame) && + images_finish_recv.find(frame_desc.images[i].header.msg_id) != + images_finish_recv.end()) { + count_images++; + } } - updateRecvImgDescTs(msg->msg_id, true); - - if (received_images.find(msg->msg_id) == received_images.end()) { - ImageDescriptor_t tmp; - tmp.landmark_descriptor_size_int8 = 0; - tmp.landmark_descriptor_size = 0; - tmp.landmark_scores_size = 0; - active_receving_image_msg_idx.insert(msg->msg_id); - received_images[msg->msg_id] = tmp; + if (frame_header_recv_time.find(image_array_idx) != + frame_header_recv_time.end() && + (tnow - frame_header_recv_time[image_array_idx] > recv_period || + count_images >= params->min_receive_images || + (count_images == 1 && frame_desc.is_lazy_frame && + params->camera_configuration == CameraConfig::STEREO_PINHOLE))) { + // When stereo and lazy frame, only one image is enough + finish_recv_image_array_idx.push_back(image_array_idx); } - received_sld_win_status[msg->msg_id] = msg->sld_win_status; - received_images[msg->msg_id].header = *msg; - received_images[msg->msg_id].landmark_num = msg->feature_num; -} - -void LoopNet::scanRecvPackets() { - std::lock_guard Guard(recv_lock); - double tnow = ros::Time::now().toSec(); - std::vector finish_recv_image_id; - static double sum_feature_num = 0; - static double sum_feature_num_all = 0; - static int sum_packets = 0; - //Processing per view - for (auto msg_id : active_receving_image_msg_idx) { - auto & _frame = received_images[msg_id]; - if (tnow - msg_header_recv_time[msg_id] > recv_period || - _frame.landmark_num == _frame.landmarks.size() || _frame.header.is_lazy_frame) { - sum_feature_num_all+=_frame.landmark_num; - sum_feature_num+=_frame.landmarks.size(); - float cur_recv_rate = ((float)_frame.landmarks.size())/((float) _frame.landmark_num); - if (params->print_network_status) { - printf("[LoopNet](%d) frame %ldc%d from D%d msg_id %ld , LM %d/%d recv duration: %.3fs recv_rate avg %.1f cur %.1f", - sum_packets, _frame.header.frame_id, _frame.header.camera_index, _frame.header.drone_id, msg_id, _frame.landmarks.size(), _frame.landmark_num, - tnow - msg_header_recv_time[msg_id], sum_feature_num/sum_feature_num_all*100, cur_recv_rate*100); - printf(" gdesc_size %d/%d lm_desc_size %d/%d\n", _frame.header.image_desc_size_int8, _frame.header.image_desc_size, - _frame.landmark_descriptor_size_int8, _frame.landmark_descriptor_size); - } - _frame.landmark_num = _frame.landmarks.size(); - finish_recv_image_id.push_back(msg_id); - images_finish_recv.insert(msg_id); - - sum_packets += 1; - msg_recv_rate_callback(_frame.header.drone_id, cur_recv_rate); - } - } - - for (auto msg_id : finish_recv_image_id) { - auto & msg = received_images[msg_id]; - //Processed recevied message - msg.landmark_num = msg.landmarks.size(); - if (msg.landmarks.size() > 0 || msg.header.is_lazy_frame) { - this->processRecvImageDesc(msg, received_sld_win_status[msg_id]); - } - received_images.erase(msg_id); - received_sld_win_status.erase(msg_id); - blacklist.insert(msg_id); - active_receving_image_msg_idx.erase(msg_id); + } + + for (auto& image_array_idx : finish_recv_image_array_idx) { + auto& frame_desc = received_image_arrays[image_array_idx]; + active_receving_image_array_idx.erase(image_array_idx); + + frame_desc.landmark_num = 0; + for (size_t i = 0; i < frame_desc.images.size(); i++) { + frame_desc.landmark_num += frame_desc.images[i].landmark_num; + if (params->print_network_status) { + printf( + "[LoopNet::finishRecvArray] frame %ldc%d from D%d, LM %ld/%d gdesc " + "%d %d\n", + frame_desc.images[i].header.frame_id, + frame_desc.images[i].header.camera_index, + frame_desc.images[i].header.drone_id, + frame_desc.images[i].landmarks.size(), + frame_desc.images[i].landmark_num, + frame_desc.images[i].header.image_desc_size_int8, + frame_desc.images[i].header.image_desc_size); + } + images_finish_recv.erase(frame_desc.images[i].header.msg_id); + if (frame_desc.images[i].header.frame_id < 0) { + // Has empty header frame + SPDLOG_INFO("Has empty header frame, returing.."); + continue; + } } - //Scan finish image array - std::vector finish_recv_image_array_idx; - for (auto image_array_idx : active_receving_image_array_idx) { - int count_images = 0; - auto & frame_desc = received_image_arrays[image_array_idx]; - for (size_t i = 0; i < frame_desc.images.size(); i++) { - if ((frame_desc.images[i].landmark_num > 0 || frame_desc.is_lazy_frame) && - images_finish_recv.find(frame_desc.images[i].header.msg_id) != images_finish_recv.end()) { - count_images ++; - } - } - if(frame_header_recv_time.find(image_array_idx) != frame_header_recv_time.end() && - (tnow - frame_header_recv_time[image_array_idx] > recv_period || count_images >= params->min_receive_images || - (count_images == 1 && frame_desc.is_lazy_frame && params->camera_configuration == CameraConfig::STEREO_PINHOLE))) { - //When stereo and lazy frame, only one image is enough - finish_recv_image_array_idx.push_back(image_array_idx); - } + if (params->print_network_status) { + printf( + "[LoopNet@%d] Recv frame %ld: %d images from drone %d, landmark %d\n", + params->self_id, frame_desc.frame_id, frame_desc.images.size(), + frame_desc.drone_id, frame_desc.landmark_num); } - for (auto & image_array_idx: finish_recv_image_array_idx) { - auto & frame_desc = received_image_arrays[image_array_idx]; - active_receving_image_array_idx.erase(image_array_idx); - - frame_desc.landmark_num = 0; - for (size_t i = 0; i < frame_desc.images.size(); i ++) { - frame_desc.landmark_num += frame_desc.images[i].landmark_num; - if (params->print_network_status) { - printf("[LoopNet::finishRecvArray] frame %ldc%d from D%d, LM %ld/%d gdesc %d %d\n", - frame_desc.images[i].header.frame_id, frame_desc.images[i].header.camera_index, - frame_desc.images[i].header.drone_id, frame_desc.images[i].landmarks.size(), frame_desc.images[i].landmark_num, - frame_desc.images[i].header.image_desc_size_int8, frame_desc.images[i].header.image_desc_size); - } - images_finish_recv.erase(frame_desc.images[i].header.msg_id); - if (frame_desc.images[i].header.frame_id < 0) { - //Has empty header frame - return; - } - } - - if (params->print_network_status) { - printf("[LoopNet@%d] Recv frame %ld: %d images from drone %d, landmark %d\n", params->self_id, - frame_desc.frame_id, frame_desc.images.size(), frame_desc.drone_id, frame_desc.landmark_num); - } - - frame_desc_callback(frame_desc); - received_image_arrays.erase(image_array_idx); - } + frame_desc_callback(frame_desc); + received_image_arrays.erase(image_array_idx); + } } void LoopNet::updateRecvImgDescTs(int64_t id, bool is_header) { - if(is_header) { - msg_header_recv_time[id] = ros::Time::now().toSec(); - } - msg_recv_last_time[id] = ros::Time::now().toSec(); + if (is_header) { + msg_header_recv_time[id] = ros::Time::now().toSec(); + } + msg_recv_last_time[id] = ros::Time::now().toSec(); } -} \ No newline at end of file +} // namespace D2FrontEnd \ No newline at end of file diff --git a/d2frontend/src/loop_utils.cpp b/d2frontend/src/loop_utils.cpp index 750b5e42..b3f57e7f 100644 --- a/d2frontend/src/loop_utils.cpp +++ b/d2frontend/src/loop_utils.cpp @@ -1,22 +1,24 @@ -#include -#include -#include -#include #include -#include #include -#include -#include -#include -#include -#include #include -#include +#include +#include + +#include +#include +#include +#include #include +#include #include -#include +#include +#include +#include +#include +#include +#include -using namespace std::chrono; +using namespace std::chrono; using namespace D2Common; using D2Common::Utility::TicToc; @@ -25,691 +27,756 @@ using D2Common::Utility::TicToc; namespace D2FrontEnd { -std::vector detectFastByRegion(cv::InputArray _img, cv::InputArray _mask, int features, int cols, int rows); +std::vector detectFastByRegion(cv::InputArray _img, + cv::InputArray _mask, int features, + int cols, int rows); -cv::Mat getImageFromMsg(const sensor_msgs::CompressedImageConstPtr &img_msg, int flag) { - return cv::imdecode(img_msg->data, flag); +cv::Mat getImageFromMsg(const sensor_msgs::CompressedImageConstPtr &img_msg, + int flag) { + return cv::imdecode(img_msg->data, flag); } -cv_bridge::CvImagePtr getImageFromMsg(const sensor_msgs::Image &img_msg) -{ - cv_bridge::CvImagePtr ptr; - // std::cout << img_msg->encoding << std::endl; - if (img_msg.encoding == "8UC1" || img_msg.encoding == "mono8") - { - ptr = cv_bridge::toCvCopy(img_msg, "8UC1"); - } else if (img_msg.encoding == "16UC1" || img_msg.encoding == "mono16") { - ptr = cv_bridge::toCvCopy(img_msg, "16UC1"); - ptr->image.convertTo(ptr->image, CV_8UC1, 1.0/256.0); - } else { - ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::BGR8); - } - return ptr; +cv_bridge::CvImagePtr getImageFromMsg(const sensor_msgs::Image &img_msg) { + cv_bridge::CvImagePtr ptr; + // std::cout << img_msg->encoding << std::endl; + if (img_msg.encoding == "8UC1" || img_msg.encoding == "mono8") { + ptr = cv_bridge::toCvCopy(img_msg, "8UC1"); + } else if (img_msg.encoding == "16UC1" || img_msg.encoding == "mono16") { + ptr = cv_bridge::toCvCopy(img_msg, "16UC1"); + ptr->image.convertTo(ptr->image, CV_8UC1, 1.0 / 256.0); + } else { + ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::BGR8); + } + return ptr; } -cv_bridge::CvImagePtr getImageFromMsg(const sensor_msgs::ImageConstPtr &img_msg) -{ - cv_bridge::CvImagePtr ptr; - // std::cout << img_msg->encoding << std::endl; - if (img_msg->encoding == "8UC1" || img_msg->encoding == "mono8") - { - ptr = cv_bridge::toCvCopy(img_msg, "8UC1"); - } else if (img_msg->encoding == "16UC1" || img_msg->encoding == "mono16") { - ptr = cv_bridge::toCvCopy(img_msg, "16UC1"); - ptr->image.convertTo(ptr->image, CV_8UC1, 1.0/256.0); - } else { - ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::BGR8); - } - return ptr; +cv_bridge::CvImagePtr getImageFromMsg( + const sensor_msgs::ImageConstPtr &img_msg) { + cv_bridge::CvImagePtr ptr; + // std::cout << img_msg->encoding << std::endl; + if (img_msg->encoding == "8UC1" || img_msg->encoding == "mono8") { + ptr = cv_bridge::toCvCopy(img_msg, "8UC1"); + } else if (img_msg->encoding == "16UC1" || img_msg->encoding == "mono16") { + ptr = cv_bridge::toCvCopy(img_msg, "16UC1"); + ptr->image.convertTo(ptr->image, CV_8UC1, 1.0 / 256.0); + } else { + ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::BGR8); + } + return ptr; } Swarm::Pose AffineRestoCamPose(Eigen::Matrix4d affine) { - Eigen::Matrix3d R; - Eigen::Vector3d T; - - R = affine.block<3, 3>(0, 0); - T = affine.block<3, 1>(0, 3); - - R = (R.normalized()).transpose(); - T = R *(-T); - - std::cout << "R of affine\n" << R << std::endl; - std::cout << "T of affine\n" << T << std::endl; - std::cout << "RtR\n" << R.transpose()*R << std::endl; - return Swarm::Pose(R, T); + Eigen::Matrix3d R; + Eigen::Vector3d T; + + R = affine.block<3, 3>(0, 0); + T = affine.block<3, 1>(0, 3); + + R = (R.normalized()).transpose(); + T = R * (-T); + + std::cout << "R of affine\n" << R << std::endl; + std::cout << "T of affine\n" << T << std::endl; + std::cout << "RtR\n" << R.transpose() * R << std::endl; + return Swarm::Pose(R, T); } -void PnPInitialFromCamPose(const Swarm::Pose &p, cv::Mat & rvec, cv::Mat & tvec) { - Eigen::Matrix3d R_w_c = p.R(); - Eigen::Matrix3d R_inital = R_w_c.inverse(); - Eigen::Vector3d T_w_c = p.pos(); - cv::Mat tmp_r; - Eigen::Vector3d P_inital = -(R_inital * T_w_c); +void PnPInitialFromCamPose(const Swarm::Pose &p, cv::Mat &rvec, cv::Mat &tvec) { + Eigen::Matrix3d R_w_c = p.R(); + Eigen::Matrix3d R_inital = R_w_c.inverse(); + Eigen::Vector3d T_w_c = p.pos(); + cv::Mat tmp_r; + Eigen::Vector3d P_inital = -(R_inital * T_w_c); - cv::eigen2cv(R_inital, tmp_r); - cv::Rodrigues(tmp_r, rvec); - cv::eigen2cv(P_inital, tvec); + cv::eigen2cv(R_inital, tmp_r); + cv::Rodrigues(tmp_r, rvec); + cv::eigen2cv(P_inital, tvec); } Swarm::Pose PnPRestoCamPose(cv::Mat rvec, cv::Mat tvec) { - cv::Mat r; - cv::Rodrigues(rvec, r); - Eigen::Matrix3d R_pnp, R_w_c_old; - cv::cv2eigen(r, R_pnp); - R_w_c_old = R_pnp.transpose(); - Eigen::Vector3d T_pnp, T_w_c_old; - cv::cv2eigen(tvec, T_pnp); - T_w_c_old = R_w_c_old * (-T_pnp); - - return Swarm::Pose(R_w_c_old, T_w_c_old); + cv::Mat r; + cv::Rodrigues(rvec, r); + Eigen::Matrix3d R_pnp, R_w_c_old; + cv::cv2eigen(r, R_pnp); + R_w_c_old = R_pnp.transpose(); + Eigen::Vector3d T_pnp, T_w_c_old; + cv::cv2eigen(tvec, T_pnp); + T_w_c_old = R_w_c_old * (-T_pnp); + + return Swarm::Pose(R_w_c_old, T_w_c_old); } cv::Vec3b extractColor(const cv::Mat &img, cv::Point2f p) { - cv::Vec3b color; - if (img.channels() == 3) { - color = img.at(p); - } else { - auto grayscale = img.at(p); - color = cv::Vec3b(grayscale, grayscale, grayscale); - } - return color; + cv::Vec3b color; + if (img.channels() == 3) { + color = img.at(p); + } else { + auto grayscale = img.at(p); + color = cv::Vec3b(grayscale, grayscale, grayscale); + } + return color; } - Eigen::MatrixXf load_csv_mat_eigen(std::string csv) { - int cols = 0, rows = 0; - std::vector buff; + int cols = 0, rows = 0; + std::vector buff; - // Read numbers from file into buffer. - std::ifstream infile; - infile.open(csv); - std::string line; + // Read numbers from file into buffer. + std::ifstream infile; + infile.open(csv); + std::string line; - while (getline(infile, line)) - { - int temp_cols = 0; - std::stringstream lineStream(line); - std::string cell; + while (getline(infile, line)) { + int temp_cols = 0; + std::stringstream lineStream(line); + std::string cell; - while (std::getline(lineStream, cell, ',')) - { - buff.emplace_back(std::stod(cell)); - temp_cols ++; - } + while (std::getline(lineStream, cell, ',')) { + buff.emplace_back(std::stod(cell)); + temp_cols++; + } - rows ++; - if (cols > 0) { - assert(cols == temp_cols && "Matrix must have same cols on each rows!"); - } else { - cols = temp_cols; - } + rows++; + if (cols > 0) { + assert(cols == temp_cols && "Matrix must have same cols on each rows!"); + } else { + cols = temp_cols; } + } - infile.close(); + infile.close(); - Eigen::MatrixXf result(rows,cols); - for (int i = 0; i < rows; i++) - for (int j = 0; j < cols; j++) - result(i,j) = buff[ cols*i+j ]; + Eigen::MatrixXf result(rows, cols); + for (int i = 0; i < rows; i++) + for (int j = 0; j < cols; j++) result(i, j) = buff[cols * i + j]; - return result; + return result; } Eigen::VectorXf load_csv_vec_eigen(std::string csv) { - int cols = 0, rows = 0; - double buff[100000]; + int cols = 0, rows = 0; + double buff[100000]; - // Read numbers from file into buffer. - std::ifstream infile; - infile.open(csv); - while (! infile.eof()) - { - std::string line; - getline(infile, line); + // Read numbers from file into buffer. + std::ifstream infile; + infile.open(csv); + while (!infile.eof()) { + std::string line; + getline(infile, line); - int temp_cols = 0; - std::stringstream stream(line); - while(! stream.eof()) - stream >> buff[cols*rows+temp_cols++]; + int temp_cols = 0; + std::stringstream stream(line); + while (!stream.eof()) stream >> buff[cols * rows + temp_cols++]; - if (temp_cols == 0) - continue; + if (temp_cols == 0) continue; - if (cols == 0) - cols = temp_cols; + if (cols == 0) cols = temp_cols; - rows++; - } + rows++; + } - infile.close(); + infile.close(); - rows--; + rows--; - // Populate matrix with numbers. - Eigen::VectorXf result(rows,cols); - for (int i = 0; i < rows; i++) - result(i) = buff[ i ]; + // Populate matrix with numbers. + Eigen::VectorXf result(rows, cols); + for (int i = 0; i < rows; i++) result(i) = buff[i]; - return result; + return result; } cv::Point2f rotate_pt_norm2d(cv::Point2f pt, Eigen::Quaterniond q) { - Eigen::Vector3d pt3d(pt.x, pt.y, 1); - pt3d = q * pt3d; + Eigen::Vector3d pt3d(pt.x, pt.y, 1); + pt3d = q * pt3d; - if (pt3d.z() < 1e-3 && pt3d.z() > 0) { - pt3d.z() = 1e-3; - } + if (pt3d.z() < 1e-3 && pt3d.z() > 0) { + pt3d.z() = 1e-3; + } - if (pt3d.z() > -1e-3 && pt3d.z() < 0) { - pt3d.z() = -1e-3; - } + if (pt3d.z() > -1e-3 && pt3d.z() < 0) { + pt3d.z() = -1e-3; + } - return cv::Point2f(pt3d.x()/ pt3d.z(), pt3d.y()/pt3d.z()); + return cv::Point2f(pt3d.x() / pt3d.z(), pt3d.y() / pt3d.z()); } - -bool inBorder(const cv::Point2f &pt, cv::Size shape) -{ - const int BORDER_SIZE = 1; - int img_x = cvRound(pt.x); - int img_y = cvRound(pt.y); - return BORDER_SIZE <= img_x && img_x < shape.width - BORDER_SIZE && BORDER_SIZE <= img_y && img_y < shape.height - BORDER_SIZE; +bool inBorder(const cv::Point2f &pt, cv::Size shape) { + const int BORDER_SIZE = 1; + int img_x = cvRound(pt.x); + int img_y = cvRound(pt.y); + return BORDER_SIZE <= img_x && img_x < shape.width - BORDER_SIZE && + BORDER_SIZE <= img_y && img_y < shape.height - BORDER_SIZE; } -std::vector matchKNN(const cv::Mat & desc_a, const cv::Mat & desc_b, double knn_match_ratio, - const std::vector pts_a, - const std::vector pts_b, - double search_local_dist) { - //Match descriptors with OpenCV knnMatch - std::vector> matches; - cv::BFMatcher bfmatcher(cv::NORM_L2); - bfmatcher.knnMatch(desc_a, desc_b, matches, 2); - std::vector good_matches; - for (auto & match : matches) { - if (match.size() < 2) { - continue; - } - if (match[0].distance < knn_match_ratio * match[1].distance) { - if (search_local_dist > 0) { - if (cv::norm(pts_a[match[0].queryIdx] - pts_b[match[0].trainIdx]) > search_local_dist) { - continue; - } - } - good_matches.push_back(match[0]); +std::vector matchKNN(const cv::Mat &desc_a, const cv::Mat &desc_b, + double knn_match_ratio, + const std::vector pts_a, + const std::vector pts_b, + double search_local_dist) { + // Match descriptors with OpenCV knnMatch + std::vector> matches; + std::vector> matches_inv; + cv::BFMatcher bfmatcher(cv::NORM_L2); + bfmatcher.knnMatch(desc_a, desc_b, matches, 2); + bfmatcher.knnMatch(desc_b, desc_a, matches_inv, 2); + // Build up dict for matches_inv + std::vector match_inv_dict(desc_b.rows, -1); + for (auto &match : matches_inv) { + if (match.size() < 2) { + continue; + } + if (match[0].distance < knn_match_ratio * match[1].distance) { + match_inv_dict[match[0].queryIdx] = match[0].trainIdx; + } + } + std::vector good_matches; + for (auto &match : matches) { + if (match.size() < 2) { + continue; + } + if (match[0].distance < knn_match_ratio * match[1].distance && + match_inv_dict[match[0].trainIdx] == match[0].queryIdx) { + if (search_local_dist > 0) { + if (cv::norm(pts_a[match[0].queryIdx] - pts_b[match[0].trainIdx]) > + search_local_dist) { + continue; } + } + good_matches.push_back(match[0]); } - return good_matches; + } + return good_matches; } - -std::vector opticalflowTrack(const cv::Mat & cur_img, const cv::Mat & prev_img, std::vector & prev_pts, - std::vector & ids, TrackLRType type, bool enable_cuda) { - if (prev_pts.size() == 0) { - return std::vector(); - } - TicToc tic; - std::vector status; - std::vector cur_pts; - float move_cols = cur_img.cols*90.0/params->undistort_fov; //slightly lower than 0.5 cols when fov=200 - - if (prev_pts.size() == 0) { - return std::vector(); - } - - if (type == WHOLE_IMG_MATCH) { - cur_pts = prev_pts; - } else { - status.resize(prev_pts.size()); - std::fill(status.begin(), status.end(), 0); - if (type == LEFT_RIGHT_IMG_MATCH) { - for (unsigned int i = 0; i < prev_pts.size(); i++) { - auto pt = prev_pts[i]; - if (pt.x < cur_img.cols - move_cols) { - pt.x += move_cols; - status[i] = 1; - cur_pts.push_back(pt); - } - } - } else { - for (unsigned int i = 0; i < prev_pts.size(); i++) { - auto pt = prev_pts[i]; - if (pt.x >= move_cols) { - pt.x -= move_cols; - status[i] = 1; - cur_pts.push_back(pt); - } - } - } - reduceVector(prev_pts, status); - reduceVector(ids, status); - } - status.resize(0); - if (cur_pts.size() == 0) { - return std::vector(); - } - std::vector err; - std::vector reverse_status; - std::vector reverse_pts; - if (enable_cuda) { - cv::cuda::GpuMat gpu_prev_img(prev_img); - cv::cuda::GpuMat gpu_cur_img(cur_img); - cv::cuda::GpuMat gpu_prev_pts(prev_pts); - cv::cuda::GpuMat gpu_cur_pts(cur_pts); - cv::cuda::GpuMat gpu_status; - cv::cuda::GpuMat reverse_gpu_status; - cv::Ptr d_pyrLK_sparse = cv::cuda::SparsePyrLKOpticalFlow::create(WIN_SIZE, PYR_LEVEL, 30, true); - d_pyrLK_sparse->calc(gpu_prev_img, gpu_cur_img, gpu_prev_pts, gpu_cur_pts, gpu_status); - gpu_status.download(status); - gpu_cur_pts.download(cur_pts); - reverse_pts = cur_pts; - for (unsigned int i = 0; i < prev_pts.size(); i++) { - auto & pt = reverse_pts[i]; - if (type == LEFT_RIGHT_IMG_MATCH && status[i] == 1) { - pt.x -= move_cols; - } - if (type == RIGHT_LEFT_IMG_MATCH && status[i] == 1) { - pt.x += move_cols; - } +std::vector opticalflowTrack(const cv::Mat &cur_img, + const cv::Mat &prev_img, + std::vector &prev_pts, + std::vector &ids, + TrackLRType type, bool enable_cuda) { + if (prev_pts.size() == 0) { + return std::vector(); + } + TicToc tic; + std::vector status; + std::vector cur_pts; + float move_cols = + cur_img.cols * 90.0 / + params->undistort_fov; // slightly lower than 0.5 cols when fov=200 + + if (prev_pts.size() == 0) { + return std::vector(); + } + + if (type == WHOLE_IMG_MATCH) { + cur_pts = prev_pts; + } else { + status.resize(prev_pts.size()); + std::fill(status.begin(), status.end(), 0); + if (type == LEFT_RIGHT_IMG_MATCH) { + for (unsigned int i = 0; i < prev_pts.size(); i++) { + auto pt = prev_pts[i]; + if (pt.x < cur_img.cols - move_cols) { + pt.x += move_cols; + status[i] = 1; + cur_pts.push_back(pt); } - cv::cuda::GpuMat reverse_gpu_pts(reverse_pts); - d_pyrLK_sparse->calc(gpu_cur_img, gpu_prev_img, gpu_cur_pts, reverse_gpu_pts, reverse_gpu_status); - reverse_gpu_pts.download(reverse_pts); - reverse_gpu_status.download(reverse_status); + } } else { - cv::calcOpticalFlowPyrLK(prev_img, cur_img, prev_pts, cur_pts, status, err, WIN_SIZE, PYR_LEVEL, - cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 30, 0.01), cv::OPTFLOW_USE_INITIAL_FLOW); - reverse_pts = cur_pts; - for (unsigned int i = 0; i < prev_pts.size(); i++) { - auto & pt = reverse_pts[i]; - if (type == LEFT_RIGHT_IMG_MATCH && status[i] == 1) { - pt.x -= move_cols; - } - if (type == RIGHT_LEFT_IMG_MATCH && status[i] == 1) { - pt.x += move_cols; - } + for (unsigned int i = 0; i < prev_pts.size(); i++) { + auto pt = prev_pts[i]; + if (pt.x >= move_cols) { + pt.x -= move_cols; + status[i] = 1; + cur_pts.push_back(pt); } - cv::calcOpticalFlowPyrLK(cur_img, prev_img, cur_pts, reverse_pts, reverse_status, err, WIN_SIZE, PYR_LEVEL, - cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 30, 0.01), cv::OPTFLOW_USE_INITIAL_FLOW); + } } - - - for(size_t i = 0; i < status.size(); i++) - { - if(status[i] && reverse_status[i] && cv::norm(prev_pts[i] - reverse_pts[i]) <= 0.5) - { - status[i] = 1; - } - else - status[i] = 0; - } - - for (int i = 0; i < int(cur_pts.size()); i++){ - if (status[i] && !inBorder(cur_pts[i], cur_img.size())) { - status[i] = 0; - } - } reduceVector(prev_pts, status); - reduceVector(cur_pts, status); reduceVector(ids, status); - return cur_pts; -} - -std::vector opticalflowTrackPyr(const cv::Mat & cur_img, std::vector & prev_pyr, - std::vector & prev_pts, std::vector & ids, TrackLRType type, bool update_pyr) { - if (prev_pts.size() == 0) { - return std::vector(); - } - TicToc tic; - std::vector status; - std::vector cur_pts; - float move_cols = cur_img.cols*90.0/params->undistort_fov; //slightly lower than 0.5 cols when fov=200 - - if (prev_pts.size() == 0) { - return std::vector(); - } - - if (type == WHOLE_IMG_MATCH) { - cur_pts = prev_pts; - } else { - status.resize(prev_pts.size()); - std::fill(status.begin(), status.end(), 0); - if (type == LEFT_RIGHT_IMG_MATCH) { - for (unsigned int i = 0; i < prev_pts.size(); i++) { - auto pt = prev_pts[i]; - if (pt.x < cur_img.cols - move_cols) { - pt.x += move_cols; - status[i] = 1; - cur_pts.push_back(pt); - } - } - } else { - for (unsigned int i = 0; i < prev_pts.size(); i++) { - auto pt = prev_pts[i]; - if (pt.x >= move_cols) { - pt.x -= move_cols; - status[i] = 1; - cur_pts.push_back(pt); - } - } - } - reduceVector(prev_pts, status); - reduceVector(ids, status); - } - status.resize(0); - if (cur_pts.size() == 0) { - return std::vector(); - } - std::vector err; - std::vector reverse_status; - std::vector reverse_pts; - + } + status.resize(0); + if (cur_pts.size() == 0) { + return std::vector(); + } + std::vector err; + std::vector reverse_status; + std::vector reverse_pts; + if (enable_cuda) { + cv::cuda::GpuMat gpu_prev_img(prev_img); cv::cuda::GpuMat gpu_cur_img(cur_img); cv::cuda::GpuMat gpu_prev_pts(prev_pts); cv::cuda::GpuMat gpu_cur_pts(cur_pts); cv::cuda::GpuMat gpu_status; cv::cuda::GpuMat reverse_gpu_status; - auto cur_pyr = buildImagePyramid(gpu_cur_img); - cv::Ptr d_pyrLK_sparse = cv::cuda::SparsePyrLKOpticalFlow::create(WIN_SIZE, PYR_LEVEL, 30, true); - d_pyrLK_sparse->calc(prev_pyr, cur_pyr, gpu_prev_pts, gpu_cur_pts, gpu_status); + cv::Ptr d_pyrLK_sparse = + cv::cuda::SparsePyrLKOpticalFlow::create(WIN_SIZE, PYR_LEVEL, 30, true); + d_pyrLK_sparse->calc(gpu_prev_img, gpu_cur_img, gpu_prev_pts, gpu_cur_pts, + gpu_status); gpu_status.download(status); gpu_cur_pts.download(cur_pts); reverse_pts = cur_pts; for (unsigned int i = 0; i < prev_pts.size(); i++) { - auto & pt = reverse_pts[i]; - if (type == LEFT_RIGHT_IMG_MATCH && status[i] == 1) { - pt.x -= move_cols; - } - if (type == RIGHT_LEFT_IMG_MATCH && status[i] == 1) { - pt.x += move_cols; - } + auto &pt = reverse_pts[i]; + if (type == LEFT_RIGHT_IMG_MATCH && status[i] == 1) { + pt.x -= move_cols; + } + if (type == RIGHT_LEFT_IMG_MATCH && status[i] == 1) { + pt.x += move_cols; + } } cv::cuda::GpuMat reverse_gpu_pts(reverse_pts); - d_pyrLK_sparse->calc(cur_pyr, prev_pyr, gpu_cur_pts, reverse_gpu_pts, reverse_gpu_status); + d_pyrLK_sparse->calc(gpu_cur_img, gpu_prev_img, gpu_cur_pts, + reverse_gpu_pts, reverse_gpu_status); reverse_gpu_pts.download(reverse_pts); reverse_gpu_status.download(reverse_status); + } else { + cv::calcOpticalFlowPyrLK( + prev_img, cur_img, prev_pts, cur_pts, status, err, WIN_SIZE, PYR_LEVEL, + cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 30, + 0.01), + cv::OPTFLOW_USE_INITIAL_FLOW); + reverse_pts = cur_pts; + for (unsigned int i = 0; i < prev_pts.size(); i++) { + auto &pt = reverse_pts[i]; + if (type == LEFT_RIGHT_IMG_MATCH && status[i] == 1) { + pt.x -= move_cols; + } + if (type == RIGHT_LEFT_IMG_MATCH && status[i] == 1) { + pt.x += move_cols; + } + } + cv::calcOpticalFlowPyrLK( + cur_img, prev_img, cur_pts, reverse_pts, reverse_status, err, WIN_SIZE, + PYR_LEVEL, + cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 30, + 0.01), + cv::OPTFLOW_USE_INITIAL_FLOW); + } + + for (size_t i = 0; i < status.size(); i++) { + if (status[i] && reverse_status[i] && + cv::norm(prev_pts[i] - reverse_pts[i]) <= 0.5) { + status[i] = 1; + } else + status[i] = 0; + } + + for (int i = 0; i < int(cur_pts.size()); i++) { + if (status[i] && !inBorder(cur_pts[i], cur_img.size())) { + status[i] = 0; + } + } + reduceVector(prev_pts, status); + reduceVector(cur_pts, status); + reduceVector(ids, status); + return cur_pts; +} - for(size_t i = 0; i < status.size(); i++) - { - if(status[i] && reverse_status[i] && cv::norm(prev_pts[i] - reverse_pts[i]) <= 0.5) - { - status[i] = 1; +LKImageInfoGPU opticalflowTrackPyr(const cv::Mat &cur_img, + const LKImageInfoGPU &prev_lk, + TrackLRType type) { + cv::cuda::GpuMat gpu_cur_img(cur_img); + auto cur_pyr = buildImagePyramid(gpu_cur_img); + auto ids = prev_lk.lk_ids; + auto prev_pts = prev_lk.lk_pts; + auto prev_types = prev_lk.lk_types; + std::vector prev_local_index = prev_lk.lk_local_index; + std::vector lk_pts_3d_norm; + if (prev_pts.size() == 0) { + LKImageInfoGPU ret; + ret.pyr = cur_pyr; + return ret; + } + TicToc tic; + std::vector status; + std::vector cur_pts; + float move_cols = + cur_img.cols * 90.0 / + params->undistort_fov; // slightly lower than 0.5 cols when fov=200 + + if (type == WHOLE_IMG_MATCH) { + cur_pts = prev_pts; + } else { + status.resize(prev_pts.size()); + std::fill(status.begin(), status.end(), 0); + if (type == LEFT_RIGHT_IMG_MATCH) { + for (unsigned int i = 0; i < prev_pts.size(); i++) { + auto pt = prev_pts[i]; + if (pt.x < cur_img.cols - move_cols) { + pt.x += move_cols; + status[i] = 1; + cur_pts.push_back(pt); } - else - status[i] = 0; - } - - for (int i = 0; i < int(cur_pts.size()); i++){ - if (status[i] && !inBorder(cur_pts[i], cur_img.size())) { - status[i] = 0; + } + } else { + for (unsigned int i = 0; i < prev_pts.size(); i++) { + auto pt = prev_pts[i]; + if (pt.x >= move_cols) { + pt.x -= move_cols; + status[i] = 1; + cur_pts.push_back(pt); } - } + } + } reduceVector(prev_pts, status); - reduceVector(cur_pts, status); + reduceVector(prev_types, status); + reduceVector(prev_local_index, status); reduceVector(ids, status); - if (update_pyr) { - prev_pyr = cur_pyr; - } - return cur_pts; -} - + } + status.resize(0); + if (cur_pts.size() == 0) { + LKImageInfoGPU ret; + ret.pyr = cur_pyr; + return ret; + } + std::vector err; + std::vector reverse_status; + std::vector reverse_pts; + + cv::cuda::GpuMat gpu_prev_pts(prev_pts); + cv::cuda::GpuMat gpu_cur_pts(cur_pts); + cv::cuda::GpuMat gpu_status; + cv::cuda::GpuMat reverse_gpu_status; + cv::Ptr d_pyrLK_sparse = + cv::cuda::SparsePyrLKOpticalFlow::create(WIN_SIZE, PYR_LEVEL, 30, true); + d_pyrLK_sparse->calc(prev_lk.pyr, cur_pyr, gpu_prev_pts, gpu_cur_pts, + gpu_status); + gpu_status.download(status); + gpu_cur_pts.download(cur_pts); + reverse_pts = cur_pts; + for (unsigned int i = 0; i < prev_pts.size(); i++) { + auto &pt = reverse_pts[i]; + if (type == LEFT_RIGHT_IMG_MATCH && status[i] == 1) { + pt.x -= move_cols; + } + if (type == RIGHT_LEFT_IMG_MATCH && status[i] == 1) { + pt.x += move_cols; + } + } + cv::cuda::GpuMat reverse_gpu_pts(reverse_pts); + d_pyrLK_sparse->calc(cur_pyr, prev_lk.pyr, gpu_cur_pts, reverse_gpu_pts, + reverse_gpu_status); + reverse_gpu_pts.download(reverse_pts); + reverse_gpu_status.download(reverse_status); + + for (size_t i = 0; i < status.size(); i++) { + if (status[i] && reverse_status[i] && + cv::norm(prev_pts[i] - reverse_pts[i]) <= 0.5) { + status[i] = 1; + } else + status[i] = 0; + } + + for (int i = 0; i < int(cur_pts.size()); i++) { + if (status[i] && !inBorder(cur_pts[i], cur_img.size())) { + status[i] = 0; + } + } + reduceVector(cur_pts, status); + reduceVector(ids, status); + reduceVector(prev_types, status); + reduceVector(prev_local_index, status); + return {cur_pts, lk_pts_3d_norm, ids, prev_local_index, prev_types, cur_pyr}; +} -void detectPoints(const cv::Mat & img, std::vector & n_pts, std::vector & cur_pts, - int require_pts, bool enable_cuda, bool use_fast, int fast_rows, int fast_cols) { - int lack_up_top_pts = require_pts - static_cast(cur_pts.size()); - cv::Mat mask; - if (params->enable_perf_output) { - ROS_INFO("Lost %d pts; Require %d will detect %d", lack_up_top_pts, require_pts, lack_up_top_pts > require_pts/4); - } - std::vector n_pts_tmp; - if (lack_up_top_pts > require_pts/4) { - int num_to_detect = lack_up_top_pts; - if (cur_pts.size() > 0) { - //We have some points, so try to detect slightly more points to avoid overlap with current points - num_to_detect = lack_up_top_pts * 2; - } - cv::Mat d_prevPts; - if (use_fast) { - n_pts_tmp = detectFastByRegion(img, mask, num_to_detect, fast_rows, fast_cols); - } else { - //Use goodFeaturesToTrack - if (enable_cuda) { - cv::Ptr detector = cv::cuda::createGoodFeaturesToTrackDetector( - img.type(), num_to_detect, 0.01, params->feature_min_dist); - cv::cuda::GpuMat d_prevPts_gpu; - cv::cuda::GpuMat img_cuda(img); - detector->detect(img_cuda, d_prevPts_gpu); - d_prevPts_gpu.download(d_prevPts); - } else { - cv::goodFeaturesToTrack(img, d_prevPts, num_to_detect, 0.01, params->feature_min_dist, mask); - } - if(!d_prevPts.empty()) { - n_pts_tmp = cv::Mat_(cv::Mat(d_prevPts)); - } - else { - n_pts_tmp.clear(); - } - } - n_pts.clear(); - std::vector all_pts = cur_pts; - for (auto & pt : n_pts_tmp) { - bool has_nearby = false; - for (auto &pt_j: all_pts) { - if (cv::norm(pt-pt_j) < params->feature_min_dist) { - has_nearby = true; - break; - } - } - if (!has_nearby) { - n_pts.push_back(pt); - all_pts.push_back(pt); - } - if (n_pts.size() >= lack_up_top_pts) { - break; - } - } +void detectPoints(const cv::Mat &img, std::vector &n_pts, + const std::vector &cur_pts, int require_pts, + bool enable_cuda, bool use_fast, int fast_rows, + int fast_cols) { + int lack_up_top_pts = require_pts - static_cast(cur_pts.size()); + cv::Mat mask; + if (params->enable_perf_output) { + ROS_INFO("Lost %d pts; Require %d will detect %d", lack_up_top_pts, + require_pts, lack_up_top_pts > require_pts / 4); + } + std::vector n_pts_tmp; + if (lack_up_top_pts > require_pts / 4) { + int num_to_detect = lack_up_top_pts; + if (cur_pts.size() > 0) { + // We have some points, so try to detect slightly more points to + // avoid overlap with current points + num_to_detect = lack_up_top_pts * 2; + } + cv::Mat d_prevPts; + if (use_fast) { + n_pts_tmp = + detectFastByRegion(img, mask, num_to_detect, fast_rows, fast_cols); } else { - n_pts.clear(); - } -} - -std::vector detectFastByRegion(cv::InputArray _img, cv::InputArray _mask, int features, int cols, int rows) { - int small_width = _img.cols() / cols; - int small_height = _img.rows() / rows; - int num_features = ceil((double)features*1.5/ ((double) cols * rows)); - auto fast = cv::cuda::FastFeatureDetector::create(10, true, cv::FastFeatureDetector::TYPE_9_16, features); - cv::cuda::GpuMat gpu_img(_img); - std::vector total_kpts; - for (int i = 0; i < cols; i ++) { - for (int j = 0; j < rows; j ++) { - std::vector kpts; - cv::Rect roi(small_width*i, small_height*j, small_width, small_height); - fast->detect(gpu_img(roi), kpts); - // printf("Detect %d features in region %d %d\n", kpts.size(), i, j); - for (auto kp : kpts) { - kp.pt.x = kp.pt.x + small_width*i; - kp.pt.y = kp.pt.y + small_height*j; - total_kpts.push_back(kp); - } - } - } - //Sort the keypoints by confidence - std::vector ret; - if (total_kpts.size() == 0) { - return ret; - } - std::sort(total_kpts.begin(), total_kpts.end(), [](const cv::KeyPoint & a, const cv::KeyPoint & b) { - return a.response > b.response; - }); - //Return the top features - for (int i = 0; i < total_kpts.size(); i ++) { - ret.push_back(total_kpts[i].pt); - if (ret.size() >= features) { - break; + // Use goodFeaturesToTrack + if (enable_cuda) { + cv::Ptr detector = + cv::cuda::createGoodFeaturesToTrackDetector( + img.type(), num_to_detect, 0.01, params->feature_min_dist); + cv::cuda::GpuMat d_prevPts_gpu; + cv::cuda::GpuMat img_cuda(img); + detector->detect(img_cuda, d_prevPts_gpu); + d_prevPts_gpu.download(d_prevPts); + } else { + cv::goodFeaturesToTrack(img, d_prevPts, num_to_detect, 0.01, + params->feature_min_dist, mask); + } + if (!d_prevPts.empty()) { + n_pts_tmp = cv::Mat_(cv::Mat(d_prevPts)); + } else { + n_pts_tmp.clear(); + } + } + n_pts.clear(); + std::vector all_pts = cur_pts; + for (auto &pt : n_pts_tmp) { + bool has_nearby = false; + for (auto &pt_j : all_pts) { + if (cv::norm(pt - pt_j) < params->feature_min_dist) { + has_nearby = true; + break; } - } - return ret; + } + if (!has_nearby) { + n_pts.push_back(pt); + all_pts.push_back(pt); + } + if (n_pts.size() >= lack_up_top_pts) { + break; + } + } + } else { + n_pts.clear(); + } } -bool pnp_result_verify(bool pnp_success, int inliers, double rperr, const Swarm::Pose & DP_old_to_new) { - bool success = pnp_success; - if (!pnp_success) { - return false; - } - if (rperr > params->loopdetectorconfig->gravity_check_thres) { - printf("[SWARM_LOOP] Check failed on RP error %.1fdeg (%.1f)deg\n", rperr*57.3, params->loopdetectorconfig->gravity_check_thres*57.3); - return false; - } - auto &_config = (*params->loopdetectorconfig); - success = (inliers >= _config.loop_inlier_feature_num) && fabs(DP_old_to_new.yaw()) < _config.accept_loop_max_yaw*DEG2RAD && DP_old_to_new.pos().norm() < _config.accept_loop_max_pos; - return success; +std::vector detectFastByRegion(cv::InputArray _img, + cv::InputArray _mask, int features, + int cols, int rows) { + int small_width = _img.cols() / cols; + int small_height = _img.rows() / rows; + int num_features = ceil((double)features * 1.5 / ((double)cols * rows)); + auto fast = cv::cuda::FastFeatureDetector::create( + 10, true, cv::FastFeatureDetector::TYPE_9_16, features); + cv::cuda::GpuMat gpu_img(_img); + std::vector total_kpts; + for (int i = 0; i < cols; i++) { + for (int j = 0; j < rows; j++) { + std::vector kpts; + cv::Rect roi(small_width * i, small_height * j, small_width, + small_height); + fast->detect(gpu_img(roi), kpts); + // printf("Detect %d features in region %d %d\n", kpts.size(), i, + // j); + for (auto kp : kpts) { + kp.pt.x = kp.pt.x + small_width * i; + kp.pt.y = kp.pt.y + small_height * j; + total_kpts.push_back(kp); + } + } + } + // Sort the keypoints by confidence + std::vector ret; + if (total_kpts.size() == 0) { + return ret; + } + std::sort(total_kpts.begin(), total_kpts.end(), + [](const cv::KeyPoint &a, const cv::KeyPoint &b) { + return a.response > b.response; + }); + // Return the top features + for (int i = 0; i < total_kpts.size(); i++) { + ret.push_back(total_kpts[i].pt); + if (ret.size() >= features) { + break; + } + } + return ret; } -double gravityCheck(const Swarm::Pose & pnp_pose, const Swarm::Pose & ego_motion) { - //This checks the gravity direction - Vector3d gravity(0, 0, 1); - Vector3d gravity_pnp = pnp_pose.R().inverse() * gravity; - Vector3d gravity_ego = ego_motion.R().inverse() * gravity; - double sin_theta = gravity_pnp.cross(gravity_ego).norm(); - return sin_theta; +bool pnp_result_verify(bool pnp_success, int inliers, double rperr, + const Swarm::Pose &DP_old_to_new) { + bool success = pnp_success; + if (!pnp_success) { + return false; + } + if (rperr > params->loopdetectorconfig->gravity_check_thres) { + printf("[SWARM_LOOP] Check failed on RP error %.1fdeg (%.1f)deg\n", + rperr * 57.3, + params->loopdetectorconfig->gravity_check_thres * 57.3); + return false; + } + auto &_config = (*params->loopdetectorconfig); + success = (inliers >= _config.loop_inlier_feature_num) && + fabs(DP_old_to_new.yaw()) < _config.accept_loop_max_yaw * DEG2RAD && + DP_old_to_new.pos().norm() < _config.accept_loop_max_pos; + return success; } -int computeRelativePosePnP(const std::vector lm_positions_a, const std::vector lm_3d_norm_b, - Swarm::Pose extrinsic_b, Swarm::Pose ego_motion_a, Swarm::Pose ego_motion_b, Swarm::Pose & DP_b_to_a, std::vector &inliers, - bool is_4dof, bool verify_gravity) { - //Compute PNP - // ROS_INFO("Matched features %ld", matched_2d_norm_old.size()); - cv::Mat K = (cv::Mat_(3, 3) << 1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0); - cv::Mat r, rvec, rvec2, t, t2, D, tmp_r; - - int iteratives = 100; - Point3fVector pts3d; - Point2fVector pts2d; - for (int i = 0; i < lm_positions_a.size(); i++) { - auto z = lm_3d_norm_b[i].z(); - if (z > 1e-1) { - pts3d.push_back(cv::Point3f(lm_positions_a[i].x(), lm_positions_a[i].y(), lm_positions_a[i].z())); - pts2d.push_back(cv::Point2f(lm_3d_norm_b[i].x()/z, lm_3d_norm_b[i].y()/z)); - } - } - if (pts3d.size() < params->loopdetectorconfig->loop_inlier_feature_num) { - return false; - } - bool success = solvePnPRansac(pts3d, pts2d, K, D, rvec, t, false, - iteratives, 5.0/params->focal_length, 0.99, inliers); - auto p_cam_old_in_new = PnPRestoCamPose(rvec, t); - auto pnp_predict_pose_b = p_cam_old_in_new*(extrinsic_b.toIsometry().inverse()); - if (!success) { - return 0; - } - DP_b_to_a = Swarm::Pose::DeltaPose(pnp_predict_pose_b, ego_motion_a, is_4dof); - if (verify_gravity) { - auto RPerr = gravityCheck(pnp_predict_pose_b, ego_motion_b); - success = pnp_result_verify(success, inliers.size(), RPerr, DP_b_to_a); - printf("[SWARM_LOOP@%d] DPose %s PnPRansac %d inlines %d/%d, dyaw %f dpos %f g_err %f \n", - params->self_id, DP_b_to_a.toStr().c_str(), success, inliers.size(), pts2d.size(), fabs(DP_b_to_a.yaw())*57.3, DP_b_to_a.pos().norm(), RPerr); - } - return success; +double gravityCheck(const Swarm::Pose &pnp_pose, + const Swarm::Pose &ego_motion) { + // This checks the gravity direction + Vector3d gravity(0, 0, 1); + Vector3d gravity_pnp = pnp_pose.R().inverse() * gravity; + Vector3d gravity_ego = ego_motion.R().inverse() * gravity; + double sin_theta = gravity_pnp.cross(gravity_ego).norm(); + return sin_theta; } -Swarm::Pose computePosePnPnonCentral(const std::vector & lm_positions_a, const std::vector & lm_3d_norm_b, - const std::vector & cam_extrinsics, const std::vector & camera_indices, std::vector &inliers) { - opengv::bearingVectors_t bearings; - std::vector camCorrespondences; - opengv::points_t points; - opengv::rotations_t camRotations; - opengv::translations_t camOffsets; - for (int i = 0; i < lm_positions_a.size(); i++) { - bearings.push_back(lm_3d_norm_b[i]); - camCorrespondences.push_back(camera_indices[i]); - points.push_back(lm_positions_a[i]); - } - for (int i = 0; i < cam_extrinsics.size(); i++) { - camRotations.push_back(cam_extrinsics[i].R()); - camOffsets.push_back(cam_extrinsics[i].pos()); - } - - //Solve with GP3P + RANSAC - opengv::absolute_pose::NoncentralAbsoluteAdapter adapter( - bearings, camCorrespondences, points, camOffsets, camRotations); - opengv::sac::Ransac< - opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem> ransac; - std::shared_ptr absposeproblem_ptr(new opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem( - adapter, opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem::GP3P)); - ransac.sac_model_ = absposeproblem_ptr; - // ransac.threshold_ = 1.0 - cos(atan(sqrt(10.0)*0.5/460.0)); - ransac.threshold_ = 0.5/params->focal_length; - ransac.max_iterations_ = 50; - ransac.computeModel(); - //Obtain relative pose results - inliers = ransac.inliers_; - auto best_transformation = ransac.model_coefficients_; - Matrix3d R = best_transformation.block<3, 3>(0, 0); - Vector3d t = best_transformation.block<3, 1>(0, 3); - Swarm::Pose p_drone_old_in_new_init(R, t); - - //Filter by inliers and perform non-linear optimization to refine. - std::set inlier_set(inliers.begin(), inliers.end()); - bearings.clear(); - camCorrespondences.clear(); - points.clear(); - for (int i = 0; i < lm_positions_a.size(); i++) { - if (inlier_set.find(i) == inlier_set.end()) { - continue; - } - bearings.push_back(lm_3d_norm_b[i]); - camCorrespondences.push_back(camera_indices[i]); - points.push_back(lm_positions_a[i]); - } - adapter.sett(t); - adapter.setR(R); - opengv::transformation_t nonlinear_transformation = - opengv::absolute_pose::optimize_nonlinear(adapter); - R = nonlinear_transformation.block<3, 3>(0, 0); - t = nonlinear_transformation.block<3, 1>(0, 3); - Swarm::Pose p_drone_old_in_new(R, t); - // printf("[InitPnP] pose_init %s pose_refine %s\n", p_drone_old_in_new_init.toStr().c_str(), - // p_drone_old_in_new.toStr().c_str()); - return p_drone_old_in_new; +int computeRelativePosePnP(const std::vector lm_positions_a, + const std::vector lm_3d_norm_b, + Swarm::Pose extrinsic_b, Swarm::Pose ego_motion_a, + Swarm::Pose ego_motion_b, Swarm::Pose &DP_b_to_a, + std::vector &inliers, bool is_4dof, + bool verify_gravity) { + // Compute PNP + // ROS_INFO("Matched features %ld", matched_2d_norm_old.size()); + cv::Mat K = (cv::Mat_(3, 3) << 1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0); + cv::Mat r, rvec, rvec2, t, t2, D, tmp_r; + + int iteratives = 100; + Point3fVector pts3d; + Point2fVector pts2d; + for (int i = 0; i < lm_positions_a.size(); i++) { + auto z = lm_3d_norm_b[i].z(); + if (z > 1e-1) { + pts3d.push_back(cv::Point3f(lm_positions_a[i].x(), lm_positions_a[i].y(), + lm_positions_a[i].z())); + pts2d.push_back( + cv::Point2f(lm_3d_norm_b[i].x() / z, lm_3d_norm_b[i].y() / z)); + } + } + if (pts3d.size() < params->loopdetectorconfig->loop_inlier_feature_num) { + return false; + } + bool success = solvePnPRansac(pts3d, pts2d, K, D, rvec, t, false, iteratives, + 5.0 / params->focal_length, 0.99, inliers); + auto p_cam_old_in_new = PnPRestoCamPose(rvec, t); + auto pnp_predict_pose_b = + p_cam_old_in_new * (extrinsic_b.toIsometry().inverse()); + if (!success) { + return 0; + } + DP_b_to_a = Swarm::Pose::DeltaPose(pnp_predict_pose_b, ego_motion_a, is_4dof); + if (verify_gravity) { + auto RPerr = gravityCheck(pnp_predict_pose_b, ego_motion_b); + success = pnp_result_verify(success, inliers.size(), RPerr, DP_b_to_a); + printf( + "[SWARM_LOOP@%d] DPose %s PnPRansac %d inlines %d/%d, dyaw %f " + "dpos %f g_err %f \n", + params->self_id, DP_b_to_a.toStr().c_str(), success, inliers.size(), + pts2d.size(), fabs(DP_b_to_a.yaw()) * 57.3, DP_b_to_a.pos().norm(), + RPerr); + } + return success; } -int computeRelativePosePnPnonCentral(const std::vector & lm_positions_a, const std::vector & lm_3d_norm_b, - const std::vector & cam_extrinsics, const std::vector & camera_indices, - Swarm::Pose drone_pose_a, Swarm::Pose ego_motion_b, - Swarm::Pose & DP_b_to_a, std::vector &inliers, bool is_4dof, bool verify_gravity) { - D2Common::Utility::TicToc tic; - auto pnp_predict_pose_b = computePosePnPnonCentral(lm_positions_a, lm_3d_norm_b, cam_extrinsics, camera_indices, inliers); - DP_b_to_a = Swarm::Pose::DeltaPose(pnp_predict_pose_b, drone_pose_a, is_4dof); - - bool success = true; - double RPerr = 0; - if (verify_gravity) { - //Verify the results - auto RPerr = gravityCheck(pnp_predict_pose_b, ego_motion_b); - success = pnp_result_verify(true, inliers.size(), RPerr, DP_b_to_a); - } - - if (params->enable_perf_output) { - printf("[SWARM_LOOP@%d] features %d/%d succ %d gPnPRansac time %.2fms RP: %s g_err %f\n", params->self_id, inliers.size(), lm_3d_norm_b.size(), success, - tic.toc(), DP_b_to_a.toStr().c_str(), RPerr); - } - return success; +Swarm::Pose computePosePnPnonCentral( + const std::vector &lm_positions_a, + const std::vector &lm_3d_norm_b, + const std::vector &cam_extrinsics, + const std::vector &camera_indices, std::vector &inliers) { + opengv::bearingVectors_t bearings; + std::vector camCorrespondences; + opengv::points_t points; + opengv::rotations_t camRotations; + opengv::translations_t camOffsets; + for (int i = 0; i < lm_positions_a.size(); i++) { + bearings.push_back(lm_3d_norm_b[i]); + camCorrespondences.push_back(camera_indices[i]); + points.push_back(lm_positions_a[i]); + } + for (int i = 0; i < cam_extrinsics.size(); i++) { + camRotations.push_back(cam_extrinsics[i].R()); + camOffsets.push_back(cam_extrinsics[i].pos()); + } + + // Solve with GP3P + RANSAC + opengv::absolute_pose::NoncentralAbsoluteAdapter adapter( + bearings, camCorrespondences, points, camOffsets, camRotations); + opengv::sac::Ransac< + opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem> + ransac; + std::shared_ptr + absposeproblem_ptr( + new opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem( + adapter, opengv::sac_problems::absolute_pose:: + AbsolutePoseSacProblem::GP3P)); + ransac.sac_model_ = absposeproblem_ptr; + // ransac.threshold_ = 1.0 - cos(atan(sqrt(10.0)*0.5/460.0)); + ransac.threshold_ = 0.5 / params->focal_length; + ransac.max_iterations_ = 50; + ransac.computeModel(); + // Obtain relative pose results + inliers = ransac.inliers_; + auto best_transformation = ransac.model_coefficients_; + Matrix3d R = best_transformation.block<3, 3>(0, 0); + Vector3d t = best_transformation.block<3, 1>(0, 3); + Swarm::Pose p_drone_old_in_new_init(R, t); + + // Filter by inliers and perform non-linear optimization to refine. + std::set inlier_set(inliers.begin(), inliers.end()); + bearings.clear(); + camCorrespondences.clear(); + points.clear(); + for (int i = 0; i < lm_positions_a.size(); i++) { + if (inlier_set.find(i) == inlier_set.end()) { + continue; + } + bearings.push_back(lm_3d_norm_b[i]); + camCorrespondences.push_back(camera_indices[i]); + points.push_back(lm_positions_a[i]); + } + adapter.sett(t); + adapter.setR(R); + opengv::transformation_t nonlinear_transformation = + opengv::absolute_pose::optimize_nonlinear(adapter); + R = nonlinear_transformation.block<3, 3>(0, 0); + t = nonlinear_transformation.block<3, 1>(0, 3); + Swarm::Pose p_drone_old_in_new(R, t); + // printf("[InitPnP] pose_init %s pose_refine %s\n", + // p_drone_old_in_new_init.toStr().c_str(), + // p_drone_old_in_new.toStr().c_str()); + return p_drone_old_in_new; } +int computeRelativePosePnPnonCentral( + const std::vector &lm_positions_a, + const std::vector &lm_3d_norm_b, + const std::vector &cam_extrinsics, + const std::vector &camera_indices, Swarm::Pose drone_pose_a, + Swarm::Pose ego_motion_b, Swarm::Pose &DP_b_to_a, std::vector &inliers, + bool is_4dof, bool verify_gravity) { + D2Common::Utility::TicToc tic; + auto pnp_predict_pose_b = computePosePnPnonCentral( + lm_positions_a, lm_3d_norm_b, cam_extrinsics, camera_indices, inliers); + DP_b_to_a = Swarm::Pose::DeltaPose(pnp_predict_pose_b, drone_pose_a, is_4dof); + + bool success = true; + double RPerr = 0; + if (verify_gravity) { + // Verify the results + auto RPerr = gravityCheck(pnp_predict_pose_b, ego_motion_b); + success = pnp_result_verify(true, inliers.size(), RPerr, DP_b_to_a); + } + + SPDLOG_INFO( + "[LoopDetector@{}] features {}/{} succ {} gPnPRansac time {:.2f}ms " + "RP: {} g_err P{}\n", + params->self_id, inliers.size(), lm_3d_norm_b.size(), success, tic.toc(), + DP_b_to_a.toStr(), RPerr); + return success; +} -std::vector buildImagePyramid(const cv::cuda::GpuMat& prevImg, int maxLevel_) { - std::vector prevPyr; - prevPyr.resize(maxLevel_ + 1); +std::vector buildImagePyramid(const cv::cuda::GpuMat &prevImg, + int maxLevel_) { + std::vector prevPyr; + prevPyr.resize(maxLevel_ + 1); - int cn = prevImg.channels(); + int cn = prevImg.channels(); - CV_Assert(cn == 1 || cn == 3 || cn == 4); + CV_Assert(cn == 1 || cn == 3 || cn == 4); - prevPyr[0] = prevImg; - for (int level = 1; level <= maxLevel_; ++level) { - cv::cuda::pyrDown(prevPyr[level - 1], prevPyr[level]); - } + prevPyr[0] = prevImg; + for (int level = 1; level <= maxLevel_; ++level) { + cv::cuda::pyrDown(prevPyr[level - 1], prevPyr[level]); + } - return prevPyr; + return prevPyr; } -} \ No newline at end of file +} // namespace D2FrontEnd \ No newline at end of file diff --git a/d2frontend/tests/camera_undistort_test.cpp b/d2frontend/tests/camera_undistort_test.cpp index 57f590c3..89c80ec6 100644 --- a/d2frontend/tests/camera_undistort_test.cpp +++ b/d2frontend/tests/camera_undistort_test.cpp @@ -1,71 +1,84 @@ -#include "d2common/fisheye_undistort.h" #include #include + #include -namespace D2FrontEnd { -std::pair readCameraConfig(const std::string & camera_name, const YAML::Node & config); -} +#include "d2common/fisheye_undistort.h" +#include "d2frontend/d2frontend_params.h" + +// namespace D2FrontEnd { +// std::pair readCameraConfig( +// const std::string& camera_name, const YAML::Node& config, int32_t extrinsic_parameter_type = 1 ); +// } using namespace D2FrontEnd; using D2Common::FisheyeUndist; int main(int argc, char** argv) { - namespace po = boost::program_options; - po::options_description desc("Allowed options"); - desc.add_options() - ("help", "produce help message") - ("image,i", po::value()->default_value(""), "path of test image to undistort") - ("calib,c", po::value()->default_value(""), "path of camera config") - ("name,n", po::value()->default_value("cam0"), "name of camera") - ("fov,f", po::value()->default_value(190.0), "fov of camera"); - po::variables_map vm; - po::store(po::parse_command_line(argc, argv, desc), vm); - po::notify(vm); - auto image_path = vm["image"].as(); - auto calib_path = vm["calib"].as(); - auto camera_name = vm["name"].as(); - auto fov = vm["fov"].as(); + namespace po = boost::program_options; + po::options_description desc("Allowed options"); + desc.add_options()("help", "produce help message")( + "image,i", po::value()->default_value(""), + "path of test image to undistort")( + "calib,c", po::value()->default_value(""), + "path of camera config")("name,n", + po::value()->default_value("cam0"), + "name of camera")( + "fov,f", po::value()->default_value(190.0), "fov of camera"); + po::variables_map vm; + po::store(po::parse_command_line(argc, argv, desc), vm); + po::notify(vm); + auto image_path = vm["image"].as(); + auto calib_path = vm["calib"].as(); + auto camera_name = vm["name"].as(); + auto fov = vm["fov"].as(); - cv::Mat img = cv::imread(image_path, cv::IMREAD_ANYCOLOR); - cv::imshow("Raw", img); - cv::waitKey(1); - printf("Read image file %s OK", image_path.c_str()); - YAML::Node config = YAML::LoadFile(calib_path); - auto ret = D2FrontEnd::readCameraConfig(camera_name, config[camera_name]); - printf("Read camera %s fov %f calib from file %s OK", camera_name.c_str(), fov, calib_path.c_str()); + cv::Mat img = cv::imread(image_path, cv::IMREAD_ANYCOLOR); + cv::imshow("Raw", img); + cv::waitKey(1); + printf("Read image file %s OK", image_path.c_str()); + YAML::Node config = YAML::LoadFile(calib_path); + auto ret = D2FrontendParams::readCameraConfig(camera_name, config[camera_name]); + printf("Read camera %s fov %f calib from file %s OK", camera_name.c_str(), + fov, calib_path.c_str()); - //Initialize undistort - int undistort_width = img.cols; - int undistort_height = img.cols/2; - int pinhole2_height = img.cols*0.75; - FisheyeUndist undistort(ret.first, 0, fov, true, FisheyeUndist::UndistortCylindrical, undistort_width, undistort_height); - auto imgs = undistort.undist_all(img, true); - cv::imshow("UndistortCylindrical", imgs[0]); - auto img_cuda = undistort.undist_id_cuda(img, 0); - cv::Mat img_cpu(img_cuda); - cv::imshow("UndistortCylindrical_cuda", img_cpu); + // Initialize undistort + int undistort_width = img.cols; + int undistort_height = img.cols / 2; + int pinhole2_height = img.cols * 0.75; + FisheyeUndist undistort(ret.first, 0, fov, true, + FisheyeUndist::UndistortCylindrical, undistort_width, + undistort_height); + auto imgs = undistort.undist_all(img, true); + cv::imshow("UndistortCylindrical", imgs[0]); + auto img_cuda = undistort.undist_id_cuda(img, 0); + cv::Mat img_cpu(img_cuda); + cv::imshow("UndistortCylindrical_cuda", img_cpu); - FisheyeUndist undistort5(ret.first, 0, fov, true, FisheyeUndist::UndistortPinhole5, undistort_width, undistort_height); - imgs = undistort5.undist_all(img, true); - cv::imshow("UndistortPinhole5", imgs[0]); + FisheyeUndist undistort5(ret.first, 0, fov, true, + FisheyeUndist::UndistortPinhole5, undistort_width, + undistort_height); + imgs = undistort5.undist_all(img, true); + cv::imshow("UndistortPinhole5", imgs[0]); - FisheyeUndist undistort2(ret.first, 0, fov, true, FisheyeUndist::UndistortPinhole2, undistort_width, pinhole2_height); - imgs = undistort2.undist_all(img, true); - cv::hconcat(imgs[0], imgs[1], img); - cv::imshow("UndistortPinhole2", img); + FisheyeUndist undistort2(ret.first, 0, fov, true, + FisheyeUndist::UndistortPinhole2, undistort_width, + pinhole2_height); + imgs = undistort2.undist_all(img, true); + cv::hconcat(imgs[0], imgs[1], img); + cv::imshow("UndistortPinhole2", img); - double err = 0; - for (int i = 0; i< undistort_width; i ++ ) { - for (int j = 0; j < undistort_height; j ++ ) { - Vector2d p(i, j), p_rep; - Vector3d p3d; - undistort.cam_top->liftProjective(p, p3d); - undistort.cam_top->spaceToPlane(p3d, p_rep); - err = err + (p_rep - p).norm(); - } + double err = 0; + for (int i = 0; i < undistort_width; i++) { + for (int j = 0; j < undistort_height; j++) { + Vector2d p(i, j), p_rep; + Vector3d p3d; + undistort.cam_top->liftProjective(p, p3d); + undistort.cam_top->spaceToPlane(p3d, p_rep); + err = err + (p_rep - p).norm(); } - printf("Undistort error %f\n", err); + } + printf("Undistort error %f\n", err); - cv::waitKey(0); + cv::waitKey(0); } \ No newline at end of file diff --git a/d2frontend/tests/d2frontend_network_tester.cpp b/d2frontend/tests/d2frontend_network_tester.cpp index f5fdbbe9..7cfbf245 100644 --- a/d2frontend/tests/d2frontend_network_tester.cpp +++ b/d2frontend/tests/d2frontend_network_tester.cpp @@ -1,72 +1,73 @@ +#include + #include "d2frontend/loop_net.h" #include "swarm_msgs/swarm_lcm_converter.hpp" #include "swarmcomm_msgs/drone_network_status.h" -#include using namespace D2FrontEnd; class SwarmNetworkTester { - LoopNet loopnet; - ros::Publisher drone_status_pub; - ros::Timer timer; - std::thread th; -public: - int self_id = -1; - SwarmNetworkTester(ros::NodeHandle & nh): - loopnet("udpm://224.0.0.251:7667?ttl=255", false, false) { - nh.param("self_id", self_id, -1); - loopnet.msg_recv_rate_callback = [&](int drone_id, float rate) { - this->receive_rate_callback(drone_id, rate); - }; + LoopNet loopnet; + ros::Publisher drone_status_pub; + ros::Timer timer; + std::thread th; - loopnet.frame_desc_callback = [&](const VisualImageDescArray &) { - }; + public: + int self_id = -1; + SwarmNetworkTester(ros::NodeHandle &nh) + : loopnet("udpm://224.0.0.251:7667?ttl=255", false, false) { + nh.param("self_id", self_id, -1); + loopnet.msg_recv_rate_callback = [&](int drone_id, float rate) { + this->receive_rate_callback(drone_id, rate); + }; - drone_status_pub = nh.advertise( - "/swarm_loop/drone_network_status", 10); - timer = nh.createTimer(ros::Duration(1.0), &SwarmNetworkTester::timerCallback, this); - th = std::thread([&] { - while(0 == loopnet.lcmHandle()) { - } - }); + loopnet.frame_desc_callback = [&](const VisualImageDescArray &) {}; - } + drone_status_pub = nh.advertise( + "/swarm_loop/drone_network_status", 10); + timer = nh.createTimer(ros::Duration(1.0), + &SwarmNetworkTester::timerCallback, this); + th = std::thread([&] { + while (0 == loopnet.lcmHandle()) { + } + }); + } - void receive_rate_callback(int drone_id, float rate) { - swarmcomm_msgs::drone_network_status status; - status.header.stamp = ros::Time::now(); - status.drone_id = drone_id; - status.active = true; - status.quality = rate; - status.bandwidth = -1; - status.hops = -1; - drone_status_pub.publish(status); - } + void receive_rate_callback(int drone_id, float rate) { + swarmcomm_msgs::drone_network_status status; + status.header.stamp = ros::Time::now(); + status.drone_id = drone_id; + status.active = true; + status.quality = rate; + status.bandwidth = -1; + status.hops = -1; + drone_status_pub.publish(status); + } - void timerCallback(const ros::TimerEvent & e) { - static int count = 0; - ImageDescriptor_t dummy_desc; - dummy_desc.header.timestamp = toLCMTime(ros::Time::now()); - dummy_desc.header.drone_id = self_id; - dummy_desc.header.msg_id = count + self_id*1000000; - dummy_desc.landmark_num = 200; - dummy_desc.landmark_descriptor.resize(200*64); - dummy_desc.landmark_descriptor_size = dummy_desc.landmark_descriptor.size(); - dummy_desc.header.image_desc.resize(4096); - dummy_desc.header.image_desc_size = 4096; - dummy_desc.image_size = 0; - dummy_desc.landmarks.resize(dummy_desc.landmark_num); - for (auto & lm : dummy_desc.landmarks) { - lm.compact.flag = 1; - } - loopnet.broadcastImgDesc(dummy_desc, SlidingWindow_t()); - count++; + void timerCallback(const ros::TimerEvent &e) { + static int count = 0; + ImageDescriptor_t dummy_desc; + dummy_desc.header.timestamp = toLCMTime(ros::Time::now()); + dummy_desc.header.drone_id = self_id; + dummy_desc.header.msg_id = count + self_id * 1000000; + dummy_desc.landmark_num = 200; + dummy_desc.landmark_descriptor.resize(200 * 64); + dummy_desc.landmark_descriptor_size = dummy_desc.landmark_descriptor.size(); + dummy_desc.header.image_desc.resize(4096); + dummy_desc.header.image_desc_size = 4096; + dummy_desc.image_size = 0; + dummy_desc.landmarks.resize(dummy_desc.landmark_num); + for (auto &lm : dummy_desc.landmarks) { + lm.compact.flag = 1; } + loopnet.broadcastImgDesc(dummy_desc, SlidingWindow_t()); + count++; + } }; -int main(int argc, char*argv[]) { - ros::init(argc, argv, "swarm_network_tester"); - ros::NodeHandle nh("swarm_network_tester"); - SwarmNetworkTester tester(nh); - ROS_INFO("swarm network tester at %d\nIniting\n", tester.self_id); - ros::spin(); +int main(int argc, char *argv[]) { + ros::init(argc, argv, "swarm_network_tester"); + ros::NodeHandle nh("swarm_network_tester"); + SwarmNetworkTester tester(nh); + ROS_INFO("swarm network tester at %d\nIniting\n", tester.self_id); + ros::spin(); } \ No newline at end of file diff --git a/d2frontend/tests/d2frontend_test.cpp b/d2frontend/tests/d2frontend_test.cpp index 2596f7e8..7e855a32 100644 --- a/d2frontend/tests/d2frontend_test.cpp +++ b/d2frontend/tests/d2frontend_test.cpp @@ -1,87 +1,97 @@ +#include #include + +#include #include #include -#include -#include -using namespace std::chrono; +using namespace std::chrono; #define DEBUG_IMAGE -LoopDetector ld("/home/xuhao/swarm_ws/src/swarm_localization/support_files/ORBvoc.txt"); +LoopDetector ld( + "/home/xuhao/swarm_ws/src/swarm_localization/support_files/ORBvoc.txt"); -std::string camera_config_path = - "/home/xuhao/swarm_ws/src/VINS-Fusion-gpu/config/vi_car/cam0_mei.yaml"; -std::string BRIEF_PATTHER_FILE = "/home/xuhao/swarm_ws/src/VINS-Fusion-gpu/support_files/brief_pattern.yml"; +std::string camera_config_path = + "/home/xuhao/swarm_ws/src/VINS-Fusion-gpu/config/vi_car/cam0_mei.yaml"; +std::string BRIEF_PATTHER_FILE = + "/home/xuhao/swarm_ws/src/VINS-Fusion-gpu/support_files/brief_pattern.yml"; std::string camera_topic = "/cam0/image_raw"; std::string viokeyframe_topic = "/vins_estimator/viokeyframe"; LoopCam cam(camera_config_path, BRIEF_PATTHER_FILE); -int test_loop_cam(ros::NodeHandle & nh) { - std::string test_img_file = "/home/xuhao/image001.png"; - ros::Publisher img_des_pub = nh.advertise("/swarm_loop/new_image_des", 1); - cv::Mat img = cv::imread(test_img_file.c_str(), cv::IMREAD_GRAYSCALE); - auto des = toROSMsg(cam.feature_detect(img)); - ros::Rate r(10); - while (true) { - ROS_INFO("Publishing the descriptors...."); - img_des_pub.publish(des); - ros::spinOnce(); - r.sleep(); - } +int test_loop_cam(ros::NodeHandle& nh) { + std::string test_img_file = "/home/xuhao/image001.png"; + ros::Publisher img_des_pub = + nh.advertise("/swarm_loop/new_image_des", 1); + cv::Mat img = cv::imread(test_img_file.c_str(), cv::IMREAD_GRAYSCALE); + auto des = toROSMsg(cam.feature_detect(img)); + ros::Rate r(10); + while (true) { + ROS_INFO("Publishing the descriptors...."); + img_des_pub.publish(des); + ros::spinOnce(); + r.sleep(); + } } - void image_callback_0(const sensor_msgs::ImageConstPtr& msg) { - static int c = 0; - if (c % 10 == 0) { - cv_bridge::CvImageConstPtr ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::MONO8); - auto img_des = cam.feature_detect(ptr->image); - ld.onImageRecv(img_des, ptr->image); - } - c++; + static int c = 0; + if (c % 10 == 0) { + cv_bridge::CvImageConstPtr ptr = + cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::MONO8); + auto img_des = cam.feature_detect(ptr->image); + ld.onImageRecv(img_des, ptr->image); + } + c++; } - void image_callback_1(const sensor_msgs::ImageConstPtr& msg) { - cam.on_camera_message(msg); + cam.on_camera_message(msg); } -void vioKFCallback(const vins::VIOKeyframe & viokf) { - auto start = high_resolution_clock::now(); - auto ret = cam.on_keyframe_message(viokf); - std::cout << "Cam Cost " << duration_cast(high_resolution_clock::now() - start).count()/1000.0 << "ms" << std::endl; - //Check ides vaild +void vioKFCallback(const vins::VIOKeyframe& viokf) { + auto start = high_resolution_clock::now(); + auto ret = cam.on_keyframe_message(viokf); + std::cout << "Cam Cost " + << duration_cast(high_resolution_clock::now() - start) + .count() / + 1000.0 + << "ms" << std::endl; + // Check ides vaild #ifdef DEBUG_IMAGE - ld.onImageRecv(ret.first, ret.second); + ld.onImageRecv(ret.first, ret.second); #else - ld.onImageRecv(ret.first); + ld.onImageRecv(ret.first); #endif - std::cout << "Cam+LD Cost " << duration_cast(high_resolution_clock::now() - start).count()/1000.0 << "ms" << std::endl; + std::cout << "Cam+LD Cost " + << duration_cast(high_resolution_clock::now() - start) + .count() / + 1000.0 + << "ms" << std::endl; } -int test_loop_detector(ros::NodeHandle & nh) { - auto sb = nh.subscribe(camera_topic, 1000, image_callback_0); - ros::spin(); +int test_loop_detector(ros::NodeHandle& nh) { + auto sb = nh.subscribe(camera_topic, 1000, image_callback_0); + ros::spin(); } -int test_single_loop(ros::NodeHandle & nh) { - auto sb = nh.subscribe(camera_topic, 1000, image_callback_1); - ros::spin(); +int test_single_loop(ros::NodeHandle& nh) { + auto sb = nh.subscribe(camera_topic, 1000, image_callback_1); + ros::spin(); } +int main(int argc, char** argv) { + ROS_INFO("SWARM_LOOP INIT"); + srand(time(NULL)); -int main(int argc, char **argv) { - ROS_INFO("SWARM_LOOP INIT"); - srand(time(NULL)); + ros::init(argc, argv, "swarm_loop_test"); + ros::NodeHandle nh("swarm_loop_test"); - ros::init(argc, argv, "swarm_loop_test"); - ros::NodeHandle nh("swarm_loop_test"); + // test_loop_detector(nh); + test_single_loop(nh); - //test_loop_detector(nh); - test_single_loop(nh); - - return 0; + return 0; } \ No newline at end of file diff --git a/d2frontend/tests/loop_tensorrt_test.cpp b/d2frontend/tests/loop_tensorrt_test.cpp index c34d5263..1f90f8b6 100644 --- a/d2frontend/tests/loop_tensorrt_test.cpp +++ b/d2frontend/tests/loop_tensorrt_test.cpp @@ -1,145 +1,163 @@ -#include "d2frontend/d2frontend_params.h" -#include "d2frontend/CNN/superpoint_onnx.h" -#include "d2frontend/CNN/superglue_onnx.h" +#include + #include "d2frontend/CNN/mobilenetvlad_onnx.h" +#include "d2frontend/CNN/superglue_onnx.h" +#include "d2frontend/CNN/superpoint_onnx.h" +#include "d2frontend/d2frontend_params.h" #include "d2frontend/utils.h" -#include using namespace Swarm; using namespace D2FrontEnd; -D2FrontendParams * D2FrontEnd::params = new D2FrontendParams; +D2FrontendParams* D2FrontEnd::params = new D2FrontendParams; int main(int argc, char* argv[]) { - if (argc < 2) { - return -1; - } - namespace po = boost::program_options; - po::options_description desc("Allowed options"); - desc.add_options() - ("help", "produce help message") - ("superpoint,s", po::value()->default_value(""), "model path of SuperPoint") - ("netvlad,n", po::value()->default_value(""), "model path of NetVLAD") - ("superglue,g", po::value()->default_value(""), "model path of SuperGlue") - ("limage,l", po::value()->default_value(""), "model path of image0") - ("rimage,r", po::value()->default_value(""), "model path of image1") - ("focal,f", po::value()->default_value(384), "image focal length") - ("width,w", po::value()->default_value(640), "image width") - ("height,h", po::value()->default_value(480), "image height") - ("tensorrt", po::value()->default_value(1), "use tensorrt") - ("num-test,t", po::value()->default_value(100), "num of tests"); - po::variables_map vm; - po::store(po::parse_command_line(argc, argv, desc), vm); - po::notify(vm); - // params->enable_perf_output = true; + if (argc < 2) { + return -1; + } + namespace po = boost::program_options; + po::options_description desc("Allowed options"); + desc.add_options()("help", "produce help message")( + "superpoint,s", po::value()->default_value(""), + "model path of SuperPoint")("netvlad,n", + po::value()->default_value(""), + "model path of NetVLAD")( + "superglue,g", po::value()->default_value(""), + "model path of SuperGlue")("limage,l", + po::value()->default_value(""), + "model path of image0")( + "rimage,r", po::value()->default_value(""), + "model path of image1")( + "focal,f", po::value()->default_value(384), "image focal length")( + "width,w", po::value()->default_value(640), "image width")( + "height,h", po::value()->default_value(480), "image height")( + "tensorrt", po::value()->default_value(1), "use tensorrt")( + "num-test,t", po::value()->default_value(100), "num of tests"); + po::variables_map vm; + po::store(po::parse_command_line(argc, argv, desc), vm); + po::notify(vm); + // params->enable_perf_output = true; - int width = vm["width"].as(); - int height = vm["height"].as(); - int num_test = vm["num-test"].as(); - double focal = vm["focal"].as(); - bool use_tensorrt = vm["tensorrt"].as(); - printf("width: %d, height: %d, num_test: %d, focal: %f, use_tensorrt: %d\n", width, height, num_test, focal, use_tensorrt); + int width = vm["width"].as(); + int height = vm["height"].as(); + int num_test = vm["num-test"].as(); + double focal = vm["focal"].as(); + bool use_tensorrt = vm["tensorrt"].as(); + printf("width: %d, height: %d, num_test: %d, focal: %f, use_tensorrt: %d\n", + width, height, num_test, focal, use_tensorrt); - cv::Mat img0 = cv::imread(vm["limage"].as()); - cv::Mat img1; - if (vm["rimage"].as() != "") { - img1 = cv::imread(vm["rimage"].as()); - } - else { - img1 = img0.clone(); - } - std::vector local_desc0, local_desc1, scores0, scores1; - std::vector kps0, kps1; - std::vector kps0_norm, kps1_norm; + cv::Mat img0 = cv::imread(vm["limage"].as()); + cv::Mat img1; + if (vm["rimage"].as() != "") { + img1 = cv::imread(vm["rimage"].as()); + } else { + img1 = img0.clone(); + } + std::vector local_desc0, local_desc1, scores0, scores1; + std::vector kps0, kps1; + std::vector kps0_norm, kps1_norm; - cv::Mat img_gray0, img_gray1; - cv::resize(img0, img_gray0, cv::Size(width, height)); - cv::resize(img1, img_gray1, cv::Size(width, height)); - cv::cvtColor(img_gray0, img_gray0, cv::COLOR_BGR2GRAY); - cv::cvtColor(img_gray1, img_gray1, cv::COLOR_BGR2GRAY); - cv::Mat show; + cv::Mat img_gray0, img_gray1; + cv::resize(img0, img_gray0, cv::Size(width, height)); + cv::resize(img1, img_gray1, cv::Size(width, height)); + cv::cvtColor(img_gray0, img_gray0, cv::COLOR_BGR2GRAY); + cv::cvtColor(img_gray1, img_gray1, cv::COLOR_BGR2GRAY); + cv::Mat show; - SuperGlueOnnx sg_onnx(vm["superglue"].as()); - MobileNetVLADONNX netvlad_onnx(vm["netvlad"].as(), 640, 480, use_tensorrt); - SuperPointONNX sp_onnx(vm["superpoint"].as(), 20, "", "", 640, 480, 0.015, 200, use_tensorrt); - std::cout << "Finish loading models" << std::endl; + SuperGlueOnnx sg_onnx(vm["superglue"].as()); + MobileNetVLADONNX netvlad_onnx(vm["netvlad"].as(), 640, 480, + use_tensorrt); + SuperPointONNX sp_onnx(vm["superpoint"].as(), 20, "", "", 640, + 480, 0.015, 200, use_tensorrt); + std::cout << "Finish loading models" << std::endl; - sp_onnx.inference(img_gray0, kps0, local_desc0, scores0); - sp_onnx.inference(img_gray1, kps1, local_desc1, scores1); - std::cout << "Finish inference superpoint features" << kps0.size() << ":" << kps1.size() << std::endl; - Eigen::Map desc0(local_desc0.data(), local_desc0.size()); - Eigen::Map desc1(local_desc1.data(), local_desc1.size()); - std::cout << "minmax coeff of superpoint" << desc0.minCoeff() << ":" << desc0.maxCoeff() << std::endl; - std::cout << "minmax coeff of superpoint" << desc1.minCoeff() << ":" << desc1.maxCoeff() << std::endl; - double max0 = desc0.cwiseAbs().maxCoeff(); - double max1 = desc1.cwiseAbs().maxCoeff(); - //Convert to int8 with scale maxCoeff()/127 - VectorXi desc_i0 = (desc0 / max0 * 127).template cast(); - VectorXi desc_i1 = (desc1 / max1 * 127).template cast(); + sp_onnx.inference(img_gray0, kps0, local_desc0, scores0); + sp_onnx.inference(img_gray1, kps1, local_desc1, scores1); + std::cout << "Finish inference superpoint features" << kps0.size() << ":" + << kps1.size() << std::endl; + Eigen::Map desc0(local_desc0.data(), local_desc0.size()); + Eigen::Map desc1(local_desc1.data(), local_desc1.size()); + std::cout << "minmax coeff of superpoint" << desc0.minCoeff() << ":" + << desc0.maxCoeff() << std::endl; + std::cout << "minmax coeff of superpoint" << desc1.minCoeff() << ":" + << desc1.maxCoeff() << std::endl; + double max0 = desc0.cwiseAbs().maxCoeff(); + double max1 = desc1.cwiseAbs().maxCoeff(); + // Convert to int8 with scale maxCoeff()/127 + VectorXi desc_i0 = (desc0 / max0 * 127).template cast(); + VectorXi desc_i1 = (desc1 / max1 * 127).template cast(); - //Recover float from int8 - desc0 = desc_i0.template cast() / 127 * max0; - desc1 = desc_i1.template cast() / 127 * max1; + // Recover float from int8 + desc0 = desc_i0.template cast() / 127 * max0; + desc1 = desc_i1.template cast() / 127 * max1; - - auto global_desc0 = netvlad_onnx.inference(img_gray0); - auto global_desc1 = netvlad_onnx.inference(img_gray1); - VectorXf gdesc0 = VectorXf::Map(global_desc0.data(), global_desc0.size()); - VectorXf gdesc1 = VectorXf::Map(global_desc1.data(), global_desc1.size()); - std::cout << "minmax coeff of netvlad0 " << gdesc0.minCoeff() << ":" << gdesc0.maxCoeff() << "norm: " << gdesc0.norm() << std::endl; - std::cout << "minmax coeff of netvlad1 " << gdesc1.minCoeff() << ":" << gdesc1.maxCoeff() << "norm: " << gdesc1.norm() << std::endl; - for (size_t i = 0; i < kps0.size(); i ++) { - kps0_norm.emplace_back(cv::Point2f((kps0[i].x-width/2)/focal, (kps0[i].y-height/2)/focal)); - } - for (size_t i = 0; i < kps1.size(); i ++) { - kps1_norm.emplace_back(cv::Point2f((kps1[i].x-width/2)/focal, (kps1[i].y-height/2)/focal)); + auto global_desc0 = netvlad_onnx.inference(img_gray0); + auto global_desc1 = netvlad_onnx.inference(img_gray1); + VectorXf gdesc0 = VectorXf::Map(global_desc0.data(), global_desc0.size()); + VectorXf gdesc1 = VectorXf::Map(global_desc1.data(), global_desc1.size()); + std::cout << "minmax coeff of netvlad0 " << gdesc0.minCoeff() << ":" + << gdesc0.maxCoeff() << "norm: " << gdesc0.norm() << std::endl; + std::cout << "minmax coeff of netvlad1 " << gdesc1.minCoeff() << ":" + << gdesc1.maxCoeff() << "norm: " << gdesc1.norm() << std::endl; + for (size_t i = 0; i < kps0.size(); i++) { + kps0_norm.emplace_back(cv::Point2f((kps0[i].x - width / 2) / focal, + (kps0[i].y - height / 2) / focal)); + } + for (size_t i = 0; i < kps1.size(); i++) { + kps1_norm.emplace_back(cv::Point2f((kps1[i].x - width / 2) / focal, + (kps1[i].y - height / 2) / focal)); + } + std::cout << "Finish inference mobilenetvlad" << std::endl; + auto matches = sg_onnx.inference(kps0_norm, kps1_norm, local_desc0, + local_desc1, scores0, scores1); + std::cout << "Finish inference superglue" << std::endl; + std::vector _kps0; + std::vector _kps1; + for (size_t i = 0; i < kps0.size(); i++) { + cv::circle(img0, kps0[i], scores0[i] * 10, cv::Scalar(255, 0, 0), 1, + cv::LINE_AA); + _kps0.emplace_back(cv::KeyPoint(kps0[i].x, kps0[i].y, scores0[i])); + kps0_norm.emplace_back(cv::Point2f(kps0[i].x / width, kps0[i].y / height)); + } + for (size_t i = 0; i < kps1.size(); i++) { + cv::circle(img1, kps1[i], scores1[i] * 10, cv::Scalar(255, 0, 0), 1, + cv::LINE_AA); + _kps1.emplace_back(cv::KeyPoint(kps1[i].x, kps1[i].y, scores1[i])); + kps1_norm.emplace_back(cv::Point2f(kps0[i].x / width, kps0[i].y / height)); + } + cv::hconcat(img0, img1, show); + cv::imshow("Procesed", show); + cv::waitKey(30); + if (num_test > 0) { + D2FrontEnd::TicToc tic; + for (unsigned int i = 0; i < num_test; i++) { + sp_onnx.inference(img_gray0, kps0, local_desc0, scores0); } - std::cout << "Finish inference mobilenetvlad" << std::endl; - auto matches = sg_onnx.inference(kps0_norm, kps1_norm, local_desc0, local_desc1, scores0, scores1); - std::cout << "Finish inference superglue" << std::endl; - std::vector _kps0; - std::vector _kps1; - for (size_t i = 0; i < kps0.size(); i ++) { - cv::circle(img0, kps0[i], scores0[i]*10, cv::Scalar(255, 0, 0), 1, cv::LINE_AA); - _kps0.emplace_back(cv::KeyPoint(kps0[i].x, kps0[i].y, scores0[i])); - kps0_norm.emplace_back(cv::Point2f(kps0[i].x / width, kps0[i].y / height)); - } - for (size_t i = 0; i < kps1.size(); i ++) { - cv::circle(img1, kps1[i], scores1[i]*10, cv::Scalar(255, 0, 0), 1, cv::LINE_AA); - _kps1.emplace_back(cv::KeyPoint(kps1[i].x, kps1[i].y, scores1[i])); - kps1_norm.emplace_back(cv::Point2f(kps0[i].x / width, kps0[i].y / height)); - } - cv::hconcat(img0, img1, show); - cv::imshow("Procesed", show); - cv::waitKey(30); - if (num_test > 0) { - D2FrontEnd::TicToc tic; - for (unsigned int i = 0; i < num_test; i ++) { - sp_onnx.inference(img_gray0, kps0, local_desc0, scores0); - } - double dt_sp = tic.toc(); - - D2FrontEnd::TicToc tic2; - for (unsigned int i = 0; i < num_test; i ++) { - netvlad_onnx.inference(img_gray0); - } - double dt_netvlad = tic2.toc(); + double dt_sp = tic.toc(); - D2FrontEnd::TicToc tic3; - for (unsigned int i = 0; i < num_test; i ++) { - sg_onnx.inference(kps0, kps1, local_desc0, local_desc1, scores0, scores1); - } - double dt_superglue = tic3.toc(); + D2FrontEnd::TicToc tic2; + for (unsigned int i = 0; i < num_test; i++) { + netvlad_onnx.inference(img_gray0); + } + double dt_netvlad = tic2.toc(); - printf("Run %d tests on SuperPoint, NetVLAD, SuperGlue\n", num_test); - printf("SuperPoint: %.1fms\n", dt_sp/num_test); - printf("NetVLAD: %.1fms\n", dt_netvlad/num_test); - printf("SuperGlue: %.1fms\n", dt_superglue/num_test); + D2FrontEnd::TicToc tic3; + for (unsigned int i = 0; i < num_test; i++) { + sg_onnx.inference(kps0, kps1, local_desc0, local_desc1, scores0, scores1); } - cv::drawMatches(img0, _kps0, img1, _kps1, matches, show); - // for (auto match : matches) { - // //Draw match scores on image - // cv::putText(show, std::to_string(1-match.distance), kps0[match.queryIdx], cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 255, 0), 1, cv::LINE_AA); - // } - cv::imshow("Matches", show); - cv::waitKey(-1); + double dt_superglue = tic3.toc(); + + printf("Run %d tests on SuperPoint, NetVLAD, SuperGlue\n", num_test); + printf("SuperPoint: %.1fms\n", dt_sp / num_test); + printf("NetVLAD: %.1fms\n", dt_netvlad / num_test); + printf("SuperGlue: %.1fms\n", dt_superglue / num_test); + } + cv::drawMatches(img0, _kps0, img1, _kps1, matches, show); + // for (auto match : matches) { + // //Draw match scores on image + // cv::putText(show, std::to_string(1-match.distance), + // kps0[match.queryIdx], cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 255, + // 0), 1, cv::LINE_AA); + // } + cv::imshow("Matches", show); + cv::waitKey(-1); } \ No newline at end of file diff --git a/d2pgo/src/ARockPGO.cpp b/d2pgo/src/ARockPGO.cpp index 0ed73e5c..d44ac423 100644 --- a/d2pgo/src/ARockPGO.cpp +++ b/d2pgo/src/ARockPGO.cpp @@ -1,108 +1,108 @@ #include "ARockPGO.hpp" + #include "d2pgo.h" namespace D2PGO { -void ARockPGO::inputDPGOData(const DPGOData & data) { - // printf("[ARockPGO@%d]input DPGOData from %d\n", self_id, data.drone_id); - const std::lock_guard lock(pgo_data_mutex); - pgo_data.emplace_back(data); +void ARockPGO::inputDPGOData(const DPGOData& data) { + // printf("[ARockPGO@%d]input DPGOData from %d\n", self_id, data.drone_id); + const std::lock_guard lock(pgo_data_mutex); + pgo_data.emplace_back(data); } -void ARockPGO::processPGOData(const DPGOData & data) { - // printf("[ARockPGO@%d]process DPGOData from %d\n", self_id, data.drone_id); - auto drone_id = data.drone_id; - for (auto it: data.frame_duals) { - auto frame_id = it.first; - auto & dual = it.second; - if (SolverWrapper::state->hasFrame(frame_id)) { - state_type * ptr; - if (perturb_mode) { - ptr = SolverWrapper::state->getPerturbState(frame_id); - } else { - ptr = SolverWrapper::state->getPoseState(frame_id); - } - if (all_estimating_params.find(ptr) != all_estimating_params.end()) { - if (data.target_id == self_id || !hasDualState(ptr, drone_id)) { - // if (data.target_id != self_id) { - // printf("[ARockPGO@%d] Discover dual for %ld from %d->%d\n", self_id, frame_id, data.drone_id, data.target_id); - // } - updated = true; - bool create = false; - auto param_info = all_estimating_params.at(ptr); - //Then we check it this param has dual. - if (!hasDualState(ptr, drone_id)) { - //Then we create a new dual state. - createDualState(param_info, drone_id, true); - create = true; - } - //Then we update the dual state. - if (param_info.type == ParamsType::POSE) { - Swarm::Pose pose(dual); - pose.to_vector(dual_states_remote[drone_id][ptr].data()); - if (create) - pose.to_vector(dual_states_local[drone_id][ptr].data()); - } else if (param_info.type == ParamsType::POSE_4D) { - dual_states_remote[drone_id][ptr] = dual; - if (create) - dual_states_local[drone_id][ptr] = dual; - } else if (param_info.type == ParamsType::POSE_PERTURB_6D) { - // printf("[ARockPGO@%d] updating dual state for %d@%d.:", self_id, param_info.id, drone_id); - // std::cout << dual.transpose() << std::endl; - dual_states_remote[drone_id][ptr] = dual; - if (create) - dual_states_local[drone_id][ptr] = dual; - } - } - } +void ARockPGO::processPGOData(const DPGOData& data) { + // printf("[ARockPGO@%d]process DPGOData from %d\n", self_id, data.drone_id); + auto drone_id = data.drone_id; + for (auto it : data.frame_duals) { + auto frame_id = it.first; + auto& dual = it.second; + if (SolverWrapper::state->hasFrame(frame_id)) { + state_type* ptr; + if (perturb_mode) { + ptr = SolverWrapper::state->getPerturbState(frame_id); + } else { + ptr = SolverWrapper::state->getPoseState(frame_id); + } + if (all_estimating_params.find(ptr) != all_estimating_params.end()) { + if (data.target_id == self_id || !hasDualState(ptr, drone_id)) { + // if (data.target_id != self_id) { + // printf("[ARockPGO@%d] Discover dual for %ld from %d->%d\n", + // self_id, frame_id, data.drone_id, data.target_id); + // } + updated = true; + bool create = false; + auto param_info = all_estimating_params.at(ptr); + // Then we check it this param has dual. + if (!hasDualState(ptr, drone_id)) { + // Then we create a new dual state. + createDualState(param_info, drone_id, true); + create = true; + } + // Then we update the dual state. + if (param_info.type == ParamsType::POSE) { + Swarm::Pose pose(dual); + pose.to_vector(dual_states_remote[drone_id][ptr].data()); + if (create) pose.to_vector(dual_states_local[drone_id][ptr].data()); + } else if (param_info.type == ParamsType::POSE_4D) { + dual_states_remote[drone_id][ptr] = dual; + if (create) dual_states_local[drone_id][ptr] = dual; + } else if (param_info.type == ParamsType::POSE_PERTURB_6D) { + // printf("[ARockPGO@%d] updating dual state for %d@%d.:", self_id, + // param_info.id, drone_id); std::cout << dual.transpose() << + // std::endl; + dual_states_remote[drone_id][ptr] = dual; + if (create) dual_states_local[drone_id][ptr] = dual; + } } + } } + } } void ARockPGO::receiveAll() { - const std::lock_guard lock(pgo_data_mutex); - //Process and delete data in pgo_data. - for (auto it = pgo_data.begin(); it != pgo_data.end();) { - auto data = *it; - processPGOData(data); - it = pgo_data.erase(it); - } + const std::lock_guard lock(pgo_data_mutex); + // Process and delete data in pgo_data. + for (auto it = pgo_data.begin(); it != pgo_data.end();) { + auto data = *it; + processPGOData(data); + it = pgo_data.erase(it); + } } void ARockPGO::broadcastData() { - // printf("ARockPGO::broadcastData\n"); - const std::lock_guard lock(pgo_data_mutex); - //broadcast the data. - for (auto it : dual_states_local) { - DPGOData data; - data.type = DPGO_DELTA_POSE_DUAL; - data.stamp = ros::Time::now().toSec(); - data.drone_id = self_id; - data.target_id = it.first; - data.reference_frame_id = SolverWrapper::state->getReferenceFrameId(); - // printf("ARockPGO::broadcastData of drone %d\n", data.target_id); - for (auto it2: it.second) { - auto ptr = it2.first; - auto & dual_state = it2.second; - ParamInfo param = all_estimating_params.at(ptr); - Swarm::Pose pose; - if (param.type == ParamsType::POSE) { - pose = Swarm::Pose(dual_state.data()); - } else if (param.type == ParamsType::POSE_4D) { - pose = Swarm::Pose(dual_state.data(), true); - } else if (param.type == ParamsType::POSE_PERTURB_6D) {//TODO:If perturb - pose = SolverWrapper::state->getFramebyId(param.id)->odom.pose(); - } - data.frame_duals[param.id] = dual_state; - data.frame_poses[param.id] = pose; - } - printf("[Drone %d] DPGO broadcast poses %ld\n", self_id, data.frame_poses.size()); - pgo->broadcastData(data); + // printf("ARockPGO::broadcastData\n"); + const std::lock_guard lock(pgo_data_mutex); + // broadcast the data. + for (auto it : dual_states_local) { + DPGOData data; + data.type = DPGO_DELTA_POSE_DUAL; + data.stamp = ros::Time::now().toSec(); + data.drone_id = self_id; + data.target_id = it.first; + data.reference_frame_id = SolverWrapper::state->getReferenceFrameId(); + // printf("ARockPGO::broadcastData of drone %d\n", data.target_id); + for (auto it2 : it.second) { + auto ptr = it2.first; + auto& dual_state = it2.second; + ParamInfo param = all_estimating_params.at(ptr); + Swarm::Pose pose; + if (param.type == ParamsType::POSE) { + pose = Swarm::Pose(dual_state.data()); + } else if (param.type == ParamsType::POSE_4D) { + pose = Swarm::Pose(dual_state.data(), true); + } else if (param.type == ParamsType::POSE_PERTURB_6D) { // TODO:If + // perturb + pose = SolverWrapper::state->getFramebyId(param.id)->odom.pose(); + } + data.frame_duals[param.id] = dual_state; + data.frame_poses[param.id] = pose; } + printf("[Drone %d] DPGO broadcast poses %ld\n", self_id, + data.frame_poses.size()); + pgo->broadcastData(data); + } } -void ARockPGO::setStateProperties() { - pgo->setStateProperties(getProblem()); -} +void ARockPGO::setStateProperties() { pgo->setStateProperties(getProblem()); } -}; \ No newline at end of file +}; // namespace D2PGO \ No newline at end of file diff --git a/d2pgo/src/d2pgo.cpp b/d2pgo/src/d2pgo.cpp index e7eeb9a5..519bd619 100644 --- a/d2pgo/src/d2pgo.cpp +++ b/d2pgo/src/d2pgo.cpp @@ -1,620 +1,702 @@ #include "d2pgo.h" -#include "ARockPGO.hpp" -#include -#include + #include +#include + #include +#include + #include "../test/posegraph_g2o.hpp" +#include "ARockPGO.hpp" #include "rot_init/rotation_initialization.hpp" namespace D2PGO { void D2PGO::addFrame(D2BaseFrame frame) { - const Guard lock(state_lock); - if (config.is_realtime && state.hasDrone(frame.drone_id)) { - //Use ego motion and current estimation to predict the frame pose - auto ego_motion = frame.initial_ego_pose; - auto cur_est_last_frame = state.getFrames(frame.drone_id).back()->odom.pose(); - auto ego_motion_last_frame = state.getFrames(frame.drone_id).back()->initial_ego_pose; - auto pose = cur_est_last_frame*Swarm::Pose::DeltaPose(ego_motion_last_frame, ego_motion); - frame.odom.pose() = pose; - } - state.addFrame(frame); - // printf("[D2PGO@%d]add frame %ld ref %d ego_pose %s pose %s from drone %d\n", self_id, frame.frame_id, frame.reference_frame_id, - // frame.initial_ego_pose.toStr().c_str(), frame.odom.pose().toStr().c_str(), frame.drone_id); - if (ego_motion_trajs.find(frame.drone_id) == ego_motion_trajs.end()) { - Swarm::DroneTrajectory traj(frame.drone_id, true); - ego_motion_trajs[frame.drone_id] = traj; - } - ego_motion_trajs[frame.drone_id].push(frame.stamp, frame.initial_ego_pose, frame.frame_id); - updated = true; - is_rot_init_convergence = false; + const Guard lock(state_lock); + if (state.hasFrame(frame.frame_id)) { + return; + } + if (config.is_realtime && state.hasDrone(frame.drone_id)) { + // Use ego motion and current estimation to predict the frame pose + auto ego_motion = frame.initial_ego_pose; + auto cur_est_last_frame = + state.getFrames(frame.drone_id).back()->odom.pose(); + auto ego_motion_last_frame = + state.getFrames(frame.drone_id).back()->initial_ego_pose; + auto pose = cur_est_last_frame * + Swarm::Pose::DeltaPose(ego_motion_last_frame, ego_motion); + frame.odom.pose() = pose; + } + state.addFrame(frame); + // printf("[D2PGO@%d]add frame %ld ref %d ego_pose %s pose %s from drone + // %d\n", self_id, frame.frame_id, frame.reference_frame_id, + // frame.initial_ego_pose.toStr().c_str(), + // frame.odom.pose().toStr().c_str(), frame.drone_id); + if (ego_motion_trajs.find(frame.drone_id) == ego_motion_trajs.end()) { + Swarm::DroneTrajectory traj(frame.drone_id, true); + ego_motion_trajs[frame.drone_id] = traj; + } + ego_motion_trajs[frame.drone_id].push(frame.stamp, frame.initial_ego_pose, + frame.frame_id); + updated = true; + is_rot_init_convergence = false; } -void D2PGO::addLoop(const Swarm::LoopEdge & loop_info, bool add_state_by_loop) { - const Guard lock(state_lock); - if (loop_info.relative_pose.pos().norm() > config.loop_distance_threshold) { - printf("[D2PGO@%d]loop distance %.1fm too large, ignore\n", self_id, loop_info.relative_pose.pos().norm()); - return; - } - all_loops.emplace_back(loop_info); - all_loops.back().id = all_loops.size() - 1; - // printf("[D2PGO::addLoop@%d] Add edge %ld<->%ld drone %d<->%d hasKF %d %d\n ", self_id, loop_info.keyframe_id_a, - // loop_info.keyframe_id_b, loop_info.id_a, loop_info.id_b, state.hasFrame(loop_info.keyframe_id_a), state.hasFrame(loop_info.keyframe_id_b)); - if (add_state_by_loop) { - // This is for debug... - if (state.hasFrame(loop_info.keyframe_id_a) && !state.hasFrame(loop_info.keyframe_id_b)) { - // Add frame idb to state - D2BaseFrame frame_desc; - frame_desc.drone_id = loop_info.id_b; - frame_desc.frame_id = loop_info.keyframe_id_b; - frame_desc.reference_frame_id = state.getFramebyId(loop_info.keyframe_id_a)->reference_frame_id; - // Initialize pose with known pose a and this loop edge - frame_desc.odom.pose() = state.getFramebyId(loop_info.keyframe_id_a)->odom.pose() * loop_info.relative_pose; - addFrame(frame_desc); - // printf("[D2PGO@%d]add frame %ld pose %s from drone %d\n", self_id, frame_desc.frame_id, - // frame_desc.odom.pose().toStr().c_str(), frame_desc.drone_id); - } else if (!state.hasFrame(loop_info.keyframe_id_a) && state.hasFrame(loop_info.keyframe_id_b)) { - // Add frame idb to state - D2BaseFrame frame_desc; - frame_desc.drone_id = loop_info.id_a; - frame_desc.frame_id = loop_info.keyframe_id_a; - frame_desc.reference_frame_id = state.getFramebyId(loop_info.keyframe_id_b)->reference_frame_id; - // Initialize pose with known pose a and this loop edge - frame_desc.odom.pose() = state.getFramebyId(loop_info.keyframe_id_b)->odom.pose() * loop_info.relative_pose.inverse(); - addFrame(frame_desc); - // printf("[D2PGO@%d]add frame %ld pose %s from drone %d\n", self_id, frame_desc.frame_id, - // frame_desc.odom.pose().toStr().c_str(), frame_desc.drone_id); - } - } - updated = true; - is_rot_init_convergence = false; +void D2PGO::addLoop(const Swarm::LoopEdge& loop_info, bool add_state_by_loop) { + const Guard lock(state_lock); + if (loop_info.relative_pose.pos().norm() > config.loop_distance_threshold) { + printf("[D2PGO@%d]loop distance %.1fm too large, ignore\n", self_id, + loop_info.relative_pose.pos().norm()); + return; + } + all_loops.emplace_back(loop_info); + all_loops.back().id = all_loops.size() - 1; + // printf("[D2PGO::addLoop@%d] Add edge %ld<->%ld drone %d<->%d hasKF %d %d\n + // ", self_id, loop_info.keyframe_id_a, + // loop_info.keyframe_id_b, loop_info.id_a, loop_info.id_b, + // state.hasFrame(loop_info.keyframe_id_a), + // state.hasFrame(loop_info.keyframe_id_b)); + if (add_state_by_loop) { + // This is for debug... + if (state.hasFrame(loop_info.keyframe_id_a) && + !state.hasFrame(loop_info.keyframe_id_b)) { + // Add frame idb to state + D2BaseFrame frame_desc; + frame_desc.drone_id = loop_info.id_b; + frame_desc.frame_id = loop_info.keyframe_id_b; + frame_desc.reference_frame_id = + state.getFramebyId(loop_info.keyframe_id_a)->reference_frame_id; + // Initialize pose with known pose a and this loop edge + frame_desc.odom.pose() = + state.getFramebyId(loop_info.keyframe_id_a)->odom.pose() * + loop_info.relative_pose; + addFrame(frame_desc); + // printf("[D2PGO@%d]add frame %ld pose %s from drone %d\n", self_id, + // frame_desc.frame_id, + // frame_desc.odom.pose().toStr().c_str(), frame_desc.drone_id); + } else if (!state.hasFrame(loop_info.keyframe_id_a) && + state.hasFrame(loop_info.keyframe_id_b)) { + // Add frame idb to state + D2BaseFrame frame_desc; + frame_desc.drone_id = loop_info.id_a; + frame_desc.frame_id = loop_info.keyframe_id_a; + frame_desc.reference_frame_id = + state.getFramebyId(loop_info.keyframe_id_b)->reference_frame_id; + // Initialize pose with known pose a and this loop edge + frame_desc.odom.pose() = + state.getFramebyId(loop_info.keyframe_id_b)->odom.pose() * + loop_info.relative_pose.inverse(); + addFrame(frame_desc); + // printf("[D2PGO@%d]add frame %ld pose %s from drone %d\n", self_id, + // frame_desc.frame_id, + // frame_desc.odom.pose().toStr().c_str(), frame_desc.drone_id); + } + } + updated = true; + is_rot_init_convergence = false; } -void D2PGO::inputDPGOData(const DPGOData & data) { - if (config.mode == PGO_MODE_DISTRIBUTED_AROCK) { - // printf("[D2PGO@%d]input pgo data from drone %d type %d\n", self_id, data.drone_id, data.type); - if (data.type == DPGODataType::DPGO_POSE_DUAL && solver!=nullptr) { - static_cast(solver)->inputDPGOData(data); +void D2PGO::inputDPGOData(const DPGOData& data) { + if (config.mode == PGO_MODE_DISTRIBUTED_AROCK) { + // printf("[D2PGO@%d]input pgo data from drone %d type %d\n", self_id, + // data.drone_id, data.type); + if (data.type == DPGODataType::DPGO_POSE_DUAL && solver != nullptr) { + static_cast(solver)->inputDPGOData(data); + } else { + if (data.type == DPGO_DELTA_POSE_DUAL) { + if (pose6d_init != nullptr) { + pose6d_init->inputDPGOData(data); } else { - if (data.type == DPGO_DELTA_POSE_DUAL) { - if (pose6d_init != nullptr) { - pose6d_init->inputDPGOData(data); - } - else { - // printf("[D2PGO@%d]input pgo data from drone %d\n", self_id, data.drone_id); - if (solver!=nullptr) - static_cast(solver)->inputDPGOData(data); - } - } else if (rot_init!=nullptr) { - // printf("Input to rot_init\n"); - rot_init->inputDPGOData(data); - } + // printf("[D2PGO@%d]input pgo data from drone %d\n", self_id, + // data.drone_id); + if (solver != nullptr) + static_cast(solver)->inputDPGOData(data); } + } else if (rot_init != nullptr) { + // printf("Input to rot_init\n"); + rot_init->inputDPGOData(data); + } } + } } - -void D2PGO::inputDPGOsignal(int drone, const std::string & signal) { - if (signal == "ROT_INIT_FINISH") { - rot_init_finished_robots.insert(drone); - } +void D2PGO::inputDPGOsignal(int drone, const std::string& signal) { + if (signal == "ROT_INIT_FINISH") { + rot_init_finished_robots.insert(drone); + } } void D2PGO::waitForRotInitFinish() { - if (rot_init_finished) { - return; - } - sendSignal("ROT_INIT_FINISH"); - rot_init_finished_robots.insert(self_id); - D2Common::Utility::TicToc timer; - int count = 0; - while (rot_init_finished_robots.size() != available_robots.size() && timer.toc()/1000 < config.rot_init_timeout) { - usleep(1000); - if (count % 100 == 0) { - sendSignal("ROT_INIT_FINISH"); - if (count % 1000 == 0) - printf("[D2PGO@%d]Waiting for rot init finish of other drones, %d/%d\n", self_id, rot_init_finished_robots.size(), available_robots.size()); - } - count++; - } - rot_init_finished = true; - printf("[D2PGO@%d]Rot init finish, %d/%d start perturb PGO\n", self_id, rot_init_finished_robots.size(), available_robots.size()); + if (rot_init_finished) { + return; + } + sendSignal("ROT_INIT_FINISH"); + rot_init_finished_robots.insert(self_id); + D2Common::Utility::TicToc timer; + int count = 0; + while (rot_init_finished_robots.size() != available_robots.size() && + timer.toc() / 1000 < config.rot_init_timeout) { + usleep(1000); + if (count % 100 == 0) { + sendSignal("ROT_INIT_FINISH"); + if (count % 1000 == 0) + printf("[D2PGO@%d]Waiting for rot init finish of other drones, %d/%d\n", + self_id, rot_init_finished_robots.size(), + available_robots.size()); + } + count++; + } + rot_init_finished = true; + printf("[D2PGO@%d]Rot init finish, %d/%d start perturb PGO\n", self_id, + rot_init_finished_robots.size(), available_robots.size()); } bool D2PGO::solve_multi(bool force_solve) { - const Guard lock(state_lock); - if ((state.size(self_id) < config.min_solve_size) && !force_solve) { - // printf("[D2PGO] Not enough frames to solve %d.\n", state.size(self_id)); - return false; - } - if (solver==nullptr) { - solver = new ARockPGO(&state, this, config.arock_config); - } else { - static_cast(solver)->resetResiduals(); - // solver = new ARockPGO(&state, this, config.arock_config); - } - // used_frames.clear(); + const Guard lock(state_lock); + if ((state.size(self_id) < config.min_solve_size) && !force_solve) { + // printf("[D2PGO] Not enough frames to solve %d.\n", state.size(self_id)); + return false; + } + if (solver == nullptr) { + solver = new ARockPGO(&state, this, config.arock_config); + } else { + static_cast(solver)->resetResiduals(); + // solver = new ARockPGO(&state, this, config.arock_config); + } + // used_frames.clear(); + used_loops.clear(); + // Use available loops for outlier rejection. + std::vector available_loops; + for (const Swarm::LoopEdge& loop_info : all_loops) { + if (state.hasFrame(loop_info.keyframe_id_a) && + state.hasFrame(loop_info.keyframe_id_b)) { + available_loops.emplace_back(loop_info); + } + } + if (config.enable_pcm) { + D2Common::Utility::TicToc tic; + auto good_loops = + rejection.OutlierRejectionLoopEdges(ros::Time::now(), available_loops); + printf("[D2PGO] Pcm takes %3.1fms, good %ld/%ld loops\n", tic.toc(), + good_loops.size(), available_loops.size()); + setupLoopFactors(solver, good_loops); + } else { + setupLoopFactors(solver, available_loops); + } + if (config.enable_ego_motion) { + setupEgoMotionFactors(solver); + } + if (config.debug_save_g2o_only) { + saveG2O(true); used_loops.clear(); - //Use available loops for outlier rejection. - std::vector available_loops; - for (const Swarm::LoopEdge & loop_info : all_loops) { - if (state.hasFrame(loop_info.keyframe_id_a) && state.hasFrame(loop_info.keyframe_id_b)) { - available_loops.emplace_back(loop_info); - } - } - if (config.enable_pcm) { - D2Common::Utility::TicToc tic; - auto good_loops = rejection.OutlierRejectionLoopEdges(ros::Time::now(), available_loops); - printf("[D2PGO] Pcm takes %3.1fms, good %ld/%ld loops\n", tic.toc(), good_loops.size(), available_loops.size()); - setupLoopFactors(solver, good_loops); - } else { - setupLoopFactors(solver, available_loops); - } - if (config.enable_ego_motion) { - setupEgoMotionFactors(solver); - } - if (config.debug_save_g2o_only) { - saveG2O(true); - used_loops.clear(); - solver->reset(); - return false; - } - if (config.enable_rotation_initialization && !isRotInitConvergence()) { - rotInitial(used_loops); - if (config.write_g2o) { - saveG2O(); - } - //When use pose6d in rot init, we do not solve ceres. - solve_count ++; - updated = false; - usleep(50000); //In this case we sleep 50ms to let the other drones to initialize rotation - return true; - } - if (config.debug_rot_init_only) { - return true; - } - //Here if enable_rotation_initialization, we need to wait other robots to finish rotation initialization. - if (config.enable_rotation_initialization) { - waitForRotInitFinish(); - } - if (config.enable_gravity_prior) { - setupGravityPriorFactors(solver); - } - SolverReport report; - if (config.rot_init_config.enable_pose6d_solver) { - if (pose6d_init!=nullptr) { - pose6d_init->reset(); - } else { - pose6d_init = new RotInit(&state, config.rot_init_config, config.arock_config, - config.mode==PGO_MODE_DISTRIBUTED_AROCK, [&](const DPGOData & data) { - this->broadcastData(data); - }); - } - pose6d_init->addLoops(used_loops); - if (isMain()) { - pose6d_init->setFixedFrameId(state.headId(self_id)); - } - report = pose6d_init->solve(true); - } else { - report = solver->solve(); - } - if (!config.perturb_mode) { - state.syncFromState(); - } - if (postsolve_callback != nullptr) { - postsolve_callback(); - } + solver->reset(); + return false; + } + if (config.enable_rotation_initialization && !isRotInitConvergence()) { + rotInitial(used_loops); if (config.write_g2o) { - saveG2O(); + saveG2O(); } - // std::cout << report.summary.FullReport() << std::endl; - printf("[D2PGO::solve@%d] solve_count %d mode [multi,%d] total frames %ld loops %d opti_time %.1fms iters %d initial cost %.2e final cost %.2e\n", - self_id, solve_count, config.mode, used_frames.size(), used_loops_count, report.total_time*1000, - report.total_iterations, report.initial_cost, report.final_cost); - solve_count ++; + // When use pose6d in rot init, we do not solve ceres. + solve_count++; updated = false; + usleep(50000); // In this case we sleep 50ms to let the other drones to + // initialize rotation + return true; + } + if (config.debug_rot_init_only) { return true; + } + // Here if enable_rotation_initialization, we need to wait other robots to + // finish rotation initialization. + if (config.enable_rotation_initialization) { + waitForRotInitFinish(); + } + if (config.enable_gravity_prior) { + setupGravityPriorFactors(solver); + } + SolverReport report; + if (config.rot_init_config.enable_pose6d_solver) { + if (pose6d_init != nullptr) { + pose6d_init->reset(); + } else { + pose6d_init = + new RotInit(&state, config.rot_init_config, config.arock_config, + config.mode == PGO_MODE_DISTRIBUTED_AROCK, + [&](const DPGOData& data) { this->broadcastData(data); }); + } + pose6d_init->addLoops(used_loops); + if (isMain()) { + pose6d_init->setFixedFrameId(state.headId(self_id)); + } + report = pose6d_init->solve(true); + } else { + report = solver->solve(); + } + if (!config.perturb_mode) { + state.syncFromState(); + } + if (postsolve_callback != nullptr) { + postsolve_callback(); + } + if (config.write_g2o) { + saveG2O(); + } + // std::cout << report.summary.FullReport() << std::endl; + printf( + "[D2PGO::solve@%d] solve_count %d mode [multi,%d] total frames %ld loops " + "%d opti_time %.1fms iters %d initial cost %.2e final cost %.2e\n", + self_id, solve_count, config.mode, used_frames.size(), used_loops_count, + report.total_time * 1000, report.total_iterations, report.initial_cost, + report.final_cost); + solve_count++; + updated = false; + return true; } bool D2PGO::solve_single() { - const Guard lock(state_lock); - if (state.size(self_id) < config.min_solve_size || !updated) { - // printf("[D2PGO] Not enough frames to solve %d.\n", state.size(self_id)); - return false; - } - used_loops.clear(); + const Guard lock(state_lock); + if (state.size(self_id) < config.min_solve_size || !updated) { + // printf("[D2PGO] Not enough frames to solve %d.\n", state.size(self_id)); + return false; + } + used_loops.clear(); - solver = new CeresSolver(&state, config.ceres_options); - // used_frames.clear(); - //Use available loops for outlier rejection. - std::vector available_loops; - for (const Swarm::LoopEdge & loop_info : all_loops) { - if (state.hasFrame(loop_info.keyframe_id_a) && state.hasFrame(loop_info.keyframe_id_b)) { - available_loops.emplace_back(loop_info); - } - } - if (config.enable_pcm) { - auto good_loops = rejection.OutlierRejectionLoopEdges(ros::Time::now(), available_loops); - setupLoopFactors(solver, good_loops); - } else { - setupLoopFactors(solver, available_loops); - } - if (config.enable_ego_motion) { - setupEgoMotionFactors(solver); - } - - if (config.enable_rotation_initialization && !isRotInitConvergence()) { - rotInitial(used_loops); - if (config.write_g2o) { - saveG2O(); - } - //Simply return here, we do solve ceres in. - delete solver; - solver = nullptr; - return solve_single(); - } + solver = new CeresSolver(&state, config.ceres_options); + // used_frames.clear(); + // Use available loops for outlier rejection. + std::vector available_loops; + for (const Swarm::LoopEdge& loop_info : all_loops) { + if (state.hasFrame(loop_info.keyframe_id_a) && + state.hasFrame(loop_info.keyframe_id_b)) { + available_loops.emplace_back(loop_info); + } + } + if (config.enable_pcm) { + auto good_loops = + rejection.OutlierRejectionLoopEdges(ros::Time::now(), available_loops); + setupLoopFactors(solver, good_loops); + } else { + setupLoopFactors(solver, available_loops); + } + if (config.enable_ego_motion) { + setupEgoMotionFactors(solver); + } - if (config.debug_rot_init_only) { - //When use pose6d in rot init, we do not solve ceres. - solve_count ++; - updated = false; - return true; - } - if (config.enable_gravity_prior) { - setupGravityPriorFactors(solver); - } - setStateProperties(solver->getProblem()); - auto report = solver->solve(); - if (config.perturb_mode) { - postPerturbSolve(); - } else { - state.syncFromState(); - } - if (postsolve_callback != nullptr) { - postsolve_callback(); - } + if (config.enable_rotation_initialization && !isRotInitConvergence()) { + rotInitial(used_loops); if (config.write_g2o) { - saveG2O(); + saveG2O(); } - printf("[D2PGO::solve@%d] solve_count %d mode single,%d total frames %ld loops %d opti_time %.1fms iters %d initial cost %.2e final cost %.2e\n", - self_id, solve_count, config.mode, used_frames.size(), used_loops_count, report.total_time*1000, - report.total_iterations, report.initial_cost, report.final_cost); - solve_count ++; + // Simply return here, we do solve ceres in. + delete solver; + solver = nullptr; + return solve_single(); + } + + if (config.debug_rot_init_only) { + // When use pose6d in rot init, we do not solve ceres. + solve_count++; updated = false; return true; + } + if (config.enable_gravity_prior) { + setupGravityPriorFactors(solver); + } + setStateProperties(solver->getProblem()); + auto report = solver->solve(); + if (config.perturb_mode) { + postPerturbSolve(); + } else { + state.syncFromState(); + } + if (postsolve_callback != nullptr) { + postsolve_callback(); + } + if (config.write_g2o) { + saveG2O(); + } + printf( + "[D2PGO::solve@%d] solve_count %d mode single,%d total frames %ld loops " + "%d opti_time %.1fms iters %d initial cost %.2e final cost %.2e\n", + self_id, solve_count, config.mode, used_frames.size(), used_loops_count, + report.total_time * 1000, report.total_iterations, report.initial_cost, + report.final_cost); + solve_count++; + updated = false; + return true; } bool D2PGO::isRotInitConvergence() const { - return is_rot_init_convergence || !config.enable_rotation_initialization; + return is_rot_init_convergence || !config.enable_rotation_initialization; } -bool D2PGO::isMain() const { - return main_id == self_id; -} +bool D2PGO::isMain() const { return main_id == self_id; } -void D2PGO::rotInitial(const std::vector & good_loops) { - if (rot_init == nullptr) { - rot_init = new RotInit(&state, config.rot_init_config, config.arock_config, - config.mode==PGO_MODE_DISTRIBUTED_AROCK, [&](const DPGOData & data) { - this->broadcastData(data); - }); - } else { - rot_init->reset(); - } - rot_init->addLoops(good_loops); - if (isMain()) { - printf("[D2PGO@%d]rotInitial: set first frame fixed\n", self_id); - rot_init->setFixedFrameId(state.headId(self_id)); - } - SolverReport report = rot_init->solve(); - if (config.mode == PGO_MODE_NON_DIST || (report.state_changes < config.rot_init_state_eps && solve_count > 10)) { - is_rot_init_convergence = true; - printf("[D2PGO@%d]rotInitial: rot init convergence: %.1f%%\n", self_id, report.state_changes*100); - } else { - printf("[D2PGO@%d]rotInitial: rot init not convergence: %.1f%% solve_count %d\n", self_id, report.state_changes*100, solve_count); - } +void D2PGO::rotInitial(const std::vector& good_loops) { + if (rot_init == nullptr) { + rot_init = + new RotInit(&state, config.rot_init_config, config.arock_config, + config.mode == PGO_MODE_DISTRIBUTED_AROCK, + [&](const DPGOData& data) { this->broadcastData(data); }); + } else { + rot_init->reset(); + } + rot_init->addLoops(good_loops); + if (isMain()) { + printf("[D2PGO@%d]rotInitial: set first frame fixed\n", self_id); + rot_init->setFixedFrameId(state.headId(self_id)); + } + SolverReport report = rot_init->solve(); + if (config.mode == PGO_MODE_NON_DIST || + (report.state_changes < config.rot_init_state_eps && solve_count > 10)) { + is_rot_init_convergence = true; + printf("[D2PGO@%d]rotInitial: rot init convergence: %.1f%%\n", self_id, + report.state_changes * 100); + } else { + printf( + "[D2PGO@%d]rotInitial: rot init not convergence: %.1f%% solve_count " + "%d\n", + self_id, report.state_changes * 100, solve_count); + } } void D2PGO::saveG2O(bool only_self) { - std::vector frames; - for (auto frame_id: used_frames) { - if (only_self) { - if (state.getFramebyId(frame_id)->drone_id == self_id) { - frames.emplace_back(state.getFramebyId(frame_id)); - } - } else { - frames.emplace_back(state.getFramebyId(frame_id)); - } - } - printf("[D2PGO::saveG2O@%d] save %ld frames\n", self_id, frames.size()); - std::string path = config.g2o_output_path + "g2o_drone_" + std::to_string(self_id) + "_iter_" + std::to_string(save_count) + ".g2o"; - std::cout << "save g2o to " << path << std::endl; - write_result_to_g2o(path, frames, used_loops, config.g2o_use_raw_data); - path = config.g2o_output_path + "frame_timestamp.txt"; - //output the timestamp of the frames. - std::fstream file; - file.open(path.c_str(), std::fstream::out); - std::cout.precision(9); - for (auto frame: frames) { - file << frame->frame_id <<" "<< std::fixed << frame->stamp << std::endl; - } - file.close(); - save_count ++; -} - -void D2PGO::evalLoop(const Swarm::LoopEdge & loop) { - auto factor = new RelPoseFactor4D(loop.relative_pose, loop.getSqrtInfoMat4D()); - auto kf_a = state.getFramebyId(loop.keyframe_id_a); - auto kf_b = state.getFramebyId(loop.keyframe_id_b); - auto pose_ptr_a = state.getPoseState(loop.keyframe_id_a); - auto pose_ptr_b = state.getPoseState(loop.keyframe_id_b); - VectorXd residuals(4); - auto pose_a = kf_a->odom.pose(); - auto pose_b = kf_b->odom.pose(); - (*factor)(pose_ptr_a, pose_ptr_b, residuals.data()); - printf("Loop %ld->%ld, RelPose %s\n", loop.keyframe_id_a, loop.keyframe_id_b, loop.relative_pose.toStr().c_str()); - printf("RelPose Est %s\n", Swarm::Pose::DeltaPose(pose_a, pose_b).toStr().c_str()); - std::cout << "sqrt_info\n:" << loop.getSqrtInfoMat4D() << std::endl; - printf("PoseA %s PoseB %s residual:", kf_a->odom.pose().toStr().c_str(), kf_b->odom.pose().toStr().c_str()); - std::cout << residuals.transpose() << "\n" << std::endl; + std::vector frames; + for (auto frame_id : used_frames) { + if (only_self) { + if (state.getFramebyId(frame_id)->drone_id == self_id) { + frames.emplace_back(state.getFramebyId(frame_id)); + } + } else { + frames.emplace_back(state.getFramebyId(frame_id)); + } + } + printf("[D2PGO::saveG2O@%d] save %ld frames\n", self_id, frames.size()); + std::string path = config.g2o_output_path + "g2o_drone_" + + std::to_string(self_id) + "_iter_" + + std::to_string(save_count) + ".g2o"; + std::cout << "save g2o to " << path << std::endl; + write_result_to_g2o(path, frames, used_loops, config.g2o_use_raw_data); + path = config.g2o_output_path + "frame_timestamp.txt"; + // output the timestamp of the frames. + std::fstream file; + file.open(path.c_str(), std::fstream::out); + std::cout.precision(9); + for (auto frame : frames) { + file << frame->frame_id << " " << std::fixed << frame->stamp << std::endl; + } + file.close(); + save_count++; } -void D2PGO::setupLoopFactors(SolverWrapper * solver, const std::vector & good_loops) { - used_loops_count = 0; - // auto loss_function = new ceres::HuberLoss(1.0); - auto loss_function = nullptr; - for (auto loop : good_loops) { - if (state.hasFrame(loop.keyframe_id_a) && state.hasFrame(loop.keyframe_id_b)) { - ceres::CostFunction * loop_factor = nullptr; - if (config.pgo_pose_dof == PGO_POSE_4D) { - loop_factor = RelPoseFactor4D::Create(loop); - // this->evalLoop(loop); - } else { - if (config.pgo_use_autodiff) { - if (config.perturb_mode && isRotInitConvergence()) { - auto qa = state.getAttitudeInit(loop.keyframe_id_a); - auto qb = state.getAttitudeInit(loop.keyframe_id_b); - loop_factor = RelPoseFactorPerturbAD::Create(loop, qa, qb); - } else { - loop_factor = RelPoseFactorAD::Create(loop); - } - } else { - loop_factor = RelPoseFactor::Create(loop); - } - } - auto res_info = RelPoseResInfo::create(loop_factor, - loss_function, loop.keyframe_id_a, loop.keyframe_id_b, config.pgo_pose_dof == PGO_POSE_4D, config.perturb_mode); - solver->addResidual(res_info); - used_frames.insert(loop.keyframe_id_a); - used_frames.insert(loop.keyframe_id_b); - auto drone_id_a = state.getFramebyId(loop.keyframe_id_a)->drone_id; - auto ts_a = state.getFramebyId(loop.keyframe_id_a)->stamp; - auto drone_id_b = state.getFramebyId(loop.keyframe_id_b)->drone_id; - auto ts_b = state.getFramebyId(loop.keyframe_id_b)->stamp; - if (used_latest_ts.find(drone_id_a) == used_latest_ts.end() || ts_a > used_latest_ts[drone_id_a]) { - used_latest_frames[drone_id_a] = loop.keyframe_id_a; - used_latest_ts[drone_id_a] = ts_a; - } - if (used_latest_ts.find(drone_id_b) == used_latest_ts.end() || ts_b > used_latest_ts[drone_id_b]) { - used_latest_frames[drone_id_b] = loop.keyframe_id_b; - used_latest_ts[drone_id_b] = ts_b; - } - used_loops_count ++; - used_loops.emplace_back(loop); - // if (loop.id_a != loop.id_b) - // printf("[D2PGO::setupLoopFactors@%d] add loop %ld->%ld pose: %s\n", - // self_id, loop.keyframe_id_a, loop.keyframe_id_b, loop.relative_pose.toStr().c_str()); - } - } +void D2PGO::evalLoop(const Swarm::LoopEdge& loop) { + auto factor = + new RelPoseFactor4D(loop.relative_pose, loop.getSqrtInfoMat4D()); + auto kf_a = state.getFramebyId(loop.keyframe_id_a); + auto kf_b = state.getFramebyId(loop.keyframe_id_b); + auto pose_ptr_a = state.getPoseState(loop.keyframe_id_a); + auto pose_ptr_b = state.getPoseState(loop.keyframe_id_b); + VectorXd residuals(4); + auto pose_a = kf_a->odom.pose(); + auto pose_b = kf_b->odom.pose(); + (*factor)(pose_ptr_a, pose_ptr_b, residuals.data()); + printf("Loop %ld->%ld, RelPose %s\n", loop.keyframe_id_a, loop.keyframe_id_b, + loop.relative_pose.toStr().c_str()); + printf("RelPose Est %s\n", + Swarm::Pose::DeltaPose(pose_a, pose_b).toStr().c_str()); + std::cout << "sqrt_info\n:" << loop.getSqrtInfoMat4D() << std::endl; + printf("PoseA %s PoseB %s residual:", kf_a->odom.pose().toStr().c_str(), + kf_b->odom.pose().toStr().c_str()); + std::cout << residuals.transpose() << "\n" << std::endl; } -void D2PGO::setupEgoMotionFactors(SolverWrapper * solver, int drone_id) { - auto frames = state.getFrames(drone_id); - auto traj = state.getEgomotionTraj(drone_id); - for (int i = 0; i < frames.size() - 1; i ++ ) { - auto frame_a = frames[i]; - auto frame_b = frames[i + 1]; - Swarm::Pose rel_pose; - if (config.pgo_pose_dof == PGO_POSE_4D) { - rel_pose = Swarm::Pose::DeltaPose(frame_a->initial_ego_pose, frame_b->initial_ego_pose, true); +void D2PGO::setupLoopFactors(SolverWrapper* solver, + const std::vector& good_loops) { + used_loops_count = 0; + // auto loss_function = new ceres::HuberLoss(1.0); + auto loss_function = nullptr; + for (auto loop : good_loops) { + if (state.hasFrame(loop.keyframe_id_a) && + state.hasFrame(loop.keyframe_id_b)) { + ceres::CostFunction* loop_factor = nullptr; + if (config.pgo_pose_dof == PGO_POSE_4D) { + loop_factor = RelPoseFactor4D::Create(loop); + // this->evalLoop(loop); + } else { + if (config.pgo_use_autodiff) { + if (config.perturb_mode && isRotInitConvergence()) { + auto qa = state.getAttitudeInit(loop.keyframe_id_a); + auto qb = state.getAttitudeInit(loop.keyframe_id_b); + loop_factor = RelPoseFactorPerturbAD::Create(loop, qa, qb); + } else { + loop_factor = RelPoseFactorAD::Create(loop); + } } else { - rel_pose = Swarm::Pose::DeltaPose(frame_a->initial_ego_pose, frame_b->initial_ego_pose); + loop_factor = RelPoseFactor::Create(loop); } - double len = rel_pose.pos().norm(); - if (len < config.min_cov_len) { - len = config.min_cov_len; - } - Eigen::Matrix6d cov = Eigen::Matrix6d::Zero(); - cov.block<3, 3>(0, 0) = Matrix3d::Identity()*config.pos_covariance_per_meter*len - + 0.5*Matrix3d::Identity()*config.yaw_covariance_per_meter*len*len; - cov.block<3, 3>(3, 3) = Matrix3d::Identity()*config.yaw_covariance_per_meter*len; - Matrix6d sqrt_info = cov.inverse().cwiseAbs().cwiseSqrt(); - Swarm::LoopEdge loop(frame_a->frame_id, frame_b->frame_id, rel_pose, sqrt_info); - if (config.pgo_pose_dof == PGO_POSE_4D) { - auto factor = RelPoseFactor4D::Create(loop); - auto res_info = RelPoseResInfo::create(factor, nullptr, frame_a->frame_id, frame_b->frame_id, true); - solver->addResidual(res_info); - } else if (config.pgo_pose_dof == PGO_POSE_6D) { - ceres::CostFunction * factor; - if (config.pgo_use_autodiff) { - if (config.perturb_mode && isRotInitConvergence()) { - auto qa = state.getAttitudeInit(frame_a->frame_id); - auto qb = state.getAttitudeInit(frame_b->frame_id); - factor = RelPoseFactorPerturbAD::Create(loop, qa, qb); - } else { - factor = RelPoseFactorAD::Create(loop); - } - } else { - factor = RelPoseFactor::Create(loop); - } - auto res_info = RelPoseResInfo::create(factor, nullptr, frame_a->frame_id, frame_b->frame_id, false, config.perturb_mode); - solver->addResidual(res_info); - } - used_frames.insert(frame_a->frame_id); - used_frames.insert(frame_b->frame_id); - used_loops.emplace_back(loop); - if (used_latest_ts.find(frame_b->frame_id) == used_latest_ts.end() || frame_b->stamp > used_latest_ts[frame_b->drone_id]) { - used_latest_frames[frame_b->drone_id] = frame_b->frame_id; - used_latest_ts[frame_b->drone_id] = frame_b->stamp; - } - } + } + auto res_info = RelPoseResInfo::create( + loop_factor, loss_function, loop.keyframe_id_a, loop.keyframe_id_b, + config.pgo_pose_dof == PGO_POSE_4D, config.perturb_mode); + solver->addResidual(res_info); + used_frames.insert(loop.keyframe_id_a); + used_frames.insert(loop.keyframe_id_b); + auto drone_id_a = state.getFramebyId(loop.keyframe_id_a)->drone_id; + auto ts_a = state.getFramebyId(loop.keyframe_id_a)->stamp; + auto drone_id_b = state.getFramebyId(loop.keyframe_id_b)->drone_id; + auto ts_b = state.getFramebyId(loop.keyframe_id_b)->stamp; + if (used_latest_ts.find(drone_id_a) == used_latest_ts.end() || + ts_a > used_latest_ts[drone_id_a]) { + used_latest_frames[drone_id_a] = loop.keyframe_id_a; + used_latest_ts[drone_id_a] = ts_a; + } + if (used_latest_ts.find(drone_id_b) == used_latest_ts.end() || + ts_b > used_latest_ts[drone_id_b]) { + used_latest_frames[drone_id_b] = loop.keyframe_id_b; + used_latest_ts[drone_id_b] = ts_b; + } + used_loops_count++; + used_loops.emplace_back(loop); + // if (loop.id_a != loop.id_b) + // printf("[D2PGO::setupLoopFactors@%d] add loop %ld->%ld pose: %s\n", + // self_id, loop.keyframe_id_a, loop.keyframe_id_b, + // loop.relative_pose.toStr().c_str()); + } + } } -void D2PGO::setupGravityPriorFactors(SolverWrapper * solver) { +void D2PGO::setupEgoMotionFactors(SolverWrapper* solver, int drone_id) { + auto frames = state.getFrames(drone_id); + auto traj = state.getEgomotionTraj(drone_id); + for (int i = 0; i < frames.size() - 1; i++) { + auto frame_a = frames[i]; + auto frame_b = frames[i + 1]; + Swarm::Pose rel_pose; if (config.pgo_pose_dof == PGO_POSE_4D) { - return; - } - printf("[D2PGO::setupGravityPriorFactors] %ld frames\n", used_frames.size()); - - Eigen::Matrix3d gravity_sqrt_info = config.rot_init_config.gravity_sqrt_info*Matrix3d::Identity(); - for (auto & frame: used_frames) { - auto frame_ptr = state.getFramebyId(frame); - auto ego_motion = frame_ptr->initial_ego_pose; - ceres::CostFunction * factor; + rel_pose = Swarm::Pose::DeltaPose(frame_a->initial_ego_pose, + frame_b->initial_ego_pose, true); + } else { + rel_pose = Swarm::Pose::DeltaPose(frame_a->initial_ego_pose, + frame_b->initial_ego_pose); + } + double len = rel_pose.pos().norm(); + if (len < config.min_cov_len) { + len = config.min_cov_len; + } + Eigen::Matrix6d cov = Eigen::Matrix6d::Zero(); + cov.block<3, 3>(0, 0) = + Matrix3d::Identity() * config.pos_covariance_per_meter * len + + 0.5 * Matrix3d::Identity() * config.yaw_covariance_per_meter * len * + len; + cov.block<3, 3>(3, 3) = + Matrix3d::Identity() * config.yaw_covariance_per_meter * len; + Matrix6d sqrt_info = cov.inverse().cwiseAbs().cwiseSqrt(); + Swarm::LoopEdge loop(frame_a->frame_id, frame_b->frame_id, rel_pose, + sqrt_info); + if (config.pgo_pose_dof == PGO_POSE_4D) { + auto factor = RelPoseFactor4D::Create(loop); + auto res_info = RelPoseResInfo::create(factor, nullptr, frame_a->frame_id, + frame_b->frame_id, true); + solver->addResidual(res_info); + } else if (config.pgo_pose_dof == PGO_POSE_6D) { + ceres::CostFunction* factor; + if (config.pgo_use_autodiff) { if (config.perturb_mode && isRotInitConvergence()) { - auto qa = state.getAttitudeInit(frame); - factor = GravityPriorPerturbAD::Create(ego_motion, gravity_sqrt_info, qa); + auto qa = state.getAttitudeInit(frame_a->frame_id); + auto qb = state.getAttitudeInit(frame_b->frame_id); + factor = RelPoseFactorPerturbAD::Create(loop, qa, qb); } else { - //Not implemented - ROS_ERROR("[D2PGO::setupGravityPriorFactors] non perturb_mode not implemented"); + factor = RelPoseFactorAD::Create(loop); } - auto res_info = GravityPriorResInfo::create(factor, nullptr, frame, config.perturb_mode); - solver->addResidual(res_info); - if (used_latest_ts.find(frame) == used_latest_ts.end() || frame_ptr->stamp > used_latest_ts[frame_ptr->drone_id]) { - used_latest_frames[frame_ptr->drone_id] = frame; - used_latest_ts[frame_ptr->drone_id] = frame_ptr->stamp; - } - } + } else { + factor = RelPoseFactor::Create(loop); + } + auto res_info = + RelPoseResInfo::create(factor, nullptr, frame_a->frame_id, + frame_b->frame_id, false, config.perturb_mode); + solver->addResidual(res_info); + } + used_frames.insert(frame_a->frame_id); + used_frames.insert(frame_b->frame_id); + used_loops.emplace_back(loop); + if (used_latest_ts.find(frame_b->frame_id) == used_latest_ts.end() || + frame_b->stamp > used_latest_ts[frame_b->drone_id]) { + used_latest_frames[frame_b->drone_id] = frame_b->frame_id; + used_latest_ts[frame_b->drone_id] = frame_b->stamp; + } + } } -void D2PGO::setupEgoMotionFactors(SolverWrapper * solver) { - if (config.mode == PGO_MODE_NON_DIST) { - for (auto drone_id : state.availableDrones()) { - setupEgoMotionFactors(solver, drone_id); - } - } else if (config.mode >= PGO_MODE_DISTRIBUTED_AROCK) { - setupEgoMotionFactors(solver, self_id); - } -} +void D2PGO::setupGravityPriorFactors(SolverWrapper* solver) { + if (config.pgo_pose_dof == PGO_POSE_4D) { + return; + } + printf("[D2PGO::setupGravityPriorFactors] %ld frames\n", used_frames.size()); -void D2PGO::setStateProperties(ceres::Problem & problem) { - ceres::Manifold* manifold; - ceres::LocalParameterization * local_parameterization; - if (config.pgo_pose_dof == PGO_POSE_4D) { - manifold = PosAngleManifold::Create(); + Eigen::Matrix3d gravity_sqrt_info = + config.rot_init_config.gravity_sqrt_info * Matrix3d::Identity(); + for (auto& frame : used_frames) { + auto frame_ptr = state.getFramebyId(frame); + auto ego_motion = frame_ptr->initial_ego_pose; + ceres::CostFunction* factor; + if (config.perturb_mode && isRotInitConvergence()) { + auto qa = state.getAttitudeInit(frame); + factor = GravityPriorPerturbAD::Create(ego_motion, gravity_sqrt_info, qa); } else { - if (!config.perturb_mode) { - if (config.pgo_use_autodiff) { - ceres::EigenQuaternionManifold quat_manifold; - ceres::EuclideanManifold<3> euc_manifold; - manifold = new ceres::ProductManifold, ceres::EigenQuaternionManifold>(euc_manifold, quat_manifold); - } else { - local_parameterization = new PoseLocalParameterization; - } - } + // Not implemented + ROS_ERROR( + "[D2PGO::setupGravityPriorFactors] non perturb_mode not implemented"); + } + auto res_info = GravityPriorResInfo::create(factor, nullptr, frame, + config.perturb_mode); + solver->addResidual(res_info); + if (used_latest_ts.find(frame) == used_latest_ts.end() || + frame_ptr->stamp > used_latest_ts[frame_ptr->drone_id]) { + used_latest_frames[frame_ptr->drone_id] = frame; + used_latest_ts[frame_ptr->drone_id] = frame_ptr->stamp; + } + } +} + +void D2PGO::setupEgoMotionFactors(SolverWrapper* solver) { + if (config.mode == PGO_MODE_NON_DIST) { + for (auto drone_id : state.availableDrones()) { + setupEgoMotionFactors(solver, drone_id); } + } else if (config.mode >= PGO_MODE_DISTRIBUTED_AROCK) { + setupEgoMotionFactors(solver, self_id); + } +} + +void D2PGO::setStateProperties(ceres::Problem& problem) { + ceres::Manifold* manifold; + ceres::LocalParameterization* local_parameterization; + if (config.pgo_pose_dof == PGO_POSE_4D) { + manifold = PosAngleManifold::Create(); + } else { if (!config.perturb_mode) { - for (auto frame_id : used_frames) { - auto pointer = state.getPoseState(frame_id); - if (!problem.HasParameterBlock(pointer)) { - continue; - } - if (config.pgo_pose_dof == PGO_POSE_4D || config.pgo_use_autodiff) { - problem.SetManifold(pointer, manifold); - } else { - problem.SetParameterization(pointer, local_parameterization); - } - } - } - if (config.mode == PGO_MODE_NON_DIST || - config.mode >= PGO_MODE_DISTRIBUTED_AROCK && self_id == main_id) { - auto frame_id = state.headId(self_id); - if (config.perturb_mode) { - auto pointer = state.getPerturbState(frame_id); - problem.SetParameterBlockConstant(pointer); - // printf("[D2PGO::setStateProperties@%d] set perturb state %ld to constant\n", self_id, frame_id); - } else { - auto pointer = state.getPoseState(frame_id); - problem.SetParameterBlockConstant(pointer); - } + if (config.pgo_use_autodiff) { + ceres::EigenQuaternionManifold quat_manifold; + ceres::EuclideanManifold<3> euc_manifold; + manifold = new ceres::ProductManifold, + ceres::EigenQuaternionManifold>( + euc_manifold, quat_manifold); + } else { + local_parameterization = new PoseLocalParameterization; + } + } + } + if (!config.perturb_mode) { + for (auto frame_id : used_frames) { + auto pointer = state.getPoseState(frame_id); + if (!problem.HasParameterBlock(pointer)) { + continue; + } + if (config.pgo_pose_dof == PGO_POSE_4D || config.pgo_use_autodiff) { + problem.SetManifold(pointer, manifold); + } else { + problem.SetParameterization(pointer, local_parameterization); + } + } + } + if (config.mode == PGO_MODE_NON_DIST || + config.mode >= PGO_MODE_DISTRIBUTED_AROCK && self_id == main_id) { + auto frame_id = state.headId(self_id); + if (config.perturb_mode) { + auto pointer = state.getPerturbState(frame_id); + problem.SetParameterBlockConstant(pointer); + // printf("[D2PGO::setStateProperties@%d] set perturb state %ld to + // constant\n", self_id, frame_id); + } else { + auto pointer = state.getPoseState(frame_id); + problem.SetParameterBlockConstant(pointer); } + } } void D2PGO::postPerturbSolve() { - for (auto frame_id : used_frames) { - auto pointer = state.getPerturbState(frame_id); - Map pos(pointer); - Map perturb_theta(pointer+3); - Quaterniond q_perturb = Utility::quatfromRotationVector(perturb_theta); - Swarm::Pose optimized_pose(pos, state.getAttitudeInit(frame_id)*q_perturb); - state.getFramebyId(frame_id)->odom.pose() = optimized_pose; - // perturb_theta.setZero(); - } + for (auto frame_id : used_frames) { + auto pointer = state.getPerturbState(frame_id); + Map pos(pointer); + Map perturb_theta(pointer + 3); + Quaterniond q_perturb = Utility::quatfromRotationVector(perturb_theta); + Swarm::Pose optimized_pose(pos, + state.getAttitudeInit(frame_id) * q_perturb); + state.getFramebyId(frame_id)->odom.pose() = optimized_pose; + // perturb_theta.setZero(); + } } std::map D2PGO::getOptimizedTrajs() { - const Guard lock(state_lock); - std::map trajs; - for (auto drone_id : state.availableDrones()) { - trajs[drone_id] = Swarm::DroneTrajectory(drone_id, false); - for (auto frame : state.getFrames(drone_id)) { - if (used_frames.find(frame->frame_id) == used_frames.end()) { - continue; - } - auto pose = frame->odom.pose(); - if (config.pgo_pose_dof == PGO_POSE_4D) { - //Then we need to combine the roll pitch from ego motion - Swarm::Pose ego_pose = frame->initial_ego_pose; - auto delta_att = ego_pose.att_yaw_only().inverse() * ego_pose.att(); - pose.att() = pose.att()*delta_att; - } - if (config.perturb_mode) { - auto pointer = state.getPerturbState(frame->frame_id); - Map pos(pointer); - Map perturb_theta(pointer+3); - Quaterniond q_perturb = Utility::quatfromRotationVector(perturb_theta); - pose = Swarm::Pose(pos, state.getAttitudeInit(frame->frame_id)*q_perturb); - } - trajs[drone_id].push(frame->stamp, pose, frame->frame_id); - } + const Guard lock(state_lock); + std::map trajs; + for (auto drone_id : state.availableDrones()) { + trajs[drone_id] = Swarm::DroneTrajectory(drone_id, false); + for (auto frame : state.getFrames(drone_id)) { + if (used_frames.count(frame->frame_id) == 0) { + continue; + } + auto pose = frame->odom.pose(); + if (config.pgo_pose_dof == PGO_POSE_4D) { + // Then we need to combine the roll pitch from ego motion + Swarm::Pose ego_pose = frame->initial_ego_pose; + auto delta_att = ego_pose.att_yaw_only().inverse() * ego_pose.att(); + pose.att() = pose.att() * delta_att; + } + if (config.perturb_mode) { + auto pointer = state.getPerturbState(frame->frame_id); + Map pos(pointer); + Map perturb_theta(pointer + 3); + Quaterniond q_perturb = Utility::quatfromRotationVector(perturb_theta); + pose = Swarm::Pose(pos, + state.getAttitudeInit(frame->frame_id) * q_perturb); + } + trajs[drone_id].push(frame->stamp, pose, frame->frame_id); + } + if (trajs[drone_id].trajectory_size() == 0) { + trajs.erase(drone_id); } - return trajs; + } + return trajs; } std::map D2PGO::getPredictedOdoms() const { - const Guard lock(state_lock); - std::map ret; - for (auto drone_id : state.availableDrones()) { - if (!state.hasEgomotionTraj(drone_id)) { - printf("[D2PGO::getPredictedOdoms] no egomotion traj for drone %d\n", drone_id); - continue; - } - const Swarm::DroneTrajectory & ego_motion_traj = state.getEgomotionTraj(drone_id); - if (ego_motion_traj.trajectory_size() > 0 && used_latest_frames.find(drone_id) != used_latest_frames.end()) { - auto latest_ego_pose = ego_motion_traj.get_latest_pose(); - auto latest_ts = ego_motion_traj.get_latest_stamp(); - auto last_est_frame_id = used_latest_frames.at(drone_id); - auto last_est_frame = state.getFramebyId(last_est_frame_id); - auto last_est_ego = last_est_frame->initial_ego_pose; - auto predicted_pose = last_est_frame->odom.pose() * last_est_ego.inverse() * latest_ego_pose; - ret[drone_id] = Swarm::Odometry(latest_ts, predicted_pose); - } - } - return ret; + const Guard lock(state_lock); + std::map ret; + for (auto drone_id : state.availableDrones()) { + if (!state.hasEgomotionTraj(drone_id)) { + printf("[D2PGO::getPredictedOdoms] no egomotion traj for drone %d\n", + drone_id); + continue; + } + const Swarm::DroneTrajectory& ego_motion_traj = + state.getEgomotionTraj(drone_id); + if (ego_motion_traj.trajectory_size() > 0 && + used_latest_frames.find(drone_id) != used_latest_frames.end()) { + auto latest_ego_pose = ego_motion_traj.get_latest_pose(); + auto latest_ts = ego_motion_traj.get_latest_stamp(); + auto last_est_frame_id = used_latest_frames.at(drone_id); + auto last_est_frame = state.getFramebyId(last_est_frame_id); + auto last_est_ego = last_est_frame->initial_ego_pose; + auto predicted_pose = last_est_frame->odom.pose() * + last_est_ego.inverse() * latest_ego_pose; + ret[drone_id] = Swarm::Odometry(latest_ts, predicted_pose); + } + } + return ret; } -void D2PGO::broadcastData(const DPGOData & data) { - bd_data_callback(data); -} +void D2PGO::broadcastData(const DPGOData& data) { bd_data_callback(data); } -void D2PGO::sendSignal(const std::string & signal) { - bd_signal_callback(signal); +void D2PGO::sendSignal(const std::string& signal) { + bd_signal_callback(signal); } std::vector D2PGO::getAllLocalFrames() { - const Guard lock(state_lock); - return state.getFrames(self_id); + const Guard lock(state_lock); + return state.getFrames(self_id); } -} \ No newline at end of file +} // namespace D2PGO \ No newline at end of file diff --git a/d2pgo/src/d2pgo_node.cpp b/d2pgo/src/d2pgo_node.cpp index 7f8d4bd6..b5b144b7 100644 --- a/d2pgo/src/d2pgo_node.cpp +++ b/d2pgo/src/d2pgo_node.cpp @@ -1,224 +1,247 @@ #include + #include "d2pgo.h" +#include "geometry_msgs/PoseStamped.h" #include "swarm_msgs/ImageArrayDescriptor.h" #include "swarm_msgs/swarm_fused.h" -#include "geometry_msgs/PoseStamped.h" #define BACKWARD_HAS_DW 1 #include -namespace backward -{ - backward::SignalHandling sh; +namespace backward { +backward::SignalHandling sh; } namespace D2PGO { class D2PGONode { - D2PGO * pgo = nullptr; - ros::Subscriber frame_sub, remote_frame_sub, loop_sub, dpgo_data_sub; - ros::Timer solver_timer; - double solver_timer_freq = 10; - D2PGOConfig config; - std::map path_pubs; - std::map odom_pubs; - ros::Publisher path_pub; - ros::Publisher dpgo_data_pub; - ros::Publisher drone_traj_pub, swarm_fused_pub; - bool write_to_file = true; - bool multi = false; - int write_to_file_step = 5; - int pub_count = 0; - std::string output_folder; - ros::NodeHandle * _nh; -protected: - void processImageArray(const swarm_msgs::VIOFrame & vioframe) { - if (vioframe.is_keyframe) { - D2BaseFrame frame(vioframe); - pgo->addFrame(frame); - } - } - - void processLoop(const swarm_msgs::LoopEdge & loop_info) { - // ROS_INFO("[D2PGONode@%d] processLoop from %ld to %ld", config.self_id, loop_info.keyframe_id_a, loop_info.keyframe_id_b); - pgo->addLoop(Swarm::LoopEdge(loop_info)); - } - - void processDPGOData(const swarm_msgs::DPGOData & data) { - if (data.drone_id != config.self_id) { - // ROS_INFO("[D2PGONode@%d] processDPGOData from drone %d", config.self_id, data.drone_id); - pgo->inputDPGOData(DPGOData(data)); - } + D2PGO *pgo = nullptr; + ros::Subscriber frame_sub, remote_frame_sub, loop_sub, dpgo_data_sub; + ros::Timer solver_timer; + double solver_timer_freq = 10; + D2PGOConfig config; + std::map path_pubs; + std::map odom_pubs; + ros::Publisher path_pub; + ros::Publisher dpgo_data_pub; + ros::Publisher drone_traj_pub, swarm_fused_pub; + bool write_to_file = true; + bool multi = false; + int write_to_file_step = 5; + int pub_count = 0; + std::string output_folder; + ros::NodeHandle *_nh; + + protected: + void processImageArray(const swarm_msgs::VIOFrame &vioframe) { + if (vioframe.is_keyframe) { + D2BaseFrame frame(vioframe); + pgo->addFrame(frame); } + } - void pubTrajs(std::map & trajs) { - for (auto it : trajs) { - auto drone_id = it.first; - auto traj = it.second; - if (path_pubs.find(drone_id) == path_pubs.end()) { - path_pubs[drone_id] = _nh->advertise("pgo_path_" + std::to_string(drone_id), 1000); - } - if (drone_id == config.self_id) { - path_pub.publish(traj.get_ros_path()); - } - drone_traj_pub.publish(traj.toRos()); - path_pubs[drone_id].publish(traj.get_ros_path()); - if (write_to_file && pub_count % write_to_file_step == 0) { - std::ofstream csv(output_folder + "/pgo_" + std::to_string(drone_id) + ".csv", std::ios::out); - for (size_t i = 0; i < traj.trajectory_size(); i++) { - double stamp = traj.stamp_by_index(i); - auto pose = traj.pose_by_index(i); - csv << std::setprecision(std::numeric_limits::digits10 + 1) << stamp << " " << pose.toStr(true) << std::endl; - } - csv.close(); - } - - } - // printf("[D2PGONode@%d] pubTrajs, %ld trajs\n", config.self_id, trajs.size()); - pub_count++; + void processLoop(const swarm_msgs::LoopEdge &loop_info) { + // ROS_INFO("[D2PGONode@%d] processLoop from %ld to %ld", config.self_id, + // loop_info.keyframe_id_a, loop_info.keyframe_id_b); + pgo->addLoop(Swarm::LoopEdge(loop_info)); + } + + void processDPGOData(const swarm_msgs::DPGOData &data) { + if (data.drone_id != config.self_id) { + // ROS_INFO("[D2PGONode@%d] processDPGOData from drone %d", + // config.self_id, data.drone_id); + pgo->inputDPGOData(DPGOData(data)); } + } - void pubLatestPredictResults() { - swarm_msgs::swarm_fused swarm_fused; - swarm_fused.header.stamp = ros::Time::now(); - swarm_fused.self_id = config.self_id; - swarm_fused.reference_frame_id = pgo->getReferenceFrameId(); - auto latest_odoms = pgo->getPredictedOdoms(); - // printf("[D2PGONode@%d] pubLatestPredictResults, %ld odoms\n", config.self_id, latest_odoms.size()); - for (auto it : latest_odoms) { - auto drone_id = it.first; - auto pose = it.second.pose(); - auto pose_ros = pose.toROS(); - swarm_fused.ids.emplace_back(drone_id); - swarm_fused.local_drone_position.emplace_back(pose_ros.position); - swarm_fused.local_drone_rotation.emplace_back(pose_ros.orientation); - swarm_fused.local_drone_yaw.emplace_back(pose.yaw()); - if (drone_id == config.self_id) { - swarm_fused.self_pos = pose_ros.position; - swarm_fused.self_yaw = pose.yaw(); - } - if (odom_pubs.find(drone_id) == odom_pubs.end()) { - odom_pubs[drone_id] = _nh->advertise("pose_" + std::to_string(drone_id), 1000); - } - geometry_msgs::PoseStamped pose_stamped; - pose_stamped.header.stamp = ros::Time(it.second.stamp); - pose_stamped.header.frame_id = "world"; - pose_stamped.pose = pose_ros; - odom_pubs[drone_id].publish(pose_stamped); + void pubTrajs(std::map &trajs) { + for (auto it : trajs) { + auto drone_id = it.first; + auto traj = it.second; + if (path_pubs.find(drone_id) == path_pubs.end()) { + path_pubs[drone_id] = _nh->advertise( + "pgo_path_" + std::to_string(drone_id), 1000); + } + if (drone_id == config.self_id) { + path_pub.publish(traj.get_ros_path()); + } + drone_traj_pub.publish(traj.toRos()); + path_pubs[drone_id].publish(traj.get_ros_path()); + if (write_to_file && pub_count % write_to_file_step == 0) { + std::ofstream csv( + output_folder + "/pgo_" + std::to_string(drone_id) + ".csv", + std::ios::out); + for (size_t i = 0; i < traj.trajectory_size(); i++) { + double stamp = traj.stamp_by_index(i); + auto pose = traj.pose_by_index(i); + csv << std::setprecision(std::numeric_limits::digits10 + + 1) + << stamp << " " << pose.toStr(true) << std::endl; } - swarm_fused_pub.publish(swarm_fused); + csv.close(); + } } + // printf("[D2PGONode@%d] pubTrajs, %ld trajs\n", config.self_id, + // trajs.size()); + pub_count++; + } - void solverTimerCallback(const ros::TimerEvent & event) { - bool succ; - if (multi) { - // printf("[D2PGO] try to solve multi......\n"); - succ = pgo->solve_multi(); - } - else { - succ = pgo->solve_single(); - } - if (succ) { - auto trajs = pgo->getOptimizedTrajs(); - pubTrajs(trajs); - } - pubLatestPredictResults(); + void pubLatestPredictResults() { + swarm_msgs::swarm_fused swarm_fused; + swarm_fused.header.stamp = ros::Time::now(); + swarm_fused.self_id = config.self_id; + swarm_fused.reference_frame_id = pgo->getReferenceFrameId(); + auto latest_odoms = pgo->getPredictedOdoms(); + // printf("[D2PGONode@%d] pubLatestPredictResults, %ld odoms\n", + // config.self_id, latest_odoms.size()); + for (auto it : latest_odoms) { + auto drone_id = it.first; + auto pose = it.second.pose(); + auto pose_ros = pose.toROS(); + swarm_fused.ids.emplace_back(drone_id); + swarm_fused.local_drone_position.emplace_back(pose_ros.position); + swarm_fused.local_drone_rotation.emplace_back(pose_ros.orientation); + swarm_fused.local_drone_yaw.emplace_back(pose.yaw()); + if (drone_id == config.self_id) { + swarm_fused.self_pos = pose_ros.position; + swarm_fused.self_yaw = pose.yaw(); + } + if (odom_pubs.find(drone_id) == odom_pubs.end()) { + odom_pubs[drone_id] = _nh->advertise( + "pose_" + std::to_string(drone_id), 1000); + } + geometry_msgs::PoseStamped pose_stamped; + pose_stamped.header.stamp = ros::Time(it.second.stamp); + pose_stamped.header.frame_id = "world"; + pose_stamped.pose = pose_ros; + odom_pubs[drone_id].publish(pose_stamped); } + swarm_fused_pub.publish(swarm_fused); + } - void Init(ros::NodeHandle & nh) { - InitParams(nh); - _nh = &nh; - pgo = new D2PGO(config); - path_pub = _nh->advertise("pgo_path", 1000); - drone_traj_pub = _nh->advertise("pgo_traj", 1000); - dpgo_data_pub = _nh->advertise("pgo_data", 1000); - swarm_fused_pub = _nh->advertise("swarm_fused", 1000); - pgo->bd_data_callback = [&] (const DPGOData & data) { - dpgo_data_pub.publish(data.toROS()); - }; - dpgo_data_sub = nh.subscribe("pgo_data", 1000, &D2PGONode::processDPGOData, this, ros::TransportHints().tcpNoDelay()); - frame_sub = nh.subscribe("frame_local", 1000, &D2PGONode::processImageArray, this, ros::TransportHints().tcpNoDelay()); - remote_frame_sub = nh.subscribe("frame_remote", 1000, &D2PGONode::processImageArray, this, ros::TransportHints().tcpNoDelay()); - loop_sub = nh.subscribe("loop", 1000, &D2PGONode::processLoop, this, ros::TransportHints().tcpNoDelay()); - solver_timer = nh.createTimer(ros::Duration(1.0/solver_timer_freq), &D2PGONode::solverTimerCallback, this); - printf("[D2PGONode@%d] Initialized\n", config.self_id); + void solverTimerCallback(const ros::TimerEvent &event) { + bool succ; + if (multi) { + // printf("[D2PGO] try to solve multi......\n"); + succ = pgo->solve_multi(); + } else { + succ = pgo->solve_single(); } + if (succ) { + auto trajs = pgo->getOptimizedTrajs(); + pubTrajs(trajs); + } + pubLatestPredictResults(); + } - void InitParams(ros::NodeHandle & nh) { - std::string vins_config_path; - nh.param("vins_config_path", vins_config_path, ""); - cv::FileStorage fsSettings; - try { - fsSettings.open(vins_config_path.c_str(), cv::FileStorage::READ); - std::cout << "PGO Loaded VINS config from " << vins_config_path << std::endl; - } catch(cv::Exception ex) { - std::cerr << "ERROR:" << ex.what() << " Can't open config file" << std::endl; - exit(-1); - } - fsSettings["output_path"] >> output_folder; - config.write_g2o = (int) fsSettings["write_g2o"]; - fsSettings["g2o_output_path"] >> config.g2o_output_path; - write_to_file = (int) fsSettings["write_pgo_to_file"]; - config.g2o_output_path = output_folder + "/";// + config.g2o_output_path; - config.mode = static_cast((int) fsSettings["pgo_mode"]); - nh.param("self_id", config.self_id, -1); - bool is_4dof; - nh.param("is_4dof", is_4dof, true); - if (is_4dof) { - config.pgo_pose_dof = PGO_POSE_4D; - } else { - config.pgo_pose_dof = PGO_POSE_6D; - } - config.pcm_rej.is_4dof = is_4dof; - - //Config ceres - config.ceres_options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;// ceres::DENSE_SCHUR; - config.ceres_options.num_threads = 1; - config.ceres_options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT;// ceres::DOGLEG; - config.ceres_options.max_solver_time_in_seconds = fsSettings["pgo_solver_time"]; - config.main_id = 1; - - //Config arock - config.arock_config.self_id = config.self_id; - config.arock_config.ceres_options = config.ceres_options; - config.arock_config.rho_frame_T = fsSettings["pgo_rho_frame_T"]; - config.arock_config.rho_frame_theta = fsSettings["pgo_rho_frame_theta"]; - config.arock_config.eta_k = fsSettings["pgo_eta_k"]; - config.arock_config.max_steps = 1; - - //Outlier rejection - config.is_realtime = true; - config.enable_pcm = (int)fsSettings["enable_pcm"]; - config.pcm_rej.pcm_thres = fsSettings["pcm_thres"]; - config.enable_rotation_initialization = false; - config.enable_gravity_prior = (int)fsSettings["enable_gravity_prior"]; - config.rot_init_config.gravity_sqrt_info = fsSettings["gravity_sqrt_info"]; - solver_timer_freq = (double) fsSettings["solver_timer_freq"]; - config.perturb_mode = true; - //Debugging - config.debug_save_g2o_only = (int) fsSettings["debug_save_g2o_only"]; - if (config.mode == PGO_MODE::PGO_MODE_NON_DIST) { - multi = false; - printf("[D2PGO] In single mode enable_pcm %d pcm_thres %.1f\n", config.enable_pcm, config.pcm_rej.pcm_thres); - } else { - multi = true; - printf("[D2PGO] In multi mode enable_pcm %d pcm_thres %.1f\n", config.enable_pcm, config.pcm_rej.pcm_thres); - } + void Init(ros::NodeHandle &nh) { + InitParams(nh); + _nh = &nh; + pgo = new D2PGO(config); + path_pub = _nh->advertise("pgo_path", 1000); + drone_traj_pub = _nh->advertise("pgo_traj", 1000); + dpgo_data_pub = _nh->advertise("pgo_data", 1000); + swarm_fused_pub = + _nh->advertise("swarm_fused", 1000); + pgo->bd_data_callback = [&](const DPGOData &data) { + dpgo_data_pub.publish(data.toROS()); + }; + dpgo_data_sub = nh.subscribe("pgo_data", 1000, &D2PGONode::processDPGOData, + this, ros::TransportHints().tcpNoDelay()); + frame_sub = nh.subscribe("frame_local", 1000, &D2PGONode::processImageArray, + this, ros::TransportHints().tcpNoDelay()); + remote_frame_sub = + nh.subscribe("frame_remote", 1000, &D2PGONode::processImageArray, this, + ros::TransportHints().tcpNoDelay()); + loop_sub = nh.subscribe("loop", 1000, &D2PGONode::processLoop, this, + ros::TransportHints().tcpNoDelay()); + solver_timer = nh.createTimer(ros::Duration(1.0 / solver_timer_freq), + &D2PGONode::solverTimerCallback, this); + printf("[D2PGONode@%d] Initialized\n", config.self_id); + } + + void InitParams(ros::NodeHandle &nh) { + std::string vins_config_path; + nh.param("vins_config_path", vins_config_path, ""); + cv::FileStorage fsSettings; + try { + fsSettings.open(vins_config_path.c_str(), cv::FileStorage::READ); + std::cout << "PGO Loaded VINS config from " << vins_config_path + << std::endl; + } catch (cv::Exception ex) { + std::cerr << "ERROR:" << ex.what() << " Can't open config file" + << std::endl; + exit(-1); + } + fsSettings["output_path"] >> output_folder; + config.write_g2o = (int)fsSettings["write_g2o"]; + fsSettings["g2o_output_path"] >> config.g2o_output_path; + write_to_file = (int)fsSettings["write_pgo_to_file"]; + config.g2o_output_path = output_folder + "/"; // + config.g2o_output_path; + config.mode = static_cast((int)fsSettings["pgo_mode"]); + nh.param("self_id", config.self_id, -1); + bool is_4dof; + nh.param("is_4dof", is_4dof, true); + if (is_4dof) { + config.pgo_pose_dof = PGO_POSE_4D; + } else { + config.pgo_pose_dof = PGO_POSE_6D; } -public: - D2PGONode(ros::NodeHandle & nh) { - Init(nh); + config.pcm_rej.is_4dof = is_4dof; + + // Config ceres + config.ceres_options.linear_solver_type = + ceres::SPARSE_NORMAL_CHOLESKY; // ceres::DENSE_SCHUR; + config.ceres_options.num_threads = 1; + config.ceres_options.trust_region_strategy_type = + ceres::LEVENBERG_MARQUARDT; // ceres::DOGLEG; + config.ceres_options.max_solver_time_in_seconds = + fsSettings["pgo_solver_time"]; + config.main_id = 1; + + // Config arock + config.arock_config.self_id = config.self_id; + config.arock_config.ceres_options = config.ceres_options; + config.arock_config.rho_frame_T = fsSettings["pgo_rho_frame_T"]; + config.arock_config.rho_frame_theta = fsSettings["pgo_rho_frame_theta"]; + config.arock_config.eta_k = fsSettings["pgo_eta_k"]; + config.arock_config.max_steps = 1; + + // Outlier rejection + config.is_realtime = true; + config.enable_pcm = (int)fsSettings["enable_pcm"]; + config.pcm_rej.pcm_thres = fsSettings["pcm_thres"]; + config.enable_rotation_initialization = false; + config.enable_gravity_prior = (int)fsSettings["enable_gravity_prior"]; + config.rot_init_config.gravity_sqrt_info = fsSettings["gravity_sqrt_info"]; + solver_timer_freq = (double)fsSettings["solver_timer_freq"]; + config.perturb_mode = true; + // Debugging + config.debug_save_g2o_only = (int)fsSettings["debug_save_g2o_only"]; + config.loop_distance_threshold = (int)fsSettings["accept_loop_max_pos"]; + if (config.mode == PGO_MODE::PGO_MODE_NON_DIST) { + multi = false; + printf("[D2PGO] In single mode enable_pcm %d pcm_thres %.1f\n", + config.enable_pcm, config.pcm_rej.pcm_thres); + } else { + multi = true; + printf("[D2PGO] In multi mode enable_pcm %d pcm_thres %.1f\n", + config.enable_pcm, config.pcm_rej.pcm_thres); } + } + + public: + D2PGONode(ros::NodeHandle &nh) { Init(nh); } }; -} +} // namespace D2PGO + +int main(int argc, char **argv) { + cv::setNumThreads(1); + ros::init(argc, argv, "d2pgo"); + ros::NodeHandle n("~"); + ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, + ros::console::levels::Info); -int main(int argc, char **argv) -{ - cv::setNumThreads(1); - ros::init(argc, argv, "d2pgo"); - ros::NodeHandle n("~"); - ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info); - - D2PGO::D2PGONode d2pgonode(n); - ros::MultiThreadedSpinner spinner(4); - spinner.spin(); - return 0; + D2PGO::D2PGONode d2pgonode(n); + ros::MultiThreadedSpinner spinner(4); + spinner.spin(); + return 0; } \ No newline at end of file diff --git a/d2pgo/src/pgostate.hpp b/d2pgo/src/pgostate.hpp index 37706816..7abdb279 100644 --- a/d2pgo/src/pgostate.hpp +++ b/d2pgo/src/pgostate.hpp @@ -109,5 +109,13 @@ class PGOState : public D2State { Eigen::Quaterniond getAttitudeInit(FrameIdType frame_id) { return initial_attitude.at(frame_id); } + + std::vector getAllDroneIds() { + std::vector drone_ids; + for (auto it : drone_frames) { + drone_ids.push_back(it.first); + } + return drone_ids; + } }; } \ No newline at end of file diff --git a/d2pgo/src/rot_init/rotation_initialization.cpp b/d2pgo/src/rot_init/rotation_initialization.cpp index 9fc7800c..9c3d7dc1 100644 --- a/d2pgo/src/rot_init/rotation_initialization.cpp +++ b/d2pgo/src/rot_init/rotation_initialization.cpp @@ -1,104 +1,111 @@ #include "rotation_initialization.hpp" -#include "rotation_initialization_base.hpp" + #include "rotation_initialization_arock.hpp" +#include "rotation_initialization_base.hpp" namespace D2PGO { -RotInit::RotInit(PGOState * _state, RotInitConfig _config, ARockSolverConfig arock_config, - bool _enable_arock, std::function _broadcastDataCallback): - enable_float32(_config.enable_float32), - enable_arock(_enable_arock) { - if (enable_arock) { - if (enable_float32) { - rot_int = new RotationInitARockf(_state, _config, arock_config, _broadcastDataCallback); - printf("[D2PGO::RotInit] Mode: ARock + float32\n"); - } else { - rot_int = new RotationInitARockd(_state, _config, arock_config, _broadcastDataCallback); - printf("[D2PGO::RotInit] Mode: ARock + double\n"); - } +RotInit::RotInit(PGOState *_state, RotInitConfig _config, + ARockSolverConfig arock_config, bool _enable_arock, + std::function _broadcastDataCallback) + : enable_float32(_config.enable_float32), enable_arock(_enable_arock) { + if (enable_arock) { + if (enable_float32) { + rot_int = new RotationInitARockf(_state, _config, arock_config, + _broadcastDataCallback); + printf("[D2PGO::RotInit] Mode: ARock + float32\n"); + } else { + rot_int = new RotationInitARockd(_state, _config, arock_config, + _broadcastDataCallback); + printf("[D2PGO::RotInit] Mode: ARock + double\n"); + } + } else { + if (enable_float32) { + rot_int = new RotationInitializationf(_state, _config); + printf("[D2PGO::RotInit] Mode: float32\n"); } else { - if (enable_float32) { - rot_int = new RotationInitializationf(_state, _config); - printf("[D2PGO::RotInit] Mode: float32\n"); - } else { - rot_int = new RotationInitializationd(_state, _config); - printf("[D2PGO::RotInit] Mode: double\n"); - } + rot_int = new RotationInitializationd(_state, _config); + printf("[D2PGO::RotInit] Mode: double\n"); } + } } -void RotInit::addLoops(const std::vector & good_loops) { - if (enable_arock) { - if (enable_float32) { - static_cast(rot_int)->addLoops(good_loops); - } else { - static_cast(rot_int)->addLoops(good_loops); - } +void RotInit::addLoops(const std::vector &good_loops) { + if (enable_arock) { + if (enable_float32) { + static_cast(rot_int)->addLoops(good_loops); + } else { + static_cast(rot_int)->addLoops(good_loops); + } + } else { + if (enable_float32) { + static_cast(rot_int)->addLoops(good_loops); } else { - if (enable_float32) { - static_cast(rot_int)->addLoops(good_loops); - } else { - static_cast(rot_int)->addLoops(good_loops); - } + static_cast(rot_int)->addLoops(good_loops); } + } } -void RotInit::inputDPGOData(const DPGOData & data) { - if (enable_arock) { - if (enable_float32) { - static_cast(rot_int)->inputDPGOData(data); - } else { - static_cast(rot_int)->inputDPGOData(data); - } - } +void RotInit::inputDPGOData(const DPGOData &data) { + if (enable_arock) { + if (enable_float32) { + static_cast(rot_int)->inputDPGOData(data); + } else { + static_cast(rot_int)->inputDPGOData(data); + } + } } void RotInit::setFixedFrameId(FrameIdType _fixed_frame_id) { - if (enable_arock) { - if (enable_float32) { - static_cast(rot_int)->setFixedFrameId(_fixed_frame_id); - } else { - static_cast(rot_int)->setFixedFrameId(_fixed_frame_id); - } + if (enable_arock) { + if (enable_float32) { + static_cast(rot_int)->setFixedFrameId( + _fixed_frame_id); + } else { + static_cast(rot_int)->setFixedFrameId( + _fixed_frame_id); + } + } else { + if (enable_float32) { + static_cast(rot_int)->setFixedFrameId( + _fixed_frame_id); } else { - if (enable_float32) { - static_cast(rot_int)->setFixedFrameId(_fixed_frame_id); - } else { - static_cast(rot_int)->setFixedFrameId(_fixed_frame_id); - } + static_cast(rot_int)->setFixedFrameId( + _fixed_frame_id); } + } } SolverReport RotInit::solve(bool solve_6d) { - if (enable_arock) { - if (enable_float32) { - static_cast(rot_int)->solve_6d = solve_6d; - return static_cast(rot_int)->solve(); - } else { - static_cast(rot_int)->solve_6d = solve_6d; - return static_cast(rot_int)->solve(); - } + if (enable_arock) { + if (enable_float32) { + static_cast(rot_int)->solve_6d = solve_6d; + return static_cast(rot_int)->solve(); } else { - if (enable_float32) { - return static_cast(rot_int)->solve(); - } else { - return static_cast(rot_int)->solve(); - } + static_cast(rot_int)->solve_6d = solve_6d; + return static_cast(rot_int)->solve(); } + } else { + if (enable_float32) { + return static_cast(rot_int)->solve(); + } else { + return static_cast(rot_int)->solve(); + } + } } void RotInit::reset() { - if (enable_arock) { - if (enable_float32) { - static_cast(rot_int)->reset(); - } else { - static_cast(rot_int)->reset(); - } + if (enable_arock) { + if (enable_float32) { + static_cast(rot_int)->reset(); + } else { + static_cast(rot_int)->reset(); + } + } else { + if (enable_float32) { + static_cast(rot_int)->reset(); } else { - if (enable_float32) { - static_cast(rot_int)->reset(); - } else { - static_cast(rot_int)->reset(); - } + static_cast(rot_int)->reset(); } + } } -} \ No newline at end of file +} // namespace D2PGO \ No newline at end of file diff --git a/d2pgo/src/swarm_outlier_rejection/swarm_outlier_rejection.cpp b/d2pgo/src/swarm_outlier_rejection/swarm_outlier_rejection.cpp index 0b48394c..78fee2f2 100644 --- a/d2pgo/src/swarm_outlier_rejection/swarm_outlier_rejection.cpp +++ b/d2pgo/src/swarm_outlier_rejection/swarm_outlier_rejection.cpp @@ -1,247 +1,304 @@ #include "swarm_outlier_rejection.hpp" -#include + +#include #include -#include "fast_max-clique_finder/src/graphIO.h" -#include "fast_max-clique_finder/src/findClique.h" + #include -#include +#include -using D2Common::Utility::TicToc; -using D2Common::FrameIdType; +#include "fast_max-clique_finder/src/findClique.h" +#include "fast_max-clique_finder/src/graphIO.h" +using D2Common::FrameIdType; +using D2Common::Utility::TicToc; #define PCM_DEBUG_OUTPUT namespace D2PGO { std::fstream pcm_errors; -FILE * f_logs; -SwarmLocalOutlierRejection::SwarmLocalOutlierRejection(int _self_id, const SwarmLocalOutlierRejectionParams &_param, std::map &_ego_motion_trajs): - self_id(_self_id), param(_param), ego_motion_trajs(_ego_motion_trajs) { - if (param.debug_write_pcm_errors) { - f_logs = fopen("/root/output/pcm_logs.txt", "w"); - pcm_errors.open("/root/output/pcm_errors.txt", std::ios::out); - pcm_errors.close(); - fclose(f_logs); - } +FILE *f_logs; +SwarmLocalOutlierRejection::SwarmLocalOutlierRejection( + int _self_id, const SwarmLocalOutlierRejectionParams &_param, + std::map &_ego_motion_trajs) + : self_id(_self_id), param(_param), ego_motion_trajs(_ego_motion_trajs) { + if (param.debug_write_pcm_errors) { + f_logs = fopen("/root/output/pcm_logs.txt", "w"); + pcm_errors.open("/root/output/pcm_errors.txt", std::ios::out); + pcm_errors.close(); + fclose(f_logs); + } } std::vector SwarmLocalOutlierRejection::good_loops() { - lcm_mutex.lock(); - std::vector ret; - for (auto & it1: good_loops_set) { - for (auto & it2: it1.second) { - for (auto it3: it2.second) { - ret.push_back(it3); - } - } + lcm_mutex.lock(); + std::vector ret; + for (auto &it1 : good_loops_set) { + for (auto &it2 : it1.second) { + for (auto it3 : it2.second) { + ret.push_back(it3); + } } - lcm_mutex.unlock(); - return ret; + } + lcm_mutex.unlock(); + return ret; } -std::vector SwarmLocalOutlierRejection::OutlierRejectionLoopEdges(ros::Time stamp, const std::vector & available_loops) { - if (param.debug_write_pcm_errors) { - pcm_errors.open("/root/output/pcm_errors.txt", std::ios::app); - f_logs = fopen("/root/output/pcm_logs.txt", "a"); +std::vector +SwarmLocalOutlierRejection::OutlierRejectionLoopEdges( + ros::Time stamp, const std::vector &available_loops) { + if (param.debug_write_pcm_errors) { + pcm_errors.open("/root/output/pcm_errors.txt", std::ios::app); + f_logs = fopen("/root/output/pcm_logs.txt", "a"); + } + + std::map>> new_loops; + std::vector good_loops; + int new_loop_count = 0; + for (auto &edge : available_loops) { + if (all_loops_set.find(edge.id) == all_loops_set.end()) { + new_loops[edge.id_a][edge.id_b].emplace_back(edge); + if (edge.id_a != edge.id_b) { + new_loops[edge.id_b][edge.id_a].emplace_back(edge); + } + if (all_loop_map.find(edge.id) == all_loop_map.end()) { + all_loop_map[edge.id] = edge; + } + all_loops_set_by_pair[edge.id_a][edge.id_b].insert(edge.id); + all_loops_set_by_pair[edge.id_b][edge.id_a].insert(edge.id); + new_loop_count += 1; } + all_loops_set.insert(edge.id); + } + printf("[SWARM_LOCAL](OutlierRejection) %d new loops, %d total loops\n", + new_loop_count, all_loops_set.size()); - std::map>> new_loops; - std::vector good_loops; - int new_loop_count = 0; - for (auto & edge: available_loops) { - if (all_loops_set.find(edge.id) == all_loops_set.end()) { - new_loops[edge.id_a][edge.id_b].emplace_back(edge); - if (edge.id_a!=edge.id_b){ - new_loops[edge.id_b][edge.id_a].emplace_back(edge); - } - if (all_loop_map.find(edge.id) == all_loop_map.end()) { - all_loop_map[edge.id] = edge; - } - all_loops_set_by_pair[edge.id_a][edge.id_b].insert(edge.id); - all_loops_set_by_pair[edge.id_b][edge.id_a].insert(edge.id); - new_loop_count += 1; + if (param.redundant) { + // Note in D2SLAM this is on because we do not broadcast the loops. + // So it's natural to be distributed. + // the other branch is for debugging only. + for (auto it_a : new_loops) { + for (auto it_b : it_a.second) { + if (it_a.first >= it_b.first) { + OutlierRejectionLoopEdgesPCM(it_b.second, it_a.first, it_b.first); } - all_loops_set.insert(edge.id); + } } - printf("[SWARM_LOCAL](OutlierRejection) %d new loops, %d total loops\n", new_loop_count, all_loops_set.size()); - - if (param.redundant) { - // Note in D2SLAM this is on because we do not broadcast the loops. - // So it's natural to be distributed. - // the other branch is for debugging only. - for (auto it_a: new_loops) { - for (auto it_b: it_a.second) { - if (it_a.first >= it_b.first) { - OutlierRejectionLoopEdgesPCM(it_b.second, it_a.first, it_b.first); - } - } - } - } else { - for (auto it_a: new_loops) { - for (auto it_b: it_a.second) { - if (it_a.first == self_id) { - OutlierRejectionLoopEdgesPCM(it_b.second, it_a.first, it_b.first); - } - } + } else { + for (auto it_a : new_loops) { + for (auto it_b : it_a.second) { + if (it_a.first == self_id) { + OutlierRejectionLoopEdgesPCM(it_b.second, it_a.first, it_b.first); } + } } + } - lcm_mutex.lock(); - for (auto & loop : available_loops) { - auto id_a = loop.id_a; - auto id_b = loop.id_b; - if (good_loops_set.find(id_a) == good_loops_set.end() || good_loops_set[id_a].find(id_b) == good_loops_set[id_a].end()) { - //The inlier set of the pair in good loop not established, so we make use all of them - good_loops.emplace_back(loop); - } else { - auto _good_loops_set = good_loops_set[loop.id_a][loop.id_b]; - if (_good_loops_set.find(loop.id) != _good_loops_set.end()) { - good_loops.emplace_back(loop); - } - } + lcm_mutex.lock(); + for (auto &loop : available_loops) { + auto id_a = loop.id_a; + auto id_b = loop.id_b; + if (good_loops_set.find(id_a) == good_loops_set.end() || + good_loops_set[id_a].find(id_b) == good_loops_set[id_a].end()) { + // The inlier set of the pair in good loop not established, so we make use + // all of them + good_loops.emplace_back(loop); + } else { + auto _good_loops_set = good_loops_set[loop.id_a][loop.id_b]; + if (_good_loops_set.find(loop.id) != _good_loops_set.end()) { + good_loops.emplace_back(loop); + } } - lcm_mutex.unlock(); + } + lcm_mutex.unlock(); + + if (param.debug_write_pcm_errors) { + pcm_errors.close(); + fclose(f_logs); + } + + return good_loops; +} + +void SwarmLocalOutlierRejection::OutlierRejectionLoopEdgesPCM( + const std::vector &new_loops, int id_a, int id_b) { + std::map bad_pair_count; - if (param.debug_write_pcm_errors) { - pcm_errors.close(); - fclose(f_logs); + auto &pcm_graph = loop_pcm_graph[id_a][id_b]; + auto &_all_loops = all_loops[id_a][id_b]; + + TicToc tic1; + + for (size_t i = 0; i < new_loops.size(); i++) { + auto &edge1 = new_loops[i]; + // Now only process inter-edges + while (pcm_graph.size() < _all_loops.size() + 1) { + pcm_graph.emplace_back(std::vector(0)); } + auto p_edge1 = edge1.relative_pose; + Matrix6d _cov_mat_1 = edge1.getCovariance(); - return good_loops; -} + for (size_t j = 0; j < _all_loops.size(); j++) { + auto &edge2 = _all_loops[j]; + auto _cov_mat_2 = edge2.getCovariance(); + ; + Matrix6d _covariance = _cov_mat_1 + _cov_mat_2; + + int same_robot_pair = edge2.same_robot_pair(edge1); + if (same_robot_pair > 0) { + // Now we can compute the consistency error. + std::pair odom_a, odom_b; + Swarm::Pose p_edge2; + double traj_a = 0, traj_b = 0; + + if (same_robot_pair == 1) { + p_edge2 = edge2.relative_pose; + // ODOM is tsa->tsb + odom_a = + ego_motion_trajs.at(edge1.id_a) + .get_relative_pose_by_frame_id( + edge1.keyframe_id_a, edge2.keyframe_id_a, param.is_4dof); + odom_b = + ego_motion_trajs.at(edge1.id_b) + .get_relative_pose_by_frame_id( + edge1.keyframe_id_b, edge2.keyframe_id_b, param.is_4dof); + if (param.debug_write_debug) { + traj_a = ego_motion_trajs.at(edge1.id_a) + .trajectory_length_by_ts(edge1.ts_a, edge2.ts_a); + traj_b = ego_motion_trajs.at(edge1.id_b) + .trajectory_length_by_ts(edge1.ts_b, edge2.ts_b); + } -void SwarmLocalOutlierRejection::OutlierRejectionLoopEdgesPCM(const std::vector & new_loops, int id_a, int id_b) { - std::map bad_pair_count; + _covariance += odom_a.second + odom_b.second; - auto & pcm_graph = loop_pcm_graph[id_a][id_b]; - auto & _all_loops = all_loops[id_a][id_b]; + } else if (same_robot_pair == 2) { + p_edge2 = edge2.relative_pose.inverse(); + odom_a = + ego_motion_trajs.at(edge1.id_a) + .get_relative_pose_by_frame_id( + edge1.keyframe_id_a, edge2.keyframe_id_b, param.is_4dof); + odom_b = + ego_motion_trajs.at(edge1.id_b) + .get_relative_pose_by_frame_id( + edge1.keyframe_id_b, edge2.keyframe_id_a, param.is_4dof); - TicToc tic1; + if (param.debug_write_debug) { + traj_a = ego_motion_trajs.at(edge1.id_a) + .trajectory_length_by_ts(edge1.ts_a, edge2.ts_b); + traj_b = ego_motion_trajs.at(edge1.id_b) + .trajectory_length_by_ts(edge1.ts_b, edge2.ts_a); + } - for (size_t i = 0; i < new_loops.size(); i++) { - auto & edge1 = new_loops[i]; - //Now only process inter-edges - while (pcm_graph.size() < _all_loops.size() + 1) { - pcm_graph.emplace_back(std::vector(0)); + _covariance += odom_a.second + odom_b.second; } - auto p_edge1 = edge1.relative_pose; - Matrix6d _cov_mat_1 = edge1.getCovariance(); - - for (size_t j = 0; j < _all_loops.size(); j++) { - auto & edge2 = _all_loops[j]; - auto _cov_mat_2 = edge2.getCovariance();; - Matrix6d _covariance = _cov_mat_1 + _cov_mat_2; - - int same_robot_pair = edge2.same_robot_pair(edge1); - if (same_robot_pair > 0) { - //Now we can compute the consistency error. - std::pair odom_a, odom_b; - Swarm::Pose p_edge2; - double traj_a = 0, traj_b = 0; - - if (same_robot_pair == 1) { - p_edge2 = edge2.relative_pose; - //ODOM is tsa->tsb - odom_a = ego_motion_trajs.at(edge1.id_a).get_relative_pose_by_frame_id(edge1.keyframe_id_a, edge2.keyframe_id_a, param.is_4dof); - odom_b = ego_motion_trajs.at(edge1.id_b).get_relative_pose_by_frame_id(edge1.keyframe_id_b, edge2.keyframe_id_b, param.is_4dof); - if (param.debug_write_debug) { - traj_a = ego_motion_trajs.at(edge1.id_a).trajectory_length_by_ts(edge1.ts_a, edge2.ts_a); - traj_b = ego_motion_trajs.at(edge1.id_b).trajectory_length_by_ts(edge1.ts_b, edge2.ts_b); - } - - _covariance += odom_a.second + odom_b.second; - - } else if (same_robot_pair == 2) { - p_edge2 = edge2.relative_pose.inverse(); - odom_a = ego_motion_trajs.at(edge1.id_a).get_relative_pose_by_frame_id(edge1.keyframe_id_a, edge2.keyframe_id_b, param.is_4dof); - odom_b = ego_motion_trajs.at(edge1.id_b).get_relative_pose_by_frame_id(edge1.keyframe_id_b, edge2.keyframe_id_a, param.is_4dof); - - if (param.debug_write_debug) { - traj_a = ego_motion_trajs.at(edge1.id_a).trajectory_length_by_ts(edge1.ts_a, edge2.ts_b); - traj_b = ego_motion_trajs.at(edge1.id_b).trajectory_length_by_ts(edge1.ts_b, edge2.ts_a); - } - - _covariance += odom_a.second + odom_b.second; - } - - Swarm::Pose err_pose = odom_a.first*p_edge2*odom_b.first.inverse()*p_edge1.inverse(); - auto logmap = err_pose.log_map(); - double smd = Swarm::computeSquaredMahalanobisDistance(logmap, _covariance); - - if (smd < param.pcm_thres) { - //Add edge i to j - pcm_graph[_all_loops.size()].push_back(j); - pcm_graph[j].push_back(_all_loops.size()); - } - - if (param.debug_write_debug) { - fprintf(f_logs, "\n"); - fprintf(f_logs, "EdgePair %ld->%ld\n", edge1.id, edge2.id); - fprintf(f_logs, "Edge1 %ld->%ld DOF %d Pose %s cov_1 [%+3.1e,%+3.1e,%+3.1e,%+3.1e,%+3.1e,%+3.1e]\n", - edge1.keyframe_id_a, edge1.keyframe_id_b, edge1.res_count, edge1.relative_pose.toStr().c_str(), - _cov_mat_1(0, 0), _cov_mat_1(1, 1), _cov_mat_1(2, 2), _cov_mat_1(3, 3), _cov_mat_1(4, 4), _cov_mat_1(5, 5)); - fprintf(f_logs, "Edge2 %ld->%ld DOF %d Pose %s cov_2 [%+3.1e,%+3.1e,%+3.1e,%+3.1e,%+3.1e,%+3.1e]\n", - edge2.keyframe_id_a, edge2.keyframe_id_b, edge2.res_count, edge2.relative_pose.toStr().c_str(), - _cov_mat_2(0, 0), _cov_mat_2(1, 1), _cov_mat_2(2, 2), _cov_mat_2(3, 3), _cov_mat_2(4, 4), _cov_mat_2(5, 5)); - - auto cov = odom_a.second; - fprintf(f_logs, "odom_a %s traj len %.2f cov (T, Q) [%+3.1e,%+3.1e,%+3.1e,%+3.1e,%+3.1e,%+3.1e]\n", odom_a.first.toStr().c_str(), - traj_a, cov(0, 0), cov(1, 1), cov(2, 2), cov(3, 3), cov(4, 4), cov(5, 5)); - cov = odom_b.second; - fprintf(f_logs, "odom_b %s traj len %.2f cov (T, Q) [%+3.1e,%+3.1e,%+3.1e,%+3.1e,%+3.1e,%+3.1e]\n", odom_b.first.toStr().c_str(), - traj_b, cov(0, 0), cov(1, 1), cov(2, 2), cov(3, 3), cov(4, 4), cov(5, 5)); - fprintf(f_logs, "err_pose %s logmap [%+3.1e,%+3.1e,%+3.1e,%+3.1e,%+3.1e,%+3.1e]\n", err_pose.toStr().c_str(), - logmap(0), logmap(1), logmap(2), logmap(3), logmap(4), logmap(5)); - fprintf(f_logs, "squaredMahalanobisDistance %f Same Direction %d _cov(T, Q) [%+3.1e,%+3.1e,%+3.1e,%+3.1e,%+3.1e,%+3.1e]\n", smd, same_robot_pair == 1, - _covariance(0, 0), _covariance(1, 1), _covariance(2, 2), _covariance(3, 3), _covariance(4, 4), _covariance(5, 5)); - } - - if (param.debug_write_pcm_errors) { - pcm_errors << edge1.id << " " << edge2.id << " " << smd << " " << std::endl; - } - - } + Swarm::Pose err_pose = + odom_a.first * p_edge2 * odom_b.first.inverse() * p_edge1.inverse(); + auto logmap = err_pose.log_map(); + double smd = + Swarm::computeSquaredMahalanobisDistance(logmap, _covariance); + + if (smd < param.pcm_thres) { + // Add edge i to j + pcm_graph[_all_loops.size()].push_back(j); + pcm_graph[j].push_back(_all_loops.size()); } - _all_loops.push_back(edge1); - } - double compute_pcm_erros = tic1.toc(); - std::vector max_clique_data; - FMC::CGraphIO pcm_graph_fmc; - pcm_graph_fmc.m_vi_Vertices.push_back(0); - for(size_t i=0;i < pcm_graph.size(); i++) { - pcm_graph_fmc.m_vi_Edges.insert(pcm_graph_fmc.m_vi_Edges.end(),pcm_graph[i].begin(),pcm_graph[i].end()); - pcm_graph_fmc.m_vi_Vertices.push_back(pcm_graph_fmc.m_vi_Edges.size()); - } - pcm_graph_fmc.CalculateVertexDegrees(); - if (param.incremental_pcm) { - TicToc tic; - int prev_max_clique_size = good_loops_set[id_a][id_b].size(); - int ret = FMC::maxCliqueHeuIncremental(pcm_graph_fmc, new_loops.size(), prev_max_clique_size, max_clique_data); - if (ret > 0 && max_clique_data.size() > 0) { - good_loops_set[id_a][id_b].clear(); - good_loops_set[id_b][id_a].clear(); - for (auto i : max_clique_data) { - good_loops_set[id_a][id_b].insert(_all_loops[i].id); - good_loops_set[id_b][id_a].insert(_all_loops[i].id); - } + if (param.debug_write_debug) { + fprintf(f_logs, "\n"); + fprintf(f_logs, "EdgePair %ld->%ld\n", edge1.id, edge2.id); + fprintf(f_logs, + "Edge1 %ld->%ld DOF %d Pose %s cov_1 " + "[%+3.1e,%+3.1e,%+3.1e,%+3.1e,%+3.1e,%+3.1e]\n", + edge1.keyframe_id_a, edge1.keyframe_id_b, edge1.res_count, + edge1.relative_pose.toStr().c_str(), _cov_mat_1(0, 0), + _cov_mat_1(1, 1), _cov_mat_1(2, 2), _cov_mat_1(3, 3), + _cov_mat_1(4, 4), _cov_mat_1(5, 5)); + fprintf(f_logs, + "Edge2 %ld->%ld DOF %d Pose %s cov_2 " + "[%+3.1e,%+3.1e,%+3.1e,%+3.1e,%+3.1e,%+3.1e]\n", + edge2.keyframe_id_a, edge2.keyframe_id_b, edge2.res_count, + edge2.relative_pose.toStr().c_str(), _cov_mat_2(0, 0), + _cov_mat_2(1, 1), _cov_mat_2(2, 2), _cov_mat_2(3, 3), + _cov_mat_2(4, 4), _cov_mat_2(5, 5)); + + auto cov = odom_a.second; + fprintf(f_logs, + "odom_a %s traj len %.2f cov (T, Q) " + "[%+3.1e,%+3.1e,%+3.1e,%+3.1e,%+3.1e,%+3.1e]\n", + odom_a.first.toStr().c_str(), traj_a, cov(0, 0), cov(1, 1), + cov(2, 2), cov(3, 3), cov(4, 4), cov(5, 5)); + cov = odom_b.second; + fprintf(f_logs, + "odom_b %s traj len %.2f cov (T, Q) " + "[%+3.1e,%+3.1e,%+3.1e,%+3.1e,%+3.1e,%+3.1e]\n", + odom_b.first.toStr().c_str(), traj_b, cov(0, 0), cov(1, 1), + cov(2, 2), cov(3, 3), cov(4, 4), cov(5, 5)); + fprintf(f_logs, + "err_pose %s logmap " + "[%+3.1e,%+3.1e,%+3.1e,%+3.1e,%+3.1e,%+3.1e]\n", + err_pose.toStr().c_str(), logmap(0), logmap(1), logmap(2), + logmap(3), logmap(4), logmap(5)); + fprintf(f_logs, + "squaredMahalanobisDistance %f Same Direction %d _cov(T, Q) " + "[%+3.1e,%+3.1e,%+3.1e,%+3.1e,%+3.1e,%+3.1e]\n", + smd, same_robot_pair == 1, _covariance(0, 0), + _covariance(1, 1), _covariance(2, 2), _covariance(3, 3), + _covariance(4, 4), _covariance(5, 5)); } - printf("[D2PGO](OutlierRejection) %d<->%d compute_pcm_errors %.1fms maxCliqueHeuInc takes %.1fms ret %d(%d) loops %ld good %ld\n", - id_a, id_b, compute_pcm_erros, tic.toc(), ret, max_clique_data.size(), _all_loops.size(), good_loops_set[id_a][id_b].size()); - } else { - TicToc tic; - FMC::maxCliqueHeu(pcm_graph_fmc, max_clique_data); - printf("[D2PGO](OutlierRejection) %d<->%d compute_pcm_errors %.1fms maxCliqueHeu takes %.1fms loops %ld good %ld\n", - id_a, id_b, compute_pcm_erros, tic.toc(), _all_loops.size(), max_clique_data.size()); - //In non-incremental mode, we need to clear the good_loops_set - good_loops_set[id_a][id_b].clear(); - good_loops_set[id_b][id_a].clear(); - for (auto i : max_clique_data) { - good_loops_set[id_a][id_b].insert(_all_loops[i].id); - good_loops_set[id_b][id_a].insert(_all_loops[i].id); + + if (param.debug_write_pcm_errors) { + pcm_errors << edge1.id << " " << edge2.id << " " << smd << " " + << std::endl; } + } + } + _all_loops.push_back(edge1); + } + + double compute_pcm_erros = tic1.toc(); + std::vector max_clique_data; + FMC::CGraphIO pcm_graph_fmc; + pcm_graph_fmc.m_vi_Vertices.push_back(0); + for (size_t i = 0; i < pcm_graph.size(); i++) { + pcm_graph_fmc.m_vi_Edges.insert(pcm_graph_fmc.m_vi_Edges.end(), + pcm_graph[i].begin(), pcm_graph[i].end()); + pcm_graph_fmc.m_vi_Vertices.push_back(pcm_graph_fmc.m_vi_Edges.size()); + } + pcm_graph_fmc.CalculateVertexDegrees(); + if (param.incremental_pcm) { + TicToc tic; + int prev_max_clique_size = good_loops_set[id_a][id_b].size(); + int ret = FMC::maxCliqueHeuIncremental( + pcm_graph_fmc, new_loops.size(), prev_max_clique_size, max_clique_data); + if (ret > 0 && max_clique_data.size() > 0) { + good_loops_set[id_a][id_b].clear(); + good_loops_set[id_b][id_a].clear(); + for (auto i : max_clique_data) { + good_loops_set[id_a][id_b].insert(_all_loops[i].id); + good_loops_set[id_b][id_a].insert(_all_loops[i].id); + } + } + printf( + "[D2PGO](OutlierRejection) %d<->%d compute_pcm_errors %.1fms " + "maxCliqueHeuInc takes %.1fms ret %d(%d) loops %ld good %ld\n", + id_a, id_b, compute_pcm_erros, tic.toc(), ret, max_clique_data.size(), + _all_loops.size(), good_loops_set[id_a][id_b].size()); + } else { + TicToc tic; + FMC::maxCliqueHeu(pcm_graph_fmc, max_clique_data); + printf( + "[D2PGO](OutlierRejection) %d<->%d compute_pcm_errors %.1fms " + "maxCliqueHeu takes %.1fms loops %ld good %ld\n", + id_a, id_b, compute_pcm_erros, tic.toc(), _all_loops.size(), + max_clique_data.size()); + // In non-incremental mode, we need to clear the good_loops_set + good_loops_set[id_a][id_b].clear(); + good_loops_set[id_b][id_a].clear(); + for (auto i : max_clique_data) { + good_loops_set[id_a][id_b].insert(_all_loops[i].id); + good_loops_set[id_b][id_a].insert(_all_loops[i].id); } + } } -} \ No newline at end of file +} // namespace D2PGO \ No newline at end of file diff --git a/d2pgo/test/d2pgo_test.cpp b/d2pgo/test/d2pgo_test.cpp index 88c880ba..5380fd9a 100644 --- a/d2pgo/test/d2pgo_test.cpp +++ b/d2pgo/test/d2pgo_test.cpp @@ -1,10 +1,13 @@ #include "d2pgo_test.hpp" -#include "posegraph_g2o.hpp" -#include "../src/d2pgo.h" -#include -#include + #include +#include #include + +#include + +#include "../src/d2pgo.h" +#include "posegraph_g2o.hpp" // #include using namespace D2PGO; @@ -17,265 +20,291 @@ using namespace D2PGO; // } class D2PGOTester { - D2PGO::D2PGO * pgo = nullptr; - std::string g2o_path; - std::string solver_type; - ros::Publisher dpgo_data_pub, dpgo_signal_pub; - ros::Subscriber dpgo_data_sub, dpgo_signal_sub; - bool is_4dof; - std::thread th, th_process_delay; - std::string output_path; - ros::NodeHandle & _nh; - bool multi = false; - int max_steps = 100; - int drone_num = 1; + D2PGO::D2PGO* pgo = nullptr; + std::string g2o_path; + std::string solver_type; + ros::Publisher dpgo_data_pub, dpgo_signal_pub; + ros::Subscriber dpgo_data_sub, dpgo_signal_sub; + bool is_4dof; + std::thread th, th_process_delay; + std::string output_path; + ros::NodeHandle& _nh; + bool multi = false; + int max_steps = 100; + int drone_num = 1; + + double simulate_delay_ms = 0; + bool enable_simulate_delay = false; + double max_solving_time = 10.0; + D2PGOConfig config; + + std::map path_pubs; + std::vector edges; + std::map keyframeid_agent_pose; + std::map buf_for_simulate_delay; - double simulate_delay_ms = 0; - bool enable_simulate_delay = false; - double max_solving_time = 10.0; - D2PGOConfig config; + public: + int self_id; + void initSubandPub(ros::NodeHandle& nh) { + dpgo_data_pub = nh.advertise("/dpgo/pgo_data", 100); + dpgo_signal_pub = + nh.advertise("/dpgo/pgo_signal", 100); + dpgo_data_sub = + nh.subscribe("/dpgo/pgo_data", 100, &D2PGOTester::processDPGOData, this, + ros::TransportHints().tcpNoDelay()); + dpgo_signal_sub = + nh.subscribe("/dpgo/pgo_signal", 100, &D2PGOTester::processDPGOSignal, + this, ros::TransportHints().tcpNoDelay()); + } - std::map path_pubs; - std::vector edges; - std::map keyframeid_agent_pose; - std::map buf_for_simulate_delay; -public: - int self_id; - void initSubandPub(ros::NodeHandle & nh) { - dpgo_data_pub = nh.advertise("/dpgo/pgo_data", 100); - dpgo_signal_pub = nh.advertise("/dpgo/pgo_signal", 100); - dpgo_data_sub = nh.subscribe("/dpgo/pgo_data", 100, &D2PGOTester::processDPGOData, this, ros::TransportHints().tcpNoDelay()); - dpgo_signal_sub = nh.subscribe("/dpgo/pgo_signal", 100, &D2PGOTester::processDPGOSignal, this, ros::TransportHints().tcpNoDelay()); + D2PGOTester(ros::NodeHandle& nh) : _nh(nh) { + nh.param("g2o_path", g2o_path, ""); + nh.param("output_path", output_path, "test.g2o"); + nh.param("self_id", self_id, -1); + nh.param("is_4dof", is_4dof, true); + nh.param("is_multi", multi, false); + bool ignore_infor; + nh.param("ignore_infor", ignore_infor, false); + nh.param("solver_type", solver_type, "arock"); + nh.param("simulate_delay_ms", simulate_delay_ms, 0.0); + nh.param("max_solving_time", max_solving_time, 0.0); + nh.param("drone_num", drone_num, 1); + if (simulate_delay_ms > 0) { + enable_simulate_delay = true; + th_process_delay = std::thread([&] { this->process_simulate_delay(); }); } - D2PGOTester(ros::NodeHandle & nh): - _nh(nh) { - nh.param("g2o_path", g2o_path, ""); - nh.param("output_path", output_path, "test.g2o"); - nh.param("self_id", self_id, -1); - nh.param("is_4dof", is_4dof, true); - nh.param("is_multi", multi, false); - bool ignore_infor; - nh.param("ignore_infor", ignore_infor, false); - nh.param("solver_type", solver_type, "arock"); - nh.param("simulate_delay_ms", simulate_delay_ms, 0.0); - nh.param("max_solving_time", max_solving_time, 0.0); - nh.param("drone_num", drone_num, 1); - if (simulate_delay_ms > 0) { - enable_simulate_delay = true; - th_process_delay = std::thread([&] { - this->process_simulate_delay(); - }); - } + if (g2o_path != "") + ROS_INFO("[D2PGO] agent %d parse g2o file: %s\n", self_id, + g2o_path.c_str()); + else + ROS_INFO("[D2PGO@%d] Need to indicate g2o path\n", self_id); + read_g2o_agent(g2o_path, keyframeid_agent_pose, edges, is_4dof, + drone_num - 1, self_id, ignore_infor); + ROS_INFO("[D2PGO@%d] Read %ld keyframes and %ld edges\n", self_id, + keyframeid_agent_pose.size(), edges.size()); - if (g2o_path != "") - ROS_INFO("[D2PGO] agent %d parse g2o file: %s\n", self_id, g2o_path.c_str()); - else - ROS_INFO("[D2PGO@%d] Need to indicate g2o path\n", self_id); - read_g2o_agent(g2o_path, keyframeid_agent_pose, edges, is_4dof, drone_num-1, self_id, ignore_infor); - ROS_INFO("[D2PGO@%d] Read %ld keyframes and %ld edges\n", self_id, keyframeid_agent_pose.size(), edges.size()); + config.self_id = self_id; + if (is_4dof) + config.pgo_pose_dof = PGO_POSE_4D; + else + config.pgo_pose_dof = PGO_POSE_6D; + nh.param("loop_distance_threshold", config.loop_distance_threshold, + 1000); + config.enable_ego_motion = false; + config.ceres_options.linear_solver_type = + ceres::SPARSE_NORMAL_CHOLESKY; // ceres::DENSE_SCHUR; + config.ceres_options.num_threads = 1; + config.ceres_options.trust_region_strategy_type = + ceres::LEVENBERG_MARQUARDT; // ceres::DOGLEG; + nh.param("ceres_max_solver_time", + config.ceres_options.max_solver_time_in_seconds, 0.1); + nh.param("ceres_max_num_iterations", + config.ceres_options.max_num_iterations, 50); + config.main_id = 0; + config.arock_config.self_id = config.self_id; + config.arock_config.verbose = true; + config.arock_config.ceres_options = config.ceres_options; + config.arock_config.max_steps = 1; + config.g2o_output_path = output_path; + config.write_g2o = false; + nh.param("max_steps", max_steps, 10); + nh.param("rho_frame_T", config.arock_config.rho_frame_T, 0.1); + nh.param("rho_frame_theta", config.arock_config.rho_frame_theta, + 0.1); + nh.param("rho_rot_mat", config.arock_config.rho_rot_mat, 0.1); + nh.param("eta_k", config.arock_config.eta_k, 0.9); + nh.param("enable_rot_init", config.enable_rotation_initialization, + true); + nh.param("rot_init_enable_gravity_prior", + config.rot_init_config.enable_gravity_prior, true); + nh.param("rot_init_gravity_sqrt_info", + config.rot_init_config.gravity_sqrt_info, 10); + nh.param("rot_init_enable_float32", + config.rot_init_config.enable_float32, false); + nh.param("enable_linear_pose6d_solver", + config.rot_init_config.enable_pose6d_solver, false); + nh.param("linear_pose6d_iterations", + config.rot_init_config.pose6d_iterations, 10); + nh.param("debug_rot_init_only", config.debug_rot_init_only, true); + nh.param("rot_init_state_eps", config.rot_init_state_eps, 0.01); + config.rot_init_config.self_id = self_id; + if (solver_type == "ceres") { + config.mode = PGO_MODE_NON_DIST; + } else { + config.mode = PGO_MODE_DISTRIBUTED_AROCK; + } - config.self_id = self_id; - if (is_4dof) - config.pgo_pose_dof = PGO_POSE_4D; - else - config.pgo_pose_dof = PGO_POSE_6D; - nh.param("loop_distance_threshold", config.loop_distance_threshold, 1000); - config.enable_ego_motion = false; - config.ceres_options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;// ceres::DENSE_SCHUR; - config.ceres_options.num_threads = 1; - config.ceres_options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT;// ceres::DOGLEG; - nh.param("ceres_max_solver_time", config.ceres_options.max_solver_time_in_seconds, 0.1); - nh.param("ceres_max_num_iterations", config.ceres_options.max_num_iterations, 50); - config.main_id = 0; - config.arock_config.self_id = config.self_id; - config.arock_config.verbose = true; - config.arock_config.ceres_options = config.ceres_options; - config.arock_config.max_steps = 1; - config.g2o_output_path = output_path; - config.write_g2o = false; - nh.param("max_steps", max_steps, 10); - nh.param("rho_frame_T", config.arock_config.rho_frame_T, 0.1); - nh.param("rho_frame_theta", config.arock_config.rho_frame_theta, 0.1); - nh.param("rho_rot_mat", config.arock_config.rho_rot_mat, 0.1); - nh.param("eta_k", config.arock_config.eta_k, 0.9); - nh.param("enable_rot_init", config.enable_rotation_initialization, true); - nh.param("rot_init_enable_gravity_prior", config.rot_init_config.enable_gravity_prior, true); - nh.param("rot_init_gravity_sqrt_info", config.rot_init_config.gravity_sqrt_info, 10); - nh.param("rot_init_enable_float32", config.rot_init_config.enable_float32, false); - nh.param("enable_linear_pose6d_solver", config.rot_init_config.enable_pose6d_solver, false); - nh.param("linear_pose6d_iterations", config.rot_init_config.pose6d_iterations, 10); - nh.param("debug_rot_init_only", config.debug_rot_init_only, true); - nh.param("rot_init_state_eps", config.rot_init_state_eps, 0.01); - config.rot_init_config.self_id = self_id; - if (solver_type == "ceres") { - config.mode = PGO_MODE_NON_DIST; - } else { - config.mode = PGO_MODE_DISTRIBUTED_AROCK; - } + pgo = new D2PGO::D2PGO(config); + std::set agent_ids; + for (int i = 0; i < drone_num; i++) { + agent_ids.insert(i); + } + pgo->setAvailableRobots(agent_ids); + for (auto& kv : keyframeid_agent_pose) { + pgo->addFrame(kv.second); + } + for (auto& edge : edges) { + pgo->addLoop(edge, true); // In this test program, we use loop to + // initialize unknown poses. + } - pgo = new D2PGO::D2PGO(config); - std::set agent_ids; - for (int i = 0; i < drone_num; i++) { - agent_ids.insert(i); - } - pgo->setAvailableRobots(agent_ids); - for (auto & kv : keyframeid_agent_pose) { - pgo->addFrame(kv.second); - } - for (auto & edge : edges) { - pgo->addLoop(edge, true); //In this test program, we use loop to initialize unknown poses. - } + pgo->bd_data_callback = [&](const DPGOData& data) { + // ROS_INFO("[D2PGO@%d] publish sync", self_id); + dpgo_data_pub.publish(data.toROS()); + }; - pgo->bd_data_callback = [&] (const DPGOData & data) { - // ROS_INFO("[D2PGO@%d] publish sync", self_id); - dpgo_data_pub.publish(data.toROS()); - }; + pgo->bd_signal_callback = [&](const std::string& signal) { + // ROS_INFO("[D2PGO@%d] publish signal %s", self_id, signal.c_str()); + swarm_msgs::DPGOSignal msg; + msg.header.stamp = ros::Time::now(); + msg.signal = signal; + msg.drone_id = self_id; + msg.target_id = -1; + dpgo_signal_pub.publish(msg); + }; - pgo->bd_signal_callback = [&] (const std::string & signal) { - // ROS_INFO("[D2PGO@%d] publish signal %s", self_id, signal.c_str()); - swarm_msgs::DPGOSignal msg; - msg.header.stamp = ros::Time::now(); - msg.signal = signal; - msg.drone_id = self_id; - msg.target_id = -1; - dpgo_signal_pub.publish(msg); - }; + pgo->postsolve_callback = [&](void) { + auto trajs = pgo->getOptimizedTrajs(); + pubTrajs(trajs); + // Sleep for visualization. + // usleep(100*1000); + }; + initSubandPub(nh); + } - pgo->postsolve_callback = [&] (void) { - auto trajs = pgo->getOptimizedTrajs(); - pubTrajs(trajs); - //Sleep for visualization. - // usleep(100*1000); - }; - initSubandPub(nh); + void processDPGOData(const swarm_msgs::DPGOData& data) { + if (data.drone_id != self_id) { + // ROS_INFO("[D2PGONode@%d] processDPGOData from drone %d", self_id, + // data.drone_id); + if (enable_simulate_delay) { + ros::Time now = ros::Time::now(); + buf_for_simulate_delay[now.toSec()] = data; + } else { + pgo->inputDPGOData(DPGOData(data)); + } } + } - void processDPGOData(const swarm_msgs::DPGOData & data) { - if (data.drone_id != self_id) { - // ROS_INFO("[D2PGONode@%d] processDPGOData from drone %d", self_id, data.drone_id); - if (enable_simulate_delay) { - ros::Time now = ros::Time::now(); - buf_for_simulate_delay[now.toSec()] = data; - } else { - pgo->inputDPGOData(DPGOData(data)); - } - } + void processDPGOSignal(const swarm_msgs::DPGOSignal& msg) { + if (msg.drone_id != self_id) { + ROS_INFO("[D2PGONode@%d] processDPGOSignal from drone %d: %s", self_id, + msg.drone_id, msg.signal.c_str()); + pgo->inputDPGOsignal(msg.drone_id, msg.signal); } + } - void processDPGOSignal(const swarm_msgs::DPGOSignal & msg) { - if (msg.drone_id != self_id) { - ROS_INFO("[D2PGONode@%d] processDPGOSignal from drone %d: %s", self_id, msg.drone_id, msg.signal.c_str()); - pgo->inputDPGOsignal(msg.drone_id, msg.signal); - } - } + void startSignalCallback(const std_msgs::Int32& msg) { startSolve(); } - void startSignalCallback(const std_msgs::Int32 & msg) { - startSolve(); - } + void pubLoops(const std::vector loops) { + // for (auto loop : loops) { + // visualization_msgs::Marker marker; + // marker.ns = "marker"; + // marker.id = loop.id; + // marker.type = visualization_msgs::Marker::LINE_STRIP; + // marker.action = visualization_msgs::Marker::ADD; + // marker.lifetime = ros::Duration(); + // marker.scale.x = 0.02; + // marker.color.r = 1.0f; + // marker.color.a = 1.0; - void pubLoops(const std::vector loops) { - // for (auto loop : loops) { - // visualization_msgs::Marker marker; - // marker.ns = "marker"; - // marker.id = loop.id; - // marker.type = visualization_msgs::Marker::LINE_STRIP; - // marker.action = visualization_msgs::Marker::ADD; - // marker.lifetime = ros::Duration(); - // marker.scale.x = 0.02; - // marker.color.r = 1.0f; - // marker.color.a = 1.0; + // geometry_msgs::Point point0, point1; + // Eigen2Point(p0, point0); + // Eigen2Point(p1, point1); + // marker.points.push_back(point0); + // marker.points.push_back(point1); + // m_markers.push_back(marker); + // } + } - // geometry_msgs::Point point0, point1; - // Eigen2Point(p0, point0); - // Eigen2Point(p1, point1); - // marker.points.push_back(point0); - // marker.points.push_back(point1); - // m_markers.push_back(marker); - // } + void pubTrajs(const std::map& trajs) { + for (auto it : trajs) { + auto drone_id = it.first; + auto traj = it.second; + if (path_pubs.find(drone_id) == path_pubs.end()) { + path_pubs[drone_id] = _nh.advertise( + "pgo_path_" + std::to_string(drone_id), 1000); + } + path_pubs[drone_id].publish(traj.get_ros_path()); } + } - void pubTrajs(const std::map & trajs) { - for (auto it : trajs) { - auto drone_id = it.first; - auto traj = it.second; - if (path_pubs.find(drone_id) == path_pubs.end()) { - path_pubs[drone_id] = _nh.advertise("pgo_path_" + std::to_string(drone_id), 1000); - } - path_pubs[drone_id].publish(traj.get_ros_path()); + void startSolve() { + th = std::thread([&]() { + Utility::TicToc t_solve; + int iter = 0; + for (int i = 0; i < max_steps; i++) { + iter++; + if (multi) { + pgo->solve_multi(true); + } else { + pgo->solve_single(); } - } + // usleep(20*1000); + if (t_solve.toc() / 1000.0 > max_solving_time) { + printf("[D2PGO%d] Solve timeout. Time: %fms\n", self_id, + t_solve.toc()); + break; + } + } + printf("[D2PGO%d] Solve done. Time: %fms iters %d\n", self_id, + t_solve.toc(), iter); + // Write data + if (config.perturb_mode) { + pgo->postPerturbSolve(); + } + writeDataG2o(); + printf("[D2PGO%d] Write data done. Finish solve.\n", self_id); + fflush(stdout); + // ros::shutdown(); + exit(0); + }); + } - void startSolve() { - th = std::thread([&]() { - Utility::TicToc t_solve; - int iter = 0; - for (int i = 0; i < max_steps; i ++) { - iter ++; - if (multi) { - pgo->solve_multi(true); - } else { - pgo->solve_single(); - } - // usleep(20*1000); - if (t_solve.toc()/1000.0 > max_solving_time) { - printf("[D2PGO%d] Solve timeout. Time: %fms\n", self_id, t_solve.toc()); - break; - } - } - printf("[D2PGO%d] Solve done. Time: %fms iters %d\n", self_id, t_solve.toc(), iter); - //Write data - if (config.perturb_mode) { - pgo->postPerturbSolve(); - } - writeDataG2o(); - printf("[D2PGO%d] Write data done. Finish solve.\n", self_id); - fflush(stdout); - // ros::shutdown(); - exit(0); - }); - } + void writeDataG2o() { + auto local_frames = pgo->getAllLocalFrames(); + write_result_to_g2o(output_path, local_frames, edges); + printf("[D2PGO@%d] Write result to %s\n", self_id, output_path.c_str()); + } - void writeDataG2o() { - auto local_frames = pgo->getAllLocalFrames(); - write_result_to_g2o(output_path, local_frames, edges); - printf("[D2PGO@%d] Write result to %s\n", self_id, output_path.c_str()); - } - - void process_simulate_delay() { - while(ros::ok()) { - ros::Time now = ros::Time::now(); - double now_sec = now.toSec(); - for (auto it = buf_for_simulate_delay.begin(); it != buf_for_simulate_delay.end();) { - if (now_sec - it->first > simulate_delay_ms / 1000.0) { - pgo->inputDPGOData(DPGOData(it->second)); - it = buf_for_simulate_delay.erase(it); - } else { - it++; - } - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); + void process_simulate_delay() { + while (ros::ok()) { + ros::Time now = ros::Time::now(); + double now_sec = now.toSec(); + for (auto it = buf_for_simulate_delay.begin(); + it != buf_for_simulate_delay.end();) { + if (now_sec - it->first > simulate_delay_ms / 1000.0) { + pgo->inputDPGOData(DPGOData(it->second)); + it = buf_for_simulate_delay.erase(it); + } else { + it++; } + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); } + } }; -int main(int argc, char ** argv) { - cv::setNumThreads(1); - ros::init(argc, argv, "d2pgo_test"); - ros::NodeHandle n("~"); - ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info); - bool wait_for_start = false; - n.param("wait_for_start", wait_for_start, ""); - D2PGOTester tester(n); - ros::Subscriber start_sub; - if (wait_for_start) { - bool is_start = false; - start_sub = n.subscribe("/dpgo/start_solve_trigger", 1, &D2PGOTester::startSignalCallback, &tester, ros::TransportHints().tcpNoDelay(true)); - } else { - tester.startSolve(); - } - printf("[D2PGO@%d] Waiting for solve\n", tester.self_id); - ros::MultiThreadedSpinner spinner(4); - spinner.spin(); - return 0; +int main(int argc, char** argv) { + cv::setNumThreads(1); + ros::init(argc, argv, "d2pgo_test"); + ros::NodeHandle n("~"); + ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, + ros::console::levels::Info); + bool wait_for_start = false; + n.param("wait_for_start", wait_for_start, ""); + D2PGOTester tester(n); + ros::Subscriber start_sub; + if (wait_for_start) { + bool is_start = false; + start_sub = n.subscribe("/dpgo/start_solve_trigger", 1, + &D2PGOTester::startSignalCallback, &tester, + ros::TransportHints().tcpNoDelay(true)); + } else { + tester.startSolve(); + } + printf("[D2PGO@%d] Waiting for solve\n", tester.self_id); + ros::MultiThreadedSpinner spinner(4); + spinner.spin(); + return 0; } \ No newline at end of file diff --git a/d2pgo/test/posegraph_g2o.cpp b/d2pgo/test/posegraph_g2o.cpp index abeb4208..69369355 100644 --- a/d2pgo/test/posegraph_g2o.cpp +++ b/d2pgo/test/posegraph_g2o.cpp @@ -1,4 +1,5 @@ #include "posegraph_g2o.hpp" + #include #include @@ -6,7 +7,9 @@ namespace fs = boost::filesystem; namespace D2PGO { -std::regex reg_vertex_se3("VERTEX_SE3:QUAT\\s+(\\S+)\\s+(\\S+)\\s+(\\S+)\\s+(\\S+)\\s+(\\S+)\\s+(\\S+)\\s+(\\S+)\\s+(\\S+)"); +std::regex reg_vertex_se3( + "VERTEX_SE3:QUAT\\s+(\\S+)\\s+(\\S+)\\s+(\\S+)\\s+(\\S+)\\s+(\\S+)\\s+(\\S+" + ")\\s+(\\S+)\\s+(\\S+)"); std::regex reg_edge_se3("EDGE_SE3:QUAT\\s+([\\s,\\S]+)"); #define IDX_WITH_AGENT_ID_MIN 6989586621679009792 @@ -16,205 +19,215 @@ extern std::default_random_engine eng; extern std::normal_distribution d; bool is_number(const std::string& s) { - return !s.empty() && std::find_if(s.begin(), - s.end(), [](unsigned char c) { return !std::isdigit(c); }) == s.end(); + return !s.empty() && std::find_if(s.begin(), s.end(), [](unsigned char c) { + return !std::isdigit(c); + }) == s.end(); } -std::pair extrackKeyframeId(const int64_t & input) { - if (input < IDX_WITH_AGENT_ID_MIN) { - return std::make_pair(0, input); - } - const int keyBits = 8*8; - const int chrBits= 1*8; - const int indexBits = keyBits - chrBits; - const int64_t chrMask = (int64_t)255 << indexBits; - const int64_t indexMask = ~chrMask; - int agent_id = ((input & chrMask) >> indexBits)-97; - FrameIdType keyframe_id = input & indexMask; - return std::make_pair(agent_id, keyframe_id); +std::pair extrackKeyframeId(const int64_t& input) { + if (input < IDX_WITH_AGENT_ID_MIN) { + return std::make_pair(0, input); + } + const int keyBits = 8 * 8; + const int chrBits = 1 * 8; + const int indexBits = keyBits - chrBits; + const int64_t chrMask = (int64_t)255 << indexBits; + const int64_t indexMask = ~chrMask; + int agent_id = ((input & chrMask) >> indexBits) - 97; + FrameIdType keyframe_id = input & indexMask; + return std::make_pair(agent_id, keyframe_id); } -std::vector> get_all(fs::path const & root, std::string const & ext) { - std::vector> paths; - if (fs::exists(root) && fs::is_directory(root)) - { - for (auto const & entry : fs::directory_iterator(root)) - { - if (fs::is_regular_file(entry) && entry.path().extension() == ext && is_number(entry.path().stem().string())) - { - // std::cout << "Filename: " << entry.path() << std::endl; - paths.emplace_back( - std::make_pair(std::stoi(entry.path().stem().string()), - entry.path().string())); - } - } +std::vector> get_all(fs::path const& root, + std::string const& ext) { + std::vector> paths; + if (fs::exists(root) && fs::is_directory(root)) { + for (auto const& entry : fs::directory_iterator(root)) { + if (fs::is_regular_file(entry) && entry.path().extension() == ext && + is_number(entry.path().stem().string())) { + // std::cout << "Filename: " << entry.path() << std::endl; + paths.emplace_back(std::make_pair( + std::stoi(entry.path().stem().string()), entry.path().string())); + } } - return paths; -} - - -bool match_vertex_se3(std::string line, int & agent_id, FrameIdType & kf_id, Swarm::Pose & pose, int max_agent_id) { - Eigen::Vector3d pos; - Eigen::Quaterniond quat; - std::smatch sm; - regex_search(line, sm, reg_vertex_se3); - // std::cout << "The matches are:\n"; - // for( int i = 0 ; i < sm.size() ; ++i ){ - // std::cout << i << ": [" << sm[i] << ']' << std::endl; - // } - // std::cout << std::endl; - if (sm.size() > 8) { - auto ret = extrackKeyframeId(std::stoll(sm[1].str())); - kf_id = ret.second; - agent_id = ret.first; - if (agent_id > max_agent_id) { - return false; - } + } + return paths; +} - pos.x() = std::stod(sm[2].str()); - pos.y() = std::stod(sm[3].str()); - pos.z() = std::stod(sm[4].str()); - - quat.x() = std::stod(sm[5].str()); - quat.y() = std::stod(sm[6].str()); - quat.z() = std::stod(sm[7].str()); - quat.w() = std::stod(sm[8].str()); - quat.normalize(); - pose = Swarm::Pose(pos, quat); - return true; +bool match_vertex_se3(std::string line, int& agent_id, FrameIdType& kf_id, + Swarm::Pose& pose, int max_agent_id) { + Eigen::Vector3d pos; + Eigen::Quaterniond quat; + std::smatch sm; + regex_search(line, sm, reg_vertex_se3); + // std::cout << "The matches are:\n"; + // for( int i = 0 ; i < sm.size() ; ++i ){ + // std::cout << i << ": [" << sm[i] << ']' << std::endl; + // } + // std::cout << std::endl; + if (sm.size() > 8) { + auto ret = extrackKeyframeId(std::stoll(sm[1].str())); + kf_id = ret.second; + agent_id = ret.first; + if (agent_id > max_agent_id) { + return false; } - return false; + + pos.x() = std::stod(sm[2].str()); + pos.y() = std::stod(sm[3].str()); + pos.z() = std::stod(sm[4].str()); + + quat.x() = std::stod(sm[5].str()); + quat.y() = std::stod(sm[6].str()); + quat.z() = std::stod(sm[7].str()); + quat.w() = std::stod(sm[8].str()); + quat.normalize(); + pose = Swarm::Pose(pos, quat); + return true; + } + return false; } -bool match_edge_se3(std::string line, int & agent_ida, FrameIdType & ida, int & agent_idb, FrameIdType & idb, Swarm::Pose & pose, Eigen::Matrix6d & information, int max_agent_id) { - Eigen::Vector3d pos; - Eigen::Quaterniond quat; - std::smatch sm; - regex_search(line, sm, reg_edge_se3); - // std::cout << "The matches are:\n"; - // for( int i = 0 ; i < sm.size() ; ++i ){ - // std::cout << i << ": [" << sm[i] << ']' << std::endl; - // } - // std::cout << std::endl; - if (sm.size() > 1) { - std::stringstream stream(sm[1]); - int64_t tmp_a, tmp_b; - stream >> tmp_a >> tmp_b; - auto ret_a = extrackKeyframeId(tmp_a); - agent_ida = ret_a.first; - ida = ret_a.second; - auto ret_b = extrackKeyframeId(tmp_b); - agent_idb = ret_b.first; - idb = ret_b.second; - if (agent_ida > max_agent_id || agent_idb > max_agent_id) { - return false; - } - stream >> pose; - for (int i = 0; i < 6 && stream.good(); ++i) { - for (int j = i; j < 6 && stream.good(); ++j) { - stream >> information(i, j); - if (i != j) { - information(j, i) = information(i, j); - } - } +bool match_edge_se3(std::string line, int& agent_ida, FrameIdType& ida, + int& agent_idb, FrameIdType& idb, Swarm::Pose& pose, + Eigen::Matrix6d& information, int max_agent_id) { + Eigen::Vector3d pos; + Eigen::Quaterniond quat; + std::smatch sm; + regex_search(line, sm, reg_edge_se3); + // std::cout << "The matches are:\n"; + // for( int i = 0 ; i < sm.size() ; ++i ){ + // std::cout << i << ": [" << sm[i] << ']' << std::endl; + // } + // std::cout << std::endl; + if (sm.size() > 1) { + std::stringstream stream(sm[1]); + int64_t tmp_a, tmp_b; + stream >> tmp_a >> tmp_b; + auto ret_a = extrackKeyframeId(tmp_a); + agent_ida = ret_a.first; + ida = ret_a.second; + auto ret_b = extrackKeyframeId(tmp_b); + agent_idb = ret_b.first; + idb = ret_b.second; + if (agent_ida > max_agent_id || agent_idb > max_agent_id) { + return false; + } + stream >> pose; + for (int i = 0; i < 6 && stream.good(); ++i) { + for (int j = i; j < 6 && stream.good(); ++j) { + stream >> information(i, j); + if (i != j) { + information(j, i) = information(i, j); } - return true; + } } - return false; + return true; + } + return false; } - -void read_g2o_agent( std::string path, std::map & keyframeid_agent_pose, - std::vector & edges, bool is_4dof, int max_agent_id, int drone_id, bool ignore_infor) { - std::ifstream infile(path); - std::string line; - while (std::getline(infile, line)) { - // std::cout << "line^" << line << "$" << std::endl; - Swarm::Pose pose; - FrameIdType id_a, id_b; - int agent_id; - auto success = match_vertex_se3(line, agent_id, id_a, pose, max_agent_id); - if (success) { - //Add new vertex here - D2BaseFrame frame; - frame.drone_id = agent_id; - frame.odom.pose() = pose; - frame.initial_ego_pose = pose; - frame.frame_id = id_a; - frame.reference_frame_id = 0; - keyframeid_agent_pose[id_a] = frame; - } else { - Eigen::Matrix6d information; - int agent_id_b; - success = match_edge_se3(line, agent_id, id_a, agent_id_b, id_b, pose, information, max_agent_id); - if (ignore_infor) { - information = Eigen::Matrix6d::Identity(); - } - if (success) { - Swarm::LoopEdge edge(id_a, id_b, pose, information); - edge.id_a = agent_id; - edge.id_b = agent_id_b; - edges.emplace_back(edge); - // std::cout << "line" << line << std::endl; - // printf("Edge drone %d->%d frame %ld->%ld\n", agent_id, agent_id_b, id_a, id_b); - // std::cout << "information:\n" << information << std::endl; - } - } +void read_g2o_agent(std::string path, + std::map& keyframeid_agent_pose, + std::vector& edges, bool is_4dof, + int max_agent_id, int drone_id, bool ignore_infor) { + std::ifstream infile(path); + std::string line; + while (std::getline(infile, line)) { + // std::cout << "line^" << line << "$" << std::endl; + Swarm::Pose pose; + FrameIdType id_a, id_b; + int agent_id; + auto success = match_vertex_se3(line, agent_id, id_a, pose, max_agent_id); + if (success) { + // Add new vertex here + D2BaseFrame frame; + frame.drone_id = agent_id; + frame.odom.pose() = pose; + frame.initial_ego_pose = pose; + frame.frame_id = id_a; + frame.reference_frame_id = 0; + keyframeid_agent_pose[id_a] = frame; + } else { + Eigen::Matrix6d information; + int agent_id_b; + success = match_edge_se3(line, agent_id, id_a, agent_id_b, id_b, pose, + information, max_agent_id); + if (ignore_infor) { + information = Eigen::Matrix6d::Identity(); + } + if (success) { + Swarm::LoopEdge edge(id_a, id_b, pose, information); + edge.id_a = agent_id; + edge.id_b = agent_id_b; + edges.emplace_back(edge); + // std::cout << "line" << line << std::endl; + // printf("Edge drone %d->%d frame %ld->%ld\n", agent_id, agent_id_b, + // id_a, id_b); std::cout << "information:\n" << information << + // std::endl; + } } + } } -void read_g2o_multi_agents(std::string path, - std::map> & keyframeid_agent_pose, - std::map> & edges, G2oParseParam param) { - auto files = get_all(path, ".g2o"); - std::sort(files.begin(), files.end()); - int agent_num = files.size(); - for (unsigned int i = 0; i < files.size(); i++) { - if (i >= param.agents_num) { - break; - } - auto file = files[i].second; - keyframeid_agent_pose[i] = std::map(); - edges[i] = std::vector(); - read_g2o_agent(file, keyframeid_agent_pose[i], edges[i], param.is_4dof, param.agents_num-1); +void read_g2o_multi_agents( + std::string path, + std::map>& keyframeid_agent_pose, + std::map>& edges, G2oParseParam param) { + auto files = get_all(path, ".g2o"); + std::sort(files.begin(), files.end()); + int agent_num = files.size(); + for (unsigned int i = 0; i < files.size(); i++) { + if (i >= param.agents_num) { + break; } - printf("g2o files %ld in path %s keyframe: %ld edges %ld\n", files.size(), path.c_str(), - keyframeid_agent_pose.size(), edges.size()); + auto file = files[i].second; + keyframeid_agent_pose[i] = std::map(); + edges[i] = std::vector(); + read_g2o_agent(file, keyframeid_agent_pose[i], edges[i], param.is_4dof, + param.agents_num - 1); + } + printf("g2o files %ld in path %s keyframe: %ld edges %ld\n", files.size(), + path.c_str(), keyframeid_agent_pose.size(), edges.size()); } -void write_result_to_g2o(const std::string & path, - const std::vector & frames, - const std::vector & edges, - bool write_ego_pose) { - std::fstream file; - file.open(path.c_str(), std::fstream::out); - for (auto & frame : frames) { - Swarm::Pose pose; - if (write_ego_pose) { - pose = frame->initial_ego_pose; - } else { - pose = frame->odom.pose(); - } - auto quat = pose.att(); - auto pos = pose.pos(); - file << "VERTEX_SE3:QUAT " << frame->frame_id << " " << pos.x() << " " << pos.y() << " " << pos.z() << " "; - file << quat.x() << " " << quat.y() << " " << quat.z() << " " << quat.w() << std::endl; +void write_result_to_g2o(const std::string& path, + const std::vector& frames, + const std::vector& edges, + bool write_ego_pose) { + std::fstream file; + file.open(path.c_str(), std::fstream::out); + for (auto& frame : frames) { + Swarm::Pose pose; + if (write_ego_pose) { + pose = frame->initial_ego_pose; + } else { + pose = frame->odom.pose(); } - - for (auto & edge : edges) { - Swarm::Pose pose = edge.relative_pose; - auto info = edge.getInfoMat(); - auto quat = pose.att(); - auto pos = pose.pos(); - file << "EDGE_SE3:QUAT " << edge.keyframe_id_a << " " << edge.keyframe_id_b << " " << pos.x() << " " << pos.y() << " " << pos.z() << " "; - file << quat.x() << " " << quat.y() << " " << quat.z() << " " << quat.w() << " "; - for (int i = 0; i < 6; ++i) { - for (int j = i; j < 6; ++j) { - file << info(i, j) << " "; - } - } - file << std::endl; + auto quat = pose.att(); + auto pos = pose.pos(); + file << "VERTEX_SE3:QUAT " << frame->frame_id << " " << pos.x() << " " + << pos.y() << " " << pos.z() << " "; + file << quat.x() << " " << quat.y() << " " << quat.z() << " " << quat.w() + << std::endl; + } + + for (auto& edge : edges) { + Swarm::Pose pose = edge.relative_pose; + auto info = edge.getInfoMat(); + auto quat = pose.att(); + auto pos = pose.pos(); + file << "EDGE_SE3:QUAT " << edge.keyframe_id_a << " " << edge.keyframe_id_b + << " " << pos.x() << " " << pos.y() << " " << pos.z() << " "; + file << quat.x() << " " << quat.y() << " " << quat.z() << " " << quat.w() + << " "; + for (int i = 0; i < 6; ++i) { + for (int j = i; j < 6; ++j) { + file << info(i, j) << " "; + } } - file.close(); + file << std::endl; + } + file.close(); } -} \ No newline at end of file +} // namespace D2PGO \ No newline at end of file diff --git a/d2pgo/test/test_pgo.cpp b/d2pgo/test/test_pgo.cpp index e7ff6b9c..4eee153b 100644 --- a/d2pgo/test/test_pgo.cpp +++ b/d2pgo/test/test_pgo.cpp @@ -1,34 +1,42 @@ +#include +#include + +#include + +#include "../src/d2pgo.h" #include "d2pgo_test.hpp" #include "posegraph_g2o.hpp" -#include "../src/d2pgo.h" -#include -#include -#include using namespace D2PGO; void testDummy() { - D2PGOConfig config; - config.self_id = 0; - config.main_id = 0; - config.loop_distance_threshold = 10000; - D2PGO::D2PGO * pgo = new D2PGO::D2PGO(config); - D2BaseFrame frame0(0.0, 0, 0, 0, true, Swarm::Pose::Identity()); - D2BaseFrame frame1(1.0, 1, 0, 0, true, Swarm::Pose(Vector3d(1, 0, 0), Quaterniond(1, 0, 0, 0))); - D2BaseFrame frame2(2.0, 2, 0, 0, true, Swarm::Pose(Vector3d(2, 0, 0), Quaterniond(1, 0, 0, 0))); - Swarm::LoopEdge edge(0, 1, Swarm::Pose(Vector3d(1, 0, 0), Quaterniond(1, 0, 0, 0)), Eigen::Matrix6d::Identity()); - Swarm::LoopEdge edge1(1, 2, Swarm::Pose(Vector3d(1, 0, 0), Quaterniond(1, 0, 0, 0)), Eigen::Matrix6d::Identity()); - Swarm::LoopEdge edge2(0, 2, Swarm::Pose(Vector3d(2, 0, 0), Quaterniond(1, 0, 0, 0)), Eigen::Matrix6d::Identity()); - std::vector loops{edge, edge1, edge2}; - pgo->addFrame(frame0); - pgo->addFrame(frame1); - pgo->addFrame(frame2); - pgo->addLoop(edge, false); - pgo->addLoop(edge1, false); - pgo->addLoop(edge2, false); - pgo->rotInitial(loops); + D2PGOConfig config; + config.self_id = 0; + config.main_id = 0; + config.loop_distance_threshold = 10000; + D2PGO::D2PGO* pgo = new D2PGO::D2PGO(config); + D2BaseFrame frame0(0.0, 0, 0, 0, true, Swarm::Pose::Identity()); + D2BaseFrame frame1(1.0, 1, 0, 0, true, + Swarm::Pose(Vector3d(1, 0, 0), Quaterniond(1, 0, 0, 0))); + D2BaseFrame frame2(2.0, 2, 0, 0, true, + Swarm::Pose(Vector3d(2, 0, 0), Quaterniond(1, 0, 0, 0))); + Swarm::LoopEdge edge(0, 1, + Swarm::Pose(Vector3d(1, 0, 0), Quaterniond(1, 0, 0, 0)), + Eigen::Matrix6d::Identity()); + Swarm::LoopEdge edge1(1, 2, + Swarm::Pose(Vector3d(1, 0, 0), Quaterniond(1, 0, 0, 0)), + Eigen::Matrix6d::Identity()); + Swarm::LoopEdge edge2(0, 2, + Swarm::Pose(Vector3d(2, 0, 0), Quaterniond(1, 0, 0, 0)), + Eigen::Matrix6d::Identity()); + std::vector loops{edge, edge1, edge2}; + pgo->addFrame(frame0); + pgo->addFrame(frame1); + pgo->addFrame(frame2); + pgo->addLoop(edge, false); + pgo->addLoop(edge1, false); + pgo->addLoop(edge2, false); + pgo->rotInitial(loops); } -int main(int argc, char ** argv) { - testDummy(); -} \ No newline at end of file +int main(int argc, char** argv) { testDummy(); } \ No newline at end of file diff --git a/d2pgo/third_party/fast_max-clique_finder/src/findClique.cpp b/d2pgo/third_party/fast_max-clique_finder/src/findClique.cpp old mode 100755 new mode 100644 index 9d139810..4d5caaeb --- a/d2pgo/third_party/fast_max-clique_finder/src/findClique.cpp +++ b/d2pgo/third_party/fast_max-clique_finder/src/findClique.cpp @@ -1,21 +1,27 @@ -/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ -/* Description: a library for finding the maximum clique of a graph */ +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * * * * * * */ +/* Description: a library for finding the maximum clique of a graph + */ /* */ /* */ -/* Authors: Bharath Pattabiraman and Md. Mostofa Ali Patwary */ -/* EECS Department, Northwestern University */ -/* email: {bpa342,mpatwary}@eecs.northwestern.edu */ +/* Authors: Bharath Pattabiraman and Md. Mostofa Ali Patwary */ +/* EECS Department, Northwestern University */ +/* email: {bpa342,mpatwary}@eecs.northwestern.edu */ /* */ -/* Copyright, 2014, Northwestern University */ -/* See COPYRIGHT notice in top-level directory. */ +/* Copyright, 2014, Northwestern University */ +/* See COPYRIGHT notice in top-level directory. */ /* */ -/* Please site the following publication if you use this package: */ -/* Bharath Pattabiraman, Md. Mostofa Ali Patwary, Assefaw H. Gebremedhin2, */ -/* Wei-keng Liao, and Alok Choudhary. */ -/* "Fast Algorithms for the Maximum Clique Problem on Massive Graphs with */ -/* Applications to Overlapping Community Detection" */ -/* http://arxiv.org/abs/1411.7460 */ -/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ +/* Please site the following publication if you use this package: */ +/* Bharath Pattabiraman, Md. Mostofa Ali Patwary, Assefaw H. Gebremedhin2, + */ +/* Wei-keng Liao, and Alok Choudhary. */ +/* "Fast Algorithms for the Maximum Clique Problem on Massive Graphs with */ +/* Applications to Overlapping Community Detection" + */ +/* http://arxiv.org/abs/1411.7460 + */ +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * * * * * * */ #include "findClique.h" @@ -26,138 +32,122 @@ int pruned3; int pruned5; /* Algorithm 2: CLIQUE: Recursive Subroutine of algorithm 1. */ -void maxCliqueHelper( CGraphIO& gio, vector* U, int sizeOfClique, int& maxClq, vector& max_clique_data_inter ) -{ - int iPos, index = 0, maxClq_prev; - vector * ptrVertex = gio.GetVerticesPtr(); - vector * ptrEdge = gio.GetEdgesPtr(); - vector U_new; - U_new.reserve(gio.GetVertexCount()); - - if( U->size() == 0 ) - { - if( sizeOfClique > maxClq ) - { - maxClq = sizeOfClique; - max_clique_data_inter.clear(); - } - return; - } - - while( U->size() > 0 ) - { - //Old Pruning - if( sizeOfClique + U->size() <= maxClq ) - return; - - index = U->back(); - U->pop_back(); - - // Loop over neighbrs of v_index. - for(int j = (*ptrVertex)[index]; j < (*ptrVertex)[index + 1]; j++ ) - //Pruning 5 - if( getDegree(ptrVertex, (*ptrEdge)[j]) >= maxClq ) - { - // Loop over U. - for(int i = 0; i < U->size(); i++) - { - if( (*ptrEdge)[j] == (*U)[i] ) - U_new.push_back( (*ptrEdge)[j]); - } - } - else - pruned3++; - - maxClq_prev = maxClq; - - maxCliqueHelper( gio, &U_new, sizeOfClique + 1, maxClq, max_clique_data_inter ); - - if(maxClq > maxClq_prev) - max_clique_data_inter.push_back(index); - - U_new.clear(); - } +void maxCliqueHelper(CGraphIO& gio, vector* U, int sizeOfClique, + int& maxClq, vector& max_clique_data_inter) { + int iPos, index = 0, maxClq_prev; + vector* ptrVertex = gio.GetVerticesPtr(); + vector* ptrEdge = gio.GetEdgesPtr(); + vector U_new; + U_new.reserve(gio.GetVertexCount()); + + if (U->size() == 0) { + if (sizeOfClique > maxClq) { + maxClq = sizeOfClique; + max_clique_data_inter.clear(); + } + return; + } + + while (U->size() > 0) { + // Old Pruning + if (sizeOfClique + U->size() <= maxClq) return; + + index = U->back(); + U->pop_back(); + + // Loop over neighbrs of v_index. + for (int j = (*ptrVertex)[index]; j < (*ptrVertex)[index + 1]; j++) + // Pruning 5 + if (getDegree(ptrVertex, (*ptrEdge)[j]) >= maxClq) { + // Loop over U. + for (int i = 0; i < U->size(); i++) { + if ((*ptrEdge)[j] == (*U)[i]) U_new.push_back((*ptrEdge)[j]); + } + } else + pruned3++; + + maxClq_prev = maxClq; + + maxCliqueHelper(gio, &U_new, sizeOfClique + 1, maxClq, + max_clique_data_inter); + + if (maxClq > maxClq_prev) max_clique_data_inter.push_back(index); + + U_new.clear(); + } } /* Algorithm 1: MAXCLIQUE: Finds maximum clique of the given graph */ -int maxClique( CGraphIO& gio, int l_bound, vector& max_clique_data ) -{ - vector * ptrVertex = gio.GetVerticesPtr(); - vector * ptrEdge = gio.GetEdgesPtr(); - vector U; - U.reserve(gio.GetVertexCount()); - vector max_clique_data_inter; - max_clique_data_inter.reserve(gio.GetVertexCount()); - max_clique_data.reserve(gio.GetVertexCount()); - int maxClq = l_bound; - int prev_maxClq; - - //cout << "Computing Max Clique... with lower bound " << maxClq << endl; - pruned1 = 0; - pruned2 = 0; - pruned3 = 0; - pruned5 = 0; - - //Bit Vector to track if vertex has been considered previously. - int *bitVec = new int[gio.GetVertexCount()]; - memset(bitVec, 0, gio.GetVertexCount() * sizeof(int)); - - for(int i = gio.GetVertexCount()-1; i >= 0; i--) - { - bitVec[i] = 1; - prev_maxClq = maxClq; - - U.clear(); - //Pruning 1 - if( getDegree(ptrVertex, i) < maxClq) - { - pruned1++; - continue; - } - - for( int j = (*ptrVertex)[i]; j < (*ptrVertex)[i + 1]; j++ ) - { - //Pruning 2 - if(bitVec[(*ptrEdge)[j]] != 1) - { - //Pruning 3 - if( getDegree(ptrVertex, (*ptrEdge)[j]) >= maxClq ) - U.push_back((*ptrEdge)[j]); - else - pruned3++; - } - else - pruned2++; - } - - maxCliqueHelper( gio, &U, 1, maxClq, max_clique_data_inter ); - - if(maxClq > prev_maxClq) - { - max_clique_data_inter.push_back(i); - max_clique_data = max_clique_data_inter; - } - max_clique_data_inter.clear(); - } - - delete [] bitVec; - max_clique_data_inter.clear(); +int maxClique(CGraphIO& gio, int l_bound, vector& max_clique_data) { + vector* ptrVertex = gio.GetVerticesPtr(); + vector* ptrEdge = gio.GetEdgesPtr(); + vector U; + U.reserve(gio.GetVertexCount()); + vector max_clique_data_inter; + max_clique_data_inter.reserve(gio.GetVertexCount()); + max_clique_data.reserve(gio.GetVertexCount()); + int maxClq = l_bound; + int prev_maxClq; + + // cout << "Computing Max Clique... with lower bound " << maxClq << endl; + pruned1 = 0; + pruned2 = 0; + pruned3 = 0; + pruned5 = 0; + + // Bit Vector to track if vertex has been considered previously. + int* bitVec = new int[gio.GetVertexCount()]; + memset(bitVec, 0, gio.GetVertexCount() * sizeof(int)); + + for (int i = gio.GetVertexCount() - 1; i >= 0; i--) { + bitVec[i] = 1; + prev_maxClq = maxClq; + + U.clear(); + // Pruning 1 + if (getDegree(ptrVertex, i) < maxClq) { + pruned1++; + continue; + } + + for (int j = (*ptrVertex)[i]; j < (*ptrVertex)[i + 1]; j++) { + // Pruning 2 + if (bitVec[(*ptrEdge)[j]] != 1) { + // Pruning 3 + if (getDegree(ptrVertex, (*ptrEdge)[j]) >= maxClq) + U.push_back((*ptrEdge)[j]); + else + pruned3++; + } else + pruned2++; + } + + maxCliqueHelper(gio, &U, 1, maxClq, max_clique_data_inter); + + if (maxClq > prev_maxClq) { + max_clique_data_inter.push_back(i); + max_clique_data = max_clique_data_inter; + } + max_clique_data_inter.clear(); + } + + delete[] bitVec; + max_clique_data_inter.clear(); #ifdef _DEBUG - cout << "Pruning 1 = " << pruned1 << endl; - cout << "Pruning 2 = " << pruned2 << endl; - cout << "Pruning 3 = " << pruned3 << endl; - cout << "Pruning 5 = " << pruned5 << endl; + cout << "Pruning 1 = " << pruned1 << endl; + cout << "Pruning 2 = " << pruned2 << endl; + cout << "Pruning 3 = " << pruned3 << endl; + cout << "Pruning 5 = " << pruned5 << endl; #endif - return maxClq; + return maxClq; } -void print_max_clique(vector& max_clique_data) -{ - //cout << "Maximum clique: "; - for(int i = 0; i < max_clique_data.size(); i++) - cout << max_clique_data[i] + 1 << " "; - cout << endl; +void print_max_clique(vector& max_clique_data) { + // cout << "Maximum clique: "; + for (int i = 0; i < max_clique_data.size(); i++) + cout << max_clique_data[i] + 1 << " "; + cout << endl; } -} \ No newline at end of file +} // namespace FMC \ No newline at end of file diff --git a/d2pgo/third_party/fast_max-clique_finder/src/findCliqueHeu.cpp b/d2pgo/third_party/fast_max-clique_finder/src/findCliqueHeu.cpp old mode 100755 new mode 100644 index 8979e065..1a262ed4 --- a/d2pgo/third_party/fast_max-clique_finder/src/findCliqueHeu.cpp +++ b/d2pgo/third_party/fast_max-clique_finder/src/findCliqueHeu.cpp @@ -1,257 +1,256 @@ -/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ -/* Description: a library for finding the maximum clique from a graph */ +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * * * * * * */ +/* Description: a library for finding the maximum clique from a graph + */ /* */ /* */ -/* Authors: Md. Mostofa Ali Patwary and Bharath Pattabiraman */ -/* EECS Department, Northwestern University */ -/* email: {mpatwary,bpa342}@eecs.northwestern.edu */ +/* Authors: Md. Mostofa Ali Patwary and Bharath Pattabiraman */ +/* EECS Department, Northwestern University */ +/* email: {mpatwary,bpa342}@eecs.northwestern.edu */ /* */ -/* Copyright, 2014, Northwestern University */ -/* See COPYRIGHT notice in top-level directory. */ +/* Copyright, 2014, Northwestern University */ +/* See COPYRIGHT notice in top-level directory. */ /* */ -/* Please site the following publication if you use this package: */ -/* Bharath Pattabiraman, Md. Mostofa Ali Patwary, Assefaw H. Gebremedhin2, */ -/* Wei-keng Liao, and Alok Choudhary. */ -/* "Fast Algorithms for the Maximum Clique Problem on Massive Graphs with */ -/* Applications to Overlapping Community Detection" */ -/* http://arxiv.org/abs/1411.7460 */ -/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ +/* Please site the following publication if you use this package: */ +/* Bharath Pattabiraman, Md. Mostofa Ali Patwary, Assefaw H. Gebremedhin2, + */ +/* Wei-keng Liao, and Alok Choudhary. */ +/* "Fast Algorithms for the Maximum Clique Problem on Massive Graphs with */ +/* Applications to Overlapping Community Detection" + */ +/* http://arxiv.org/abs/1411.7460 + */ +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * * * * * * */ -#include "findClique.h" -#include +#include #include #include -#include + +#include + +#include "findClique.h" namespace FMC { int maxDegree, maxClq; /* Algorithm 2: MaxCliqueHeu: A heuristic to find maximum clique */ -int maxCliqueHeu(CGraphIO& gio) -{ - vector * p_v_i_Vertices = gio.GetVerticesPtr(); - vector * p_v_i_Edges = gio.GetEdgesPtr(); - - //srand(time(NULL)); - - maxDegree = gio.GetMaximumVertexDegree(); - int maxClq = - 1, u, icc; - vector < int > v_i_S; - vector < int > v_i_S1; - v_i_S.resize(maxDegree + 1, 0); - v_i_S1.resize(maxDegree + 1, 0); - - int iPos, iPos1, iCount, iCount1; - - int notComputed = 0; - - // compute the max clique for each vertex - for(int iCandidateVertex = 0; iCandidateVertex < p_v_i_Vertices->size() - 1; iCandidateVertex++) - { - // Pruning 1 - if(maxClq > ((*p_v_i_Vertices)[iCandidateVertex + 1] - (*p_v_i_Vertices)[iCandidateVertex])) - { - notComputed++; - continue; - } - - iPos = 0; - v_i_S[iPos++] = iCandidateVertex; - int z, imdv = iCandidateVertex, imd = (*p_v_i_Vertices)[iCandidateVertex + 1] - (*p_v_i_Vertices)[iCandidateVertex]; - - int iLoopCount = (*p_v_i_Vertices)[iCandidateVertex + 1]; - for(int j = (*p_v_i_Vertices)[iCandidateVertex]; j < iLoopCount; j++) - { - // Pruning 3 - if(maxClq <= ((*p_v_i_Vertices)[(*p_v_i_Edges)[j] + 1] - (*p_v_i_Vertices)[(*p_v_i_Edges)[j]])) - v_i_S[iPos++] = (*p_v_i_Edges)[j]; - } - - icc = 0; - - while(iPos > 0) - { - int imdv1 = -1, imd1 = -1; - - icc++; - - // generate a random number x from 0 to iPos -1 - // aasign that imdv = x and imd = d(x) - //imdv = v_i_S[rand() % iPos]; - imdv = v_i_S[iPos-1]; - imd = (*p_v_i_Vertices)[imdv + 1] - (*p_v_i_Vertices)[imdv]; - - iPos1 = 0; - - for(int j = 0; j < iPos; j++) - { - iLoopCount = (*p_v_i_Vertices)[imdv + 1]; - for(int k = (*p_v_i_Vertices)[imdv]; k < iLoopCount; k++) - { - // Pruning 5 - if(v_i_S[j] == (*p_v_i_Edges)[k] && maxClq <= ((*p_v_i_Vertices)[(*p_v_i_Edges)[k] + 1] - (*p_v_i_Vertices)[(*p_v_i_Edges)[k]])) - { - v_i_S1[iPos1++] = v_i_S[j]; // calculate the max degree vertex here - - break; - } - } - } - - for(int j = 0; j < iPos1; j++) - v_i_S[j] = v_i_S1[j]; - - iPos = iPos1; - - imdv = imdv1; - imd = imd1; - } - - if(maxClq < icc) - maxClq = icc; - - } - - return maxClq; +int maxCliqueHeu(CGraphIO& gio) { + vector* p_v_i_Vertices = gio.GetVerticesPtr(); + vector* p_v_i_Edges = gio.GetEdgesPtr(); + + // srand(time(NULL)); + + maxDegree = gio.GetMaximumVertexDegree(); + int maxClq = -1, u, icc; + vector v_i_S; + vector v_i_S1; + v_i_S.resize(maxDegree + 1, 0); + v_i_S1.resize(maxDegree + 1, 0); + + int iPos, iPos1, iCount, iCount1; + + int notComputed = 0; + + // compute the max clique for each vertex + for (int iCandidateVertex = 0; iCandidateVertex < p_v_i_Vertices->size() - 1; + iCandidateVertex++) { + // Pruning 1 + if (maxClq > ((*p_v_i_Vertices)[iCandidateVertex + 1] - + (*p_v_i_Vertices)[iCandidateVertex])) { + notComputed++; + continue; + } + + iPos = 0; + v_i_S[iPos++] = iCandidateVertex; + int z, imdv = iCandidateVertex, + imd = (*p_v_i_Vertices)[iCandidateVertex + 1] - + (*p_v_i_Vertices)[iCandidateVertex]; + + int iLoopCount = (*p_v_i_Vertices)[iCandidateVertex + 1]; + for (int j = (*p_v_i_Vertices)[iCandidateVertex]; j < iLoopCount; j++) { + // Pruning 3 + if (maxClq <= ((*p_v_i_Vertices)[(*p_v_i_Edges)[j] + 1] - + (*p_v_i_Vertices)[(*p_v_i_Edges)[j]])) + v_i_S[iPos++] = (*p_v_i_Edges)[j]; + } + + icc = 0; + + while (iPos > 0) { + int imdv1 = -1, imd1 = -1; + + icc++; + + // generate a random number x from 0 to iPos -1 + // aasign that imdv = x and imd = d(x) + // imdv = v_i_S[rand() % iPos]; + imdv = v_i_S[iPos - 1]; + imd = (*p_v_i_Vertices)[imdv + 1] - (*p_v_i_Vertices)[imdv]; + + iPos1 = 0; + + for (int j = 0; j < iPos; j++) { + iLoopCount = (*p_v_i_Vertices)[imdv + 1]; + for (int k = (*p_v_i_Vertices)[imdv]; k < iLoopCount; k++) { + // Pruning 5 + if (v_i_S[j] == (*p_v_i_Edges)[k] && + maxClq <= ((*p_v_i_Vertices)[(*p_v_i_Edges)[k] + 1] - + (*p_v_i_Vertices)[(*p_v_i_Edges)[k]])) { + v_i_S1[iPos1++] = v_i_S[j]; // calculate the max degree vertex here + + break; + } + } + } + + for (int j = 0; j < iPos1; j++) v_i_S[j] = v_i_S1[j]; + + iPos = iPos1; + + imdv = imdv1; + imd = imd1; + } + + if (maxClq < icc) maxClq = icc; + } + + return maxClq; } /* Algorithm 2: MaxCliqueHeu: A heuristic to find maximum clique */ /* Modified by Josh Mangelson to return possible Max clique */ -int maxCliqueHeu(CGraphIO& gio, vector& max_clique_data) -{ - vector * p_v_i_Vertices = gio.GetVerticesPtr(); - vector * p_v_i_Edges = gio.GetEdgesPtr(); - - vector max_clique_data_inter; - max_clique_data_inter.reserve(gio.GetVertexCount()); - max_clique_data.reserve(gio.GetVertexCount()); - - //srand(time(NULL)); - - maxDegree = gio.GetMaximumVertexDegree(); - int maxClq = - 1, u, icc; - vector < int > v_i_S; - vector < int > v_i_S1; - v_i_S.resize(maxDegree + 1, 0); - v_i_S1.resize(maxDegree + 1, 0); - - int iPos, iPos1, iCount, iCount1; - - int notComputed = 0; - - // compute the max clique for each vertex - for(int iCandidateVertex = 0; iCandidateVertex < p_v_i_Vertices->size() - 1; iCandidateVertex++) - { - max_clique_data_inter.clear (); - - // std::cout << iCandidateVertex << " ---------------\n"; - // Pruning 1 - if(maxClq > ((*p_v_i_Vertices)[iCandidateVertex + 1] - (*p_v_i_Vertices)[iCandidateVertex])) - { - notComputed++; - continue; - } - - iPos = 0; - v_i_S[iPos++] = iCandidateVertex; - int z, imdv = iCandidateVertex, imd = (*p_v_i_Vertices)[iCandidateVertex + 1] - (*p_v_i_Vertices)[iCandidateVertex]; - - int iLoopCount = (*p_v_i_Vertices)[iCandidateVertex + 1]; - for(int j = (*p_v_i_Vertices)[iCandidateVertex]; j < iLoopCount; j++) - { - // Pruning 3 - if(maxClq <= ((*p_v_i_Vertices)[(*p_v_i_Edges)[j] + 1] - (*p_v_i_Vertices)[(*p_v_i_Edges)[j]])) - v_i_S[iPos++] = (*p_v_i_Edges)[j]; - } - - // std::cout << iPos << std::endl; - // for(int x = 0; x < v_i_S.size(); x++) - // std::cout << v_i_S[x] << ", "; - // std::cout << std::endl << "------------" << std::endl; - - - icc = 0; - max_clique_data_inter.push_back(iCandidateVertex); - - while(iPos > 0) - { - int imdv1 = -1, imd1 = -1; - - icc++; - - // generate a random number x from 0 to iPos -1 - // aasign that imdv = x and imd = d(x) - //imdv = v_i_S[rand() % iPos]; - imdv = v_i_S[iPos-1]; - imd = (*p_v_i_Vertices)[imdv + 1] - (*p_v_i_Vertices)[imdv]; - - iPos1 = 0; - - for(int j = 0; j < iPos; j++) - { - iLoopCount = (*p_v_i_Vertices)[imdv + 1]; - for(int k = (*p_v_i_Vertices)[imdv]; k < iLoopCount; k++) - { - // Pruning 5 - if(v_i_S[j] == (*p_v_i_Edges)[k] && maxClq <= ((*p_v_i_Vertices)[(*p_v_i_Edges)[k] + 1] - (*p_v_i_Vertices)[(*p_v_i_Edges)[k]])) - { - v_i_S1[iPos1++] = v_i_S[j]; // calculate the max degree vertex here - - break; - } - } - } - - - // for(int x = 0; x < v_i_S.size(); x++) - // std::cout << v_i_S[x] << ", "; - // std::cout << std::endl; - // std::cout << "v_i_S1: "; - // for(int x = 0; x < iPos1; x++) - // std::cout << v_i_S1[x] << ", "; - // std::cout << std::endl; - // std::cout << imdv << ", " << imd << std::endl; - // std::cout << iPos << ", " << iPos1 << std::endl; - - for(int j = 0; j < iPos1; j++) - v_i_S[j] = v_i_S1[j]; - - if (iPos1 != 0) { - // max_clique_data_inter.push_back(iPos); - max_clique_data_inter.push_back(imdv);; - } - - // for(int x=0; x < max_clique_data_inter.size();x++){ - // std::cout << max_clique_data_inter[x] << ", "; - // } - // std::cout << std::endl; - - - iPos = iPos1; - - imdv = imdv1; - imd = imd1; - } - - if(maxClq < icc){ - maxClq = icc; - max_clique_data = max_clique_data_inter; +int maxCliqueHeu(CGraphIO& gio, vector& max_clique_data) { + vector* p_v_i_Vertices = gio.GetVerticesPtr(); + vector* p_v_i_Edges = gio.GetEdgesPtr(); + + vector max_clique_data_inter; + max_clique_data_inter.reserve(gio.GetVertexCount()); + max_clique_data.reserve(gio.GetVertexCount()); + + // srand(time(NULL)); + + maxDegree = gio.GetMaximumVertexDegree(); + int maxClq = -1, u, icc; + vector v_i_S; + vector v_i_S1; + v_i_S.resize(maxDegree + 1, 0); + v_i_S1.resize(maxDegree + 1, 0); + + int iPos, iPos1, iCount, iCount1; + + int notComputed = 0; + + // compute the max clique for each vertex + for (int iCandidateVertex = 0; iCandidateVertex < p_v_i_Vertices->size() - 1; + iCandidateVertex++) { + max_clique_data_inter.clear(); + + // std::cout << iCandidateVertex << " ---------------\n"; + // Pruning 1 + if (maxClq > ((*p_v_i_Vertices)[iCandidateVertex + 1] - + (*p_v_i_Vertices)[iCandidateVertex])) { + notComputed++; + continue; + } + + iPos = 0; + v_i_S[iPos++] = iCandidateVertex; + int z, imdv = iCandidateVertex, + imd = (*p_v_i_Vertices)[iCandidateVertex + 1] - + (*p_v_i_Vertices)[iCandidateVertex]; + + int iLoopCount = (*p_v_i_Vertices)[iCandidateVertex + 1]; + for (int j = (*p_v_i_Vertices)[iCandidateVertex]; j < iLoopCount; j++) { + // Pruning 3 + if (maxClq <= ((*p_v_i_Vertices)[(*p_v_i_Edges)[j] + 1] - + (*p_v_i_Vertices)[(*p_v_i_Edges)[j]])) + v_i_S[iPos++] = (*p_v_i_Edges)[j]; + } + + // std::cout << iPos << std::endl; + // for(int x = 0; x < v_i_S.size(); x++) + // std::cout << v_i_S[x] << ", "; + // std::cout << std::endl << "------------" << std::endl; + + icc = 0; + max_clique_data_inter.push_back(iCandidateVertex); + + while (iPos > 0) { + int imdv1 = -1, imd1 = -1; + + icc++; + + // generate a random number x from 0 to iPos -1 + // aasign that imdv = x and imd = d(x) + // imdv = v_i_S[rand() % iPos]; + imdv = v_i_S[iPos - 1]; + imd = (*p_v_i_Vertices)[imdv + 1] - (*p_v_i_Vertices)[imdv]; + + iPos1 = 0; + + for (int j = 0; j < iPos; j++) { + iLoopCount = (*p_v_i_Vertices)[imdv + 1]; + for (int k = (*p_v_i_Vertices)[imdv]; k < iLoopCount; k++) { + // Pruning 5 + if (v_i_S[j] == (*p_v_i_Edges)[k] && + maxClq <= ((*p_v_i_Vertices)[(*p_v_i_Edges)[k] + 1] - + (*p_v_i_Vertices)[(*p_v_i_Edges)[k]])) { + v_i_S1[iPos1++] = v_i_S[j]; // calculate the max degree vertex here + + break; + } } - } - - return maxClq; + } + + // for(int x = 0; x < v_i_S.size(); x++) + // std::cout << v_i_S[x] << ", "; + // std::cout << std::endl; + // std::cout << "v_i_S1: "; + // for(int x = 0; x < iPos1; x++) + // std::cout << v_i_S1[x] << ", "; + // std::cout << std::endl; + // std::cout << imdv << ", " << imd << std::endl; + // std::cout << iPos << ", " << iPos1 << std::endl; + + for (int j = 0; j < iPos1; j++) v_i_S[j] = v_i_S1[j]; + + if (iPos1 != 0) { + // max_clique_data_inter.push_back(iPos); + max_clique_data_inter.push_back(imdv); + ; + } + + // for(int x=0; x < max_clique_data_inter.size();x++){ + // std::cout << max_clique_data_inter[x] << ", "; + // } + // std::cout << std::endl; + + iPos = iPos1; + + imdv = imdv1; + imd = imd1; + } + + if (maxClq < icc) { + maxClq = icc; + max_clique_data = max_clique_data_inter; + } + } + + return maxClq; } template -vector sort_indexes(const vector &v) -{ +vector sort_indexes(const vector& v) { vector idx(v.size()); iota(idx.begin(), idx.end(), 0); sort(idx.begin(), idx.end(), - [&v](size_t i1, size_t i2) {return v[i1] > v[i2];}); + [&v](size_t i1, size_t i2) { return v[i1] > v[i2]; }); return idx; } -} \ No newline at end of file +} // namespace FMC \ No newline at end of file diff --git a/d2pgo/third_party/fast_max-clique_finder/src/findCliqueHeuInc.cpp b/d2pgo/third_party/fast_max-clique_finder/src/findCliqueHeuInc.cpp index ebb30e32..ef298ba3 100644 --- a/d2pgo/third_party/fast_max-clique_finder/src/findCliqueHeuInc.cpp +++ b/d2pgo/third_party/fast_max-clique_finder/src/findCliqueHeuInc.cpp @@ -1,15 +1,16 @@ // This file is modified from the original file at: // https://github.com/MIT-SPARK/Kimera-RPGO/blob/master/include/KimeraRPGO/max_clique_finder/findCliqueHeu.cpp -#include "findClique.h" -#include +#include #include #include -#include + +#include + +#include "findClique.h" namespace FMC { -int maxCliqueHeuIncremental(CGraphIO& gio, - size_t num_new_lc, +int maxCliqueHeuIncremental(CGraphIO& gio, size_t num_new_lc, size_t prev_maxclique_size, - vector & max_clique_data) { + vector& max_clique_data) { vector* p_v_i_Vertices = gio.GetVerticesPtr(); vector* p_v_i_Edges = gio.GetEdgesPtr(); // srand(time(NULL)); @@ -29,8 +30,7 @@ int maxCliqueHeuIncremental(CGraphIO& gio, // compute the max clique for each vertex // TODO tricky indexing ... for (size_t iCandidateVertex = p_v_i_Vertices->size() - num_new_lc - 1; - iCandidateVertex < p_v_i_Vertices->size() - 1; - iCandidateVertex++) { + iCandidateVertex < p_v_i_Vertices->size() - 1; iCandidateVertex++) { // Pruning 1 if (maxClq > ((*p_v_i_Vertices)[iCandidateVertex + 1] - (*p_v_i_Vertices)[iCandidateVertex])) { @@ -97,4 +97,4 @@ int maxCliqueHeuIncremental(CGraphIO& gio, return maxClq; } -} \ No newline at end of file +} // namespace FMC \ No newline at end of file diff --git a/d2pgo/third_party/fast_max-clique_finder/src/graphIO.cpp b/d2pgo/third_party/fast_max-clique_finder/src/graphIO.cpp old mode 100755 new mode 100644 index e86b31e1..51647b6c --- a/d2pgo/third_party/fast_max-clique_finder/src/graphIO.cpp +++ b/d2pgo/third_party/fast_max-clique_finder/src/graphIO.cpp @@ -1,258 +1,242 @@ -/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ -/* Description: an I/O library for reading a graph */ +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * * * * * * */ +/* Description: an I/O library for reading a graph + */ /* */ /* */ -/* Authors: Md. Mostofa Ali Patwary and Bharath Pattabiraman */ -/* EECS Department, Northwestern University */ -/* email: {mpatwary,bpa342}@eecs.northwestern.edu */ +/* Authors: Md. Mostofa Ali Patwary and Bharath Pattabiraman */ +/* EECS Department, Northwestern University */ +/* email: {mpatwary,bpa342}@eecs.northwestern.edu */ /* */ -/* Copyright, 2014, Northwestern University */ -/* See COPYRIGHT notice in top-level directory. */ +/* Copyright, 2014, Northwestern University */ +/* See COPYRIGHT notice in top-level directory. */ /* */ -/* Please site the following publication if you use this package: */ -/* Bharath Pattabiraman, Md. Mostofa Ali Patwary, Assefaw H. Gebremedhin2, */ -/* Wei-keng Liao, and Alok Choudhary. */ -/* "Fast Algorithms for the Maximum Clique Problem on Massive Graphs with */ -/* Applications to Overlapping Community Detection" */ -/* http://arxiv.org/abs/1411.7460 */ -/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ +/* Please site the following publication if you use this package: */ +/* Bharath Pattabiraman, Md. Mostofa Ali Patwary, Assefaw H. Gebremedhin2, + */ +/* Wei-keng Liao, and Alok Choudhary. */ +/* "Fast Algorithms for the Maximum Clique Problem on Massive Graphs with */ +/* Applications to Overlapping Community Detection" + */ +/* http://arxiv.org/abs/1411.7460 + */ +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * * * * * * */ #include "graphIO.h" namespace FMC { -CGraphIO::~CGraphIO() -{ - m_vi_Vertices.clear(); - m_vi_OrderedVertices.clear(); - m_vi_Edges.clear(); - m_vd_Values.clear(); +CGraphIO::~CGraphIO() { + m_vi_Vertices.clear(); + m_vi_OrderedVertices.clear(); + m_vi_Edges.clear(); + m_vd_Values.clear(); } -bool CGraphIO::readGraph(string s_InputFile, float connStrength) -{ - string fileExtension = getFileExtension(s_InputFile); - if(fileExtension == "mtx") - { - // matrix market format - return ReadMatrixMarketAdjacencyGraph(s_InputFile, connStrength); - } - else if(fileExtension == "gr") - { - // gr format - return ReadMeTiSAdjacencyGraph(s_InputFile); - } - else - return false; +bool CGraphIO::readGraph(string s_InputFile, float connStrength) { + string fileExtension = getFileExtension(s_InputFile); + if (fileExtension == "mtx") { + // matrix market format + return ReadMatrixMarketAdjacencyGraph(s_InputFile, connStrength); + } else if (fileExtension == "gr") { + // gr format + return ReadMeTiSAdjacencyGraph(s_InputFile); + } else + return false; } -bool CGraphIO::ReadMatrixMarketAdjacencyGraph(string s_InputFile, float connStrength) -{ - istringstream in2; - string line=""; - map > nodeList; - map > valueList; - int col=0, row=0, rowIndex=0, colIndex=0; - int entry_counter = 0, num_of_entries = 0; - double value; - - ifstream in (s_InputFile.c_str()); - if(!in) - { - cout<0&&line[0]=='%') //ignore comment line - getline(in,line); - - in2.str(line); - in2 >> row >> col >> num_of_entries; - - if(row!=col) - { - cout<<"* WARNING: GraphInputOutput::ReadMatrixMarketAdjacencyGraph()"<> rowIndex >> colIndex >> value; - rowIndex--; - colIndex--; - - if(rowIndex < 0 || rowIndex >= row) - cout << "Something wrong rowIndex " << rowIndex << " row " << row << endl; - - if(colIndex < 0 || colIndex >= col) - cout << "Something wrong colIndex " << colIndex << " col " << col << endl; - - if(rowIndex == colIndex) - { - continue; - } - - // This is to handle directed graphs. If the edge is already present, skip. If not add. - int exists=0; - for(int k=0; k connStrength) - { - nodeList[rowIndex].push_back(colIndex); - nodeList[colIndex].push_back(rowIndex); - } - } - else - { - nodeList[rowIndex].push_back(colIndex); - nodeList[colIndex].push_back(rowIndex); - } - - if(b_getValue && value > connStrength) - { - valueList[rowIndex].push_back(value); - valueList[colIndex].push_back(value); - } - } - } - } - - //cout << "No. of upper triangular pruned: " << num_upper_triangular << endl; - m_vi_Vertices.push_back(m_vi_Edges.size()); - - for(int i=0;i < row; i++) - { - m_vi_Edges.insert(m_vi_Edges.end(),nodeList[i].begin(),nodeList[i].end()); - m_vi_Vertices.push_back(m_vi_Edges.size()); - } - - if(b_getValue) - { - for(int i=0;i > nodeList; + map > valueList; + int col = 0, row = 0, rowIndex = 0, colIndex = 0; + int entry_counter = 0, num_of_entries = 0; + double value; + + ifstream in(s_InputFile.c_str()); + if (!in) { + cout << m_s_InputFile << " not Found!" << endl; + return false; + } + + char data[LINE_LENGTH]; + char banner[LINE_LENGTH]; + char mtx[LINE_LENGTH]; + char crd[LINE_LENGTH]; + char data_type[LINE_LENGTH]; + char storage_scheme[LINE_LENGTH]; + char* p; + bool b_getValue = true; + int num_upper_triangular = 0; + + // read the banner + getline(in, line); + strcpy(data, line.c_str()); + + if (sscanf(data, "%s %s %s %s %s", banner, mtx, crd, data_type, + storage_scheme) != 5) { + cout << "Matrix file banner is missing!!!" << endl; + return false; + } + + // intersted about the forth part + for (p = data_type; *p != '\0'; *p = tolower(*p), p++) + ; + + if (strcmp(data_type, "pattern") == 0) b_getValue = false; + + getline(in, line); + while (line.size() > 0 && line[0] == '%') // ignore comment line + getline(in, line); + + in2.str(line); + in2 >> row >> col >> num_of_entries; + + if (row != col) { + cout << "* WARNING: GraphInputOutput::ReadMatrixMarketAdjacencyGraph()" + << endl; + cout << "*\t row!=col. This is not a square matrix. Can't process." << endl; + return false; + } + + while (!in.eof() && + entry_counter < + num_of_entries) // there should be (num_of_entries+1) lines in the + // input file (excluding the comments) + { + getline(in, line); + entry_counter++; + + if (line != "") { + in2.clear(); + in2.str(line); + + in2 >> rowIndex >> colIndex >> value; + rowIndex--; + colIndex--; + + if (rowIndex < 0 || rowIndex >= row) + cout << "Something wrong rowIndex " << rowIndex << " row " << row + << endl; + + if (colIndex < 0 || colIndex >= col) + cout << "Something wrong colIndex " << colIndex << " col " << col + << endl; + + if (rowIndex == colIndex) { + continue; + } + + // This is to handle directed graphs. If the edge is already present, + // skip. If not add. + int exists = 0; + for (int k = 0; k < nodeList[rowIndex].size(); k++) { + if (colIndex == nodeList[rowIndex][k]) { + exists = 1; + break; + } + } + + if (exists == 1) { + num_upper_triangular++; + } else { + if (b_getValue) { + if (value > connStrength) { + nodeList[rowIndex].push_back(colIndex); + nodeList[colIndex].push_back(rowIndex); + } + } else { + nodeList[rowIndex].push_back(colIndex); + nodeList[colIndex].push_back(rowIndex); + } + + if (b_getValue && value > connStrength) { + valueList[rowIndex].push_back(value); + valueList[colIndex].push_back(value); + } + } + } + } + + // cout << "No. of upper triangular pruned: " << num_upper_triangular << endl; + m_vi_Vertices.push_back(m_vi_Edges.size()); + + for (int i = 0; i < row; i++) { + m_vi_Edges.insert(m_vi_Edges.end(), nodeList[i].begin(), nodeList[i].end()); + m_vi_Vertices.push_back(m_vi_Edges.size()); + } + + if (b_getValue) { + for (int i = 0; i < row; i++) { + m_vd_Values.insert(m_vd_Values.end(), valueList[i].begin(), + valueList[i].end()); + } + } + + nodeList.clear(); + valueList.clear(); + CalculateVertexDegrees(); + return true; } +bool CGraphIO::ReadMeTiSAdjacencyGraph(string s_InputFile) { return true; } -bool CGraphIO::ReadMeTiSAdjacencyGraph(string s_InputFile) -{ - return true; -} - -void CGraphIO::CalculateVertexDegrees() -{ - int i_VertexCount = m_vi_Vertices.size() - 1; +void CGraphIO::CalculateVertexDegrees() { + int i_VertexCount = m_vi_Vertices.size() - 1; - m_i_MaximumVertexDegree = -1; - m_i_MinimumVertexDegree = -1; + m_i_MaximumVertexDegree = -1; + m_i_MinimumVertexDegree = -1; - for(int i = 0; i < i_VertexCount; i++) - { - int i_VertexDegree = m_vi_Vertices[i + 1] - m_vi_Vertices[i]; + for (int i = 0; i < i_VertexCount; i++) { + int i_VertexDegree = m_vi_Vertices[i + 1] - m_vi_Vertices[i]; - if(m_i_MaximumVertexDegree < i_VertexDegree) - { - m_i_MaximumVertexDegree = i_VertexDegree; - } + if (m_i_MaximumVertexDegree < i_VertexDegree) { + m_i_MaximumVertexDegree = i_VertexDegree; + } - if(m_i_MinimumVertexDegree == -1) - { - m_i_MinimumVertexDegree = i_VertexDegree; - } - else if(m_i_MinimumVertexDegree > i_VertexDegree) - { - m_i_MinimumVertexDegree = i_VertexDegree; - } - } + if (m_i_MinimumVertexDegree == -1) { + m_i_MinimumVertexDegree = i_VertexDegree; + } else if (m_i_MinimumVertexDegree > i_VertexDegree) { + m_i_MinimumVertexDegree = i_VertexDegree; + } + } - m_d_AverageVertexDegree = (double)m_vi_Edges.size()/i_VertexCount; + m_d_AverageVertexDegree = (double)m_vi_Edges.size() / i_VertexCount; - return; + return; } -string CGraphIO::getFileExtension(string fileName) -{ - string::size_type result; - string fileExtension = ""; - - //1. see if the fileName is given in full path - /*result = fileName.rfind("/", fileName.size() - 1); - if(result != string::npos) - { - //found the path (file prefix) - //get the path, including the last DIR_loopclosure - path = fileName.substr(0,result+1); - //remove the path from the fileName - fileName = fileName.substr(result+1); - } - */ - - //2. see if the fileName has file extension. For example ".mtx" - result = fileName.rfind('.', fileName.size() - 1); - if(result != string::npos) - { - //found the fileExtension - //get the fileExtension excluding the '.' - fileExtension = fileName.substr(result+1); - //remove the fileExtension from the fileName - //fileName = fileName.substr(0,result); - } - - //3. get the name of the input file - //name = fileName; - - return fileExtension; +string CGraphIO::getFileExtension(string fileName) { + string::size_type result; + string fileExtension = ""; + + // 1. see if the fileName is given in full path + /*result = fileName.rfind("/", fileName.size() - 1); + if(result != string::npos) + { + //found the path (file prefix) + //get the path, including the last DIR_loopclosure + path = fileName.substr(0,result+1); + //remove the path from the fileName + fileName = fileName.substr(result+1); + } + */ + + // 2. see if the fileName has file extension. For example ".mtx" + result = fileName.rfind('.', fileName.size() - 1); + if (result != string::npos) { + // found the fileExtension + // get the fileExtension excluding the '.' + fileExtension = fileName.substr(result + 1); + // remove the fileExtension from the fileName + // fileName = fileName.substr(0,result); + } + + // 3. get the name of the input file + // name = fileName; + + return fileExtension; } -} \ No newline at end of file +} // namespace FMC \ No newline at end of file diff --git a/d2pgo/third_party/fast_max-clique_finder/src/utils.cpp b/d2pgo/third_party/fast_max-clique_finder/src/utils.cpp old mode 100755 new mode 100644 index b8db5cb6..797d3910 --- a/d2pgo/third_party/fast_max-clique_finder/src/utils.cpp +++ b/d2pgo/third_party/fast_max-clique_finder/src/utils.cpp @@ -1,52 +1,56 @@ -/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ -/* Description: a library for finding the maximum clique of a graph */ +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * * * * * * */ +/* Description: a library for finding the maximum clique of a graph + */ /* */ /* */ -/* Authors: Bharath Pattabiraman and Md. Mostofa Ali Patwary */ -/* EECS Department, Northwestern University */ -/* email: {bpa342,mpatwary}@eecs.northwestern.edu */ +/* Authors: Bharath Pattabiraman and Md. Mostofa Ali Patwary */ +/* EECS Department, Northwestern University */ +/* email: {bpa342,mpatwary}@eecs.northwestern.edu */ /* */ -/* Copyright, 2014, Northwestern University */ -/* See COPYRIGHT notice in top-level directory. */ +/* Copyright, 2014, Northwestern University */ +/* See COPYRIGHT notice in top-level directory. */ /* */ -/* Please site the following publication if you use this package: */ -/* Bharath Pattabiraman, Md. Mostofa Ali Patwary, Assefaw H. Gebremedhin2, */ -/* Wei-keng Liao, and Alok Choudhary. */ -/* "Fast Algorithms for the Maximum Clique Problem on Massive Graphs with */ -/* Applications to Overlapping Community Detection" */ -/* http://arxiv.org/abs/1411.7460 */ -/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ +/* Please site the following publication if you use this package: */ +/* Bharath Pattabiraman, Md. Mostofa Ali Patwary, Assefaw H. Gebremedhin2, + */ +/* Wei-keng Liao, and Alok Choudhary. */ +/* "Fast Algorithms for the Maximum Clique Problem on Massive Graphs with */ +/* Applications to Overlapping Community Detection" + */ +/* http://arxiv.org/abs/1411.7460 + */ +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * * * * * * */ #include "findClique.h" namespace FMC { -bool fexists(const char *filename) -{ - ifstream ifile(filename); - return ifile.is_open(); +bool fexists(const char *filename) { + ifstream ifile(filename); + return ifile.is_open(); } -double wtime() // returns wall time in seconds +double wtime() // returns wall time in seconds { - timeval tv; - gettimeofday(&tv, NULL); - return (double)tv.tv_sec + (double)tv.tv_usec/1000000; + timeval tv; + gettimeofday(&tv, NULL); + return (double)tv.tv_sec + (double)tv.tv_usec / 1000000; } -void usage(char *argv0) -{ - const char *params = - "Usage: %s [options...] inputfile\n" - "OPTIONS:\n" - "\t-t algorithm type\t: 0 for exact, 1 for heuristic(default)\n" - "\t-l input lower bound\t: intial max clique size e.g. default 0\n" - "\t-p print clique\t\t: This parameter if want to print the max clique (only for exact algorithm)\n"; - fprintf(stderr, params, argv0); - exit(-1); +void usage(char *argv0) { + const char *params = + "Usage: %s [options...] inputfile\n" + "OPTIONS:\n" + "\t-t algorithm type\t: 0 for exact, 1 for heuristic(default)\n" + "\t-l input lower bound\t: intial max clique size e.g. default 0\n" + "\t-p print clique\t\t: This parameter if want to print the max clique " + "(only for exact algorithm)\n"; + fprintf(stderr, params, argv0); + exit(-1); } -int getDegree(vector* ptrVtx, int idx) -{ - return ( (*ptrVtx)[idx+1] - (*ptrVtx)[idx] ); +int getDegree(vector *ptrVtx, int idx) { + return ((*ptrVtx)[idx + 1] - (*ptrVtx)[idx]); } -} \ No newline at end of file +} // namespace FMC \ No newline at end of file diff --git a/d2vins/CMakeLists.txt b/d2vins/CMakeLists.txt index dee03c83..3d732c51 100644 --- a/d2vins/CMakeLists.txt +++ b/d2vins/CMakeLists.txt @@ -60,6 +60,7 @@ add_library(${PROJECT_NAME}_estimator src/factors/projectionOneFrameTwoCamFactor.cpp src/factors/prior_factor.cpp src/network/d2vins_net.cpp + src/utils/solve_5pts.cpp ) target_link_libraries(${PROJECT_NAME}_estimator @@ -83,5 +84,6 @@ target_link_libraries(${PROJECT_NAME}_node ${CERES_LIBRARIES} dw lcm + spdlog ) diff --git a/d2vins/launch/GRACO.launch b/d2vins/launch/GRACO.launch new file mode 100644 index 00000000..677b72aa --- /dev/null +++ b/d2vins/launch/GRACO.launch @@ -0,0 +1,86 @@ + + + + + + + + + + + + + + + + + + + + + + + + + lcm_uri: udpm://224.0.0.251:7667?ttl=1 + jpg_quality: 75 + + query_thres: 0.2 + init_query_thres: 0.2 + + min_movement_keyframe: 0.1 + enable_pub_remote_img: false + enable_sub_remote_img: false + accept_min_3d_pts: 30 + recv_msg_duration: 0.5 + superpoint_thres: 0.1 + triangle_thres: 0.012 + #triangle_thres: 0.008 + min_direction_loop: 1 + + detector_match_thres: 0.7 + send_all_features: false + lower_cam_as_main: false + min_match_per_dir: 20 + output_raw_superpoint_desc: false + depth_far_thres: 100 + loop_cov_pos: 0.02 + loop_cov_ang: 0.01 + odometry_consistency_threshold: 3.0 + pos_covariance_per_meter: 0.01 + yaw_covariance_per_meter: 0.003 + enable_network: true + ransacReprojThreshold: 10.0 + long_track_thres: 40 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/d2vins/launch/VIRAL.launch b/d2vins/launch/VIRAL.launch new file mode 100644 index 00000000..996922b8 --- /dev/null +++ b/d2vins/launch/VIRAL.launch @@ -0,0 +1,86 @@ + + + + + + + + + + + + + + + + + + + + + + + + + lcm_uri: udpm://224.0.0.251:7667?ttl=1 + jpg_quality: 75 + + query_thres: 0.2 + init_query_thres: 0.2 + + min_movement_keyframe: 0.1 + enable_pub_remote_img: false + enable_sub_remote_img: false + accept_min_3d_pts: 30 + recv_msg_duration: 0.5 + superpoint_thres: 0.1 + triangle_thres: 0.012 + #triangle_thres: 0.008 + min_direction_loop: 1 + + detector_match_thres: 0.7 + send_all_features: false + lower_cam_as_main: false + min_match_per_dir: 20 + output_raw_superpoint_desc: false + depth_far_thres: 100 + loop_cov_pos: 0.02 + loop_cov_ang: 0.01 + odometry_consistency_threshold: 3.0 + pos_covariance_per_meter: 0.01 + yaw_covariance_per_meter: 0.003 + enable_network: true + ransacReprojThreshold: 10.0 + long_track_thres: 40 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/d2vins/launch/quadcam.launch b/d2vins/launch/quadcam.launch index 9d018a76..1d50a966 100644 --- a/d2vins/launch/quadcam.launch +++ b/d2vins/launch/quadcam.launch @@ -23,7 +23,6 @@ - lcm_uri: udpm://224.0.0.251:7667?ttl=1 @@ -58,7 +57,7 @@ - + @@ -89,7 +88,7 @@ - + diff --git a/d2vins/launch/realsense.launch b/d2vins/launch/realsense.launch index 131198e0..d12df743 100644 --- a/d2vins/launch/realsense.launch +++ b/d2vins/launch/realsense.launch @@ -16,8 +16,8 @@ - - + + @@ -37,7 +37,7 @@ enable_sub_remote_img: false accept_min_3d_pts: 30 recv_msg_duration: 0.5 - superpoint_thres: 0.2 + superpoint_thres: 0.1 triangle_thres: 0.012 #triangle_thres: 0.008 min_direction_loop: 1 @@ -72,7 +72,6 @@ - diff --git a/d2vins/src/MSCKF/MSCKF.cpp b/d2vins/src/MSCKF/MSCKF.cpp index be2b33e0..8493e232 100644 --- a/d2vins/src/MSCKF/MSCKF.cpp +++ b/d2vins/src/MSCKF/MSCKF.cpp @@ -1,130 +1,134 @@ #include "MSCKF.hpp" + #include namespace D2VINS { MSCKF::MSCKF() { - Q_imu.setZero(); - Q_imu.block<3, 3>(0, 0) = _config.gyr_n*Matrix3d::Identity(); - Q_imu.block<3, 3>(3, 3) = _config.gyr_w*Matrix3d::Identity(); - Q_imu.block<3, 3>(6, 6) = _config.acc_n*Matrix3d::Identity(); - Q_imu.block<3, 3>(9, 9) = _config.acc_w*Matrix3d::Identity(); + Q_imu.setZero(); + Q_imu.block<3, 3>(0, 0) = _config.gyr_n * Matrix3d::Identity(); + Q_imu.block<3, 3>(3, 3) = _config.gyr_w * Matrix3d::Identity(); + Q_imu.block<3, 3>(6, 6) = _config.acc_n * Matrix3d::Identity(); + Q_imu.block<3, 3>(9, 9) = _config.acc_w * Matrix3d::Identity(); } void MSCKF::initFirstPose() { - auto q0 = Utility::g2R(imubuf.mean_acc()); - nominal_state.q_imu = q0; + auto q0 = Utility::g2R(imubuf.mean_acc()); + nominal_state.q_imu = q0; } -void MSCKF::predict(const double t, const IMUData & imudata) { - //Follows Mourikis, Anastasios I., and Stergios I. Roumeliotis. - // "A multi-state constraint Kalman filter for vision-aided inertial navigation." - // Proceedings 2007 IEEE International Conference on Robotics and Automation. IEEE, 2007. - // Sect III-B - if (t_last < 0 ) { - t_last = t; - } - - imubuf.add(imudata); - if (!initFirstPoseFlag) { - //First pose is inited in keyframe. So first pose must is a keyframe. - return; - } - - double dt = t - t_last; - - static IMUData imudatalast; - - //trapezoidal integration - Vector3d gyro = (imudata.gyro + imudatalast.gyro)/2; - Vector3d acc = (imudata.acc + imudatalast.acc)/2; - imudatalast = imudata; - - Vector3d angvel_hat = imudata.gyro - error_state.bias_gyro; //Planet angular velocity is ignored - Matrix3d Rq_hat = nominal_state.get_imu_R(); - Vector3d acc_hat = imudata.acc - error_state.bias_acc; - - //Nominal State - Quaterniond omg_l(0, angvel_hat.x(), angvel_hat.y(), angvel_hat.z()); - - //Naive intergation - auto qdot = nominal_state.q_imu * omg_l; - auto vdot = Rq_hat*acc_hat + IMUData::Gravity; - auto pdot = nominal_state.v_imu; - - //Internal the quaternion is save as [qw, qx, qy, qz] in Eigen - nominal_state.q_imu.coeffs().block<3, 1>(0, 0) += qdot.coeffs().block<3, 1>(0, 0)*dt; - nominal_state.q_imu.normalize(); - nominal_state.v_imu += vdot*dt; - nominal_state.p_imu += pdot*dt; - - - //Error state - // Model: - // d (x_err)/dt = F_mat * x_err + G * n_imu - // x_err: error state vector - - Eigen::Matrix F_mat; - F_mat.setZero(); - - //Rows 1-3, dynamics on quat - F_mat.block<3, 3>(0, 0) = skewSymmetric(angvel_hat); - F_mat.block<3, 3>(0, 3) = - Matrix3d::Identity(); - - //Rows 4-6, dynamics on bias is empty - //Rows 7-9, dynamics on velocity - F_mat.block<3, 3>(6, 0) = - Rq_hat * skewSymmetric(acc_hat); - F_mat.block<3, 3>(6, 6) = - 2 * skewSymmetric(acc_hat); - F_mat.block<3, 3>(6, 9) = - Rq_hat; - F_mat.block<3, 3>(6, 12) = - skewSymmetric(acc_hat); - //Rows 10-12, dynamics on bias is empty - - //Rows 13-15, dynamics on position - F_mat.block<3, 3>(12, 6) = Matrix3d::Identity(); - - Eigen::Matrix G_mat; - G_mat.setZero(); - G_mat.block<3, 3>(0, 0) = - Matrix3d::Identity(); - G_mat.block<3, 3>(3, 3) = Matrix3d::Identity(); - G_mat.block<3, 3>(6, 6) = - Rq_hat; - G_mat.block<3, 3>(9, 9) = Matrix3d::Identity(); - - //Now we need to intergate this, naive approach is trapezoidal rule - //x_err_new = F_mat*x_err_last*dt + x_err_last - //Or \dot Phi = F_mat Phi, Phi(0) = I - // Phi = I + F_mat Phi * dt - Eigen::Matrix Phi; - Phi.setIdentity(); - Phi = Phi + F_mat*dt; - auto G = G_mat*dt; - - // Suggest by (268)-(269) in Sola J. Quaternion kinematics for the error-state Kalman filter - // We don't predict the error state space - // Instead, we only predict the P of error state, and predict the nominal state - auto P_new = Phi*error_state.getImuP()*Phi.transpose() + G*Q_imu*G.transpose(); - auto P_imu_other_new = Phi*error_state.getImuOtherP(); - - //Set states to error_state - error_state.setImuP(P_new); - error_state.setImuOtherP(P_imu_other_new); +void MSCKF::predict(const double t, const IMUData& imudata) { + // Follows Mourikis, Anastasios I., and Stergios I. Roumeliotis. + // "A multi-state constraint Kalman filter for vision-aided inertial + // navigation." Proceedings 2007 IEEE International Conference on Robotics and + // Automation. IEEE, 2007. Sect III-B + if (t_last < 0) { + t_last = t; + } + + imubuf.add(imudata); + if (!initFirstPoseFlag) { + // First pose is inited in keyframe. So first pose must is a keyframe. + return; + } + + double dt = t - t_last; + + static IMUData imudatalast; + + // trapezoidal integration + Vector3d gyro = (imudata.gyro + imudatalast.gyro) / 2; + Vector3d acc = (imudata.acc + imudatalast.acc) / 2; + imudatalast = imudata; + + Vector3d angvel_hat = + imudata.gyro - + error_state.bias_gyro; // Planet angular velocity is ignored + Matrix3d Rq_hat = nominal_state.get_imu_R(); + Vector3d acc_hat = imudata.acc - error_state.bias_acc; + + // Nominal State + Quaterniond omg_l(0, angvel_hat.x(), angvel_hat.y(), angvel_hat.z()); + + // Naive intergation + auto qdot = nominal_state.q_imu * omg_l; + auto vdot = Rq_hat * acc_hat + IMUData::Gravity; + auto pdot = nominal_state.v_imu; + + // Internal the quaternion is save as [qw, qx, qy, qz] in Eigen + nominal_state.q_imu.coeffs().block<3, 1>(0, 0) += + qdot.coeffs().block<3, 1>(0, 0) * dt; + nominal_state.q_imu.normalize(); + nominal_state.v_imu += vdot * dt; + nominal_state.p_imu += pdot * dt; + + // Error state + // Model: + // d (x_err)/dt = F_mat * x_err + G * n_imu + // x_err: error state vector + + Eigen::Matrix F_mat; + F_mat.setZero(); + + // Rows 1-3, dynamics on quat + F_mat.block<3, 3>(0, 0) = skewSymmetric(angvel_hat); + F_mat.block<3, 3>(0, 3) = -Matrix3d::Identity(); + + // Rows 4-6, dynamics on bias is empty + // Rows 7-9, dynamics on velocity + F_mat.block<3, 3>(6, 0) = -Rq_hat * skewSymmetric(acc_hat); + F_mat.block<3, 3>(6, 6) = -2 * skewSymmetric(acc_hat); + F_mat.block<3, 3>(6, 9) = -Rq_hat; + F_mat.block<3, 3>(6, 12) = -skewSymmetric(acc_hat); + // Rows 10-12, dynamics on bias is empty + + // Rows 13-15, dynamics on position + F_mat.block<3, 3>(12, 6) = Matrix3d::Identity(); + + Eigen::Matrix G_mat; + G_mat.setZero(); + G_mat.block<3, 3>(0, 0) = -Matrix3d::Identity(); + G_mat.block<3, 3>(3, 3) = Matrix3d::Identity(); + G_mat.block<3, 3>(6, 6) = -Rq_hat; + G_mat.block<3, 3>(9, 9) = Matrix3d::Identity(); + + // Now we need to intergate this, naive approach is trapezoidal rule + // x_err_new = F_mat*x_err_last*dt + x_err_last + // Or \dot Phi = F_mat Phi, Phi(0) = I + // Phi = I + F_mat Phi * dt + Eigen::Matrix Phi; + Phi.setIdentity(); + Phi = Phi + F_mat * dt; + auto G = G_mat * dt; + + // Suggest by (268)-(269) in Sola J. Quaternion kinematics for the error-state + // Kalman filter We don't predict the error state space Instead, we only + // predict the P of error state, and predict the nominal state + auto P_new = + Phi * error_state.getImuP() * Phi.transpose() + G * Q_imu * G.transpose(); + auto P_imu_other_new = Phi * error_state.getImuOtherP(); + + // Set states to error_state + error_state.setImuP(P_new); + error_state.setImuOtherP(P_imu_other_new); } void MSCKF::addKeyframe(const double t) { - //For convience, we require t here is exact same to last imu t - if (!initFirstPoseFlag) { - if (imubuf.size() >= _config.init_imu_num) { - initFirstPose(); - } - return; - } - if (t_last >= 0) { - assert(fabs(t - t_last) < 1.0/_config.IMU_FREQ && "MSCKF new image must be added EXACTLY after the corresponding imu is applied!"); + // For convience, we require t here is exact same to last imu t + if (!initFirstPoseFlag) { + if (imubuf.size() >= _config.init_imu_num) { + initFirstPose(); } - error_state.stateAugmentation(t); - nominal_state.addKeyframe(t); + return; + } + if (t_last >= 0) { + assert(fabs(t - t_last) < 1.0 / _config.IMU_FREQ && + "MSCKF new image must be added EXACTLY after the corresponding imu " + "is applied!"); + } + error_state.stateAugmentation(t); + nominal_state.addKeyframe(t); } -void MSCKF::update(const LandmarkPerId & feature_by_id) { - -} +void MSCKF::update(const LandmarkPerId& feature_by_id) {} -} \ No newline at end of file +} // namespace D2VINS \ No newline at end of file diff --git a/d2vins/src/MSCKF/MSCKF_state.cpp b/d2vins/src/MSCKF/MSCKF_state.cpp index 9e78bf35..00afdf1a 100644 --- a/d2vins/src/MSCKF/MSCKF_state.cpp +++ b/d2vins/src/MSCKF/MSCKF_state.cpp @@ -1,100 +1,106 @@ #include "MSCKF_state.hpp" -MSCKFStateVector::MSCKFStateVector(): - q_imu(1.0, 0.0, 0.0, 0.0), - bias_gyro(0.0, 0.0, 0.0), - v_imu(0.0, 0.0, 0.0), - bias_acc(0.0, 0.0, 0.0), - p_imu(0.0, 0.0, 0.0) -{ +MSCKFStateVector::MSCKFStateVector() + : q_imu(1.0, 0.0, 0.0, 0.0), + bias_gyro(0.0, 0.0, 0.0), + v_imu(0.0, 0.0, 0.0), + bias_acc(0.0, 0.0, 0.0), + p_imu(0.0, 0.0, 0.0) {} + +Matrix3d MSCKFStateVector::get_imu_R() const { + return q_imu.toRotationMatrix(); } -Matrix3d MSCKFStateVector::get_imu_R() const { - return q_imu.toRotationMatrix(); +MSCKFErrorStateVector::MSCKFErrorStateVector(const MSCKFStateVector& state0) { + ang.setZero(); + pos.setZero(); + v_imu.setZero(); + bias_gyro.setZero(); + bias_acc.setZero(); + camera_extrisincs.resize(state0.camera_extrisincs.size()); + for (auto& extrinsic : camera_extrisincs) { + extrinsic.first.setZero(); + extrinsic.second.setZero(); + } + + sld_win_poses.resize(state0.sld_win_poses.size()); + + for (auto& pose : camera_extrisincs) { + pose.first.setZero(); + pose.second.setZero(); + } } -MSCKFErrorStateVector::MSCKFErrorStateVector(const MSCKFStateVector & state0) { - ang.setZero(); - pos.setZero(); - v_imu.setZero(); - bias_gyro.setZero(); - bias_acc.setZero(); - camera_extrisincs.resize(state0.camera_extrisincs.size()); - for (auto & extrinsic : camera_extrisincs) { - extrinsic.first.setZero(); - extrinsic.second.setZero(); - } - - sld_win_poses.resize(state0.sld_win_poses.size()); - - for (auto & pose : camera_extrisincs) { - pose.first.setZero(); - pose.second.setZero(); - } -} - -void MSCKFErrorStateVector::reset(MSCKFStateVector & nominal) { - assert(camera_extrisincs.size() == nominal.camera_extrisincs.size()); - assert(sld_win_poses.size() == nominal.sld_win_poses.size()); +void MSCKFErrorStateVector::reset(MSCKFStateVector& nominal) { + assert(camera_extrisincs.size() == nominal.camera_extrisincs.size()); + assert(sld_win_poses.size() == nominal.sld_win_poses.size()); - //TODO: + // TODO: } -void MSCKFErrorStateVector::setImuVector(Eigen::Matrix v) { - ang = v.block<3, 1>(0, 0); - bias_gyro = v.block<3, 1>(3, 0); - v_imu = v.block<3, 1>(6, 0); - bias_acc = v.block<3, 1>(9, 0); - pos = v.block<3, 1>(12, 0); +void MSCKFErrorStateVector::setImuVector( + Eigen::Matrix v) { + ang = v.block<3, 1>(0, 0); + bias_gyro = v.block<3, 1>(3, 0); + v_imu = v.block<3, 1>(6, 0); + bias_acc = v.block<3, 1>(9, 0); + pos = v.block<3, 1>(12, 0); } -void MSCKFErrorStateVector::setImuP(Eigen::Matrix _P) { - P.block(0, 0) = _P; +void MSCKFErrorStateVector::setImuP( + Eigen::Matrix _P) { + P.block(0, 0) = _P; } void MSCKFStateVector::addKeyframe(double t) { - sld_win_poses.push_back(Swarm::Pose(q_imu, p_imu)); + sld_win_poses.push_back(Swarm::Pose(q_imu, p_imu)); } -Eigen::Matrix MSCKFErrorStateVector::getImuVector() const { - Eigen::Matrix v; - v.block<3, 1>(0, 0) = ang; - v.block<3, 1>(3, 0) = bias_gyro; - v.block<3, 1>(6, 0) = v_imu; - v.block<3, 1>(9, 0) = bias_acc; - v.block<3, 1>(12, 0) = pos; - return v; +Eigen::Matrix MSCKFErrorStateVector::getImuVector() + const { + Eigen::Matrix v; + v.block<3, 1>(0, 0) = ang; + v.block<3, 1>(3, 0) = bias_gyro; + v.block<3, 1>(6, 0) = v_imu; + v.block<3, 1>(9, 0) = bias_acc; + v.block<3, 1>(12, 0) = pos; + return v; } -Eigen::Matrix MSCKFErrorStateVector::getImuP() const { - return P.block(0, 0); +Eigen::Matrix +MSCKFErrorStateVector::getImuP() const { + return P.block(0, 0); } -Eigen::Matrix MSCKFErrorStateVector::getImuOtherP() const { - return P.block(0, IMU_STATE_DIM, IMU_STATE_DIM, P.cols() - IMU_STATE_DIM); +Eigen::Matrix +MSCKFErrorStateVector::getImuOtherP() const { + return P.block(0, IMU_STATE_DIM, IMU_STATE_DIM, P.cols() - IMU_STATE_DIM); } -void MSCKFErrorStateVector::setImuOtherP(Eigen::Matrix _P) { - P.block(0, IMU_STATE_DIM, IMU_STATE_DIM, P.cols() - IMU_STATE_DIM) = _P; - P.block(IMU_STATE_DIM, 0, P.cols() - IMU_STATE_DIM, IMU_STATE_DIM) = _P.transpose(); +void MSCKFErrorStateVector::setImuOtherP( + Eigen::Matrix _P) { + P.block(0, IMU_STATE_DIM, IMU_STATE_DIM, P.cols() - IMU_STATE_DIM) = _P; + P.block(IMU_STATE_DIM, 0, P.cols() - IMU_STATE_DIM, IMU_STATE_DIM) = + _P.transpose(); } void MSCKFErrorStateVector::stateAugmentation(double t) { - // This function modified from (14) - (16) in [Mourikis et al. 2007]. - // The original state records image poses in [Mourikis et al. 2007]. - // [Li M. et al. 2013] suggest to directly use IMU poses. - // Our implementations also record IMU poses because we will make this appliable to arbitrary number of cameras. - - int prev_dim = stateDimFull(); - sld_win_poses.push_back(std::make_pair(ang, pos)); - auto G = MatrixXd(prev_dim + 6, prev_dim); - G.setZero(); - - auto J = MatrixXd(6, prev_dim); - J.block(0, 0, 3, 3) = Matrix3d::Identity(); - J.block(3, 12, 3, 3) = Matrix3d::Identity(); - - G.block(0, 0, prev_dim, prev_dim).setIdentity(); - G.block(prev_dim, 0, 6, prev_dim) = J; - P = G * P * G.transpose(); + // This function modified from (14) - (16) in [Mourikis et al. 2007]. + // The original state records image poses in [Mourikis et al. 2007]. + // [Li M. et al. 2013] suggest to directly use IMU poses. + // Our implementations also record IMU poses because we will make this + // appliable to arbitrary number of cameras. + + int prev_dim = stateDimFull(); + sld_win_poses.push_back(std::make_pair(ang, pos)); + auto G = MatrixXd(prev_dim + 6, prev_dim); + G.setZero(); + + auto J = MatrixXd(6, prev_dim); + J.block(0, 0, 3, 3) = Matrix3d::Identity(); + J.block(3, 12, 3, 3) = Matrix3d::Identity(); + + G.block(0, 0, prev_dim, prev_dim).setIdentity(); + G.block(prev_dim, 0, 6, prev_dim) = J; + P = G * P * G.transpose(); } diff --git a/d2vins/src/d2vins_node.cpp b/d2vins/src/d2vins_node.cpp index a6ddb568..9be2fb62 100644 --- a/d2vins/src/d2vins_node.cpp +++ b/d2vins/src/d2vins_node.cpp @@ -1,256 +1,326 @@ -#include #include -#include +#include +#include #include -#include "sensor_msgs/Imu.h" -#include "estimator/d2estimator.hpp" -#include "network/d2vins_net.hpp" +#include +#include +#include + +#include #include #include -#include -#include -#include +#include + +#include "estimator/d2estimator.hpp" +#include "network/d2vins_net.hpp" +#include "sensor_msgs/Imu.h" using namespace std::chrono; #define BACKWARD_HAS_DW 1 #include -namespace backward -{ - backward::SignalHandling sh; +namespace backward { +backward::SignalHandling sh; } using namespace D2VINS; using namespace D2Common; using namespace std::chrono; -class D2VINSNode : public D2FrontEnd::D2Frontend -{ - typedef std::lock_guard Guard; - D2Estimator * estimator = nullptr; - D2VINSNet * d2vins_net = nullptr; - ros::Subscriber imu_sub, pgo_fused_sub; - ros::Publisher visual_array_pub; - int frame_count = 0; - std::queue viokf_queue; - std::mutex queue_lock; - ros::Timer estimator_timer, solver_timer; - std::thread thread_comm, thread_solver, thread_viokf; - bool has_received_imu = false; - double last_imu_ts = 0; - bool need_solve = false; - std::set ready_drones; - std::map pgo_poses; - std::map> vins_poses; -protected: - Swarm::Pose getMotionPredict(double stamp) const override { - return estimator->getMotionPredict(stamp).first.pose(); +class D2VINSNode : public D2FrontEnd::D2Frontend { +public: + void stopAllThread(){ + thread_viokf_running = false; + thread_viokf.join(); + if (params->estimation_mode == D2Common::DISTRIBUTED_CAMERA_CONSENUS) { + thread_solver_running = false; + thread_solver.join(); } + thread_comm_running = false; + thread_comm.join(); + } +private: + typedef std::lock_guard Guard; + D2Estimator *estimator = nullptr; + D2VINSNet *d2vins_net = nullptr; + ros::Subscriber imu_sub, pgo_fused_sub; + ros::Publisher visual_array_pub; + int frame_count = 0; + std::queue viokf_queue; + std::mutex queue_lock; + ros::Timer estimator_timer, solver_timer; - virtual void backendFrameCallback(const D2Common::VisualImageDescArray & viokf) override { - if (params->estimation_mode < D2VINSConfig::SERVER_MODE) { - Guard guard(queue_lock); - viokf_queue.emplace(viokf); - } - frame_count ++; - }; + std::thread thread_comm, thread_solver, thread_viokf; - void processRemoteImage(VisualImageDescArray & frame_desc, bool succ_track) override { - { - ready_drones.insert(frame_desc.drone_id); - vins_poses[frame_desc.drone_id] = std::make_pair(frame_desc.reference_frame_id, frame_desc.pose_drone); - if (params->estimation_mode != D2VINSConfig::SINGLE_DRONE_MODE && succ_track && - !frame_desc.is_lazy_frame && frame_desc.matched_frame < 0) { - estimator->inputRemoteImage(frame_desc); - } else { - if (params->estimation_mode != D2VINSConfig::SINGLE_DRONE_MODE && - frame_desc.sld_win_status.size() > 0) { - estimator->updateSldwin(frame_desc.drone_id, frame_desc.sld_win_status); - } - if (frame_desc.matched_frame < 0) { - VINSFrame frame(frame_desc); - estimator->getVisualizer().pubFrame(&frame); - } - } - } - D2Frontend::processRemoteImage(frame_desc, succ_track); - if (params->pub_visual_frame) { - visual_array_pub.publish(frame_desc.toROS()); - } - } + bool thread_viokf_running = false; + bool thread_solver_running = false; + bool thread_comm_running = false; - void updateOutModuleSldWinAndLandmarkDB() { - //Frame related operations. Need to be protected by frame_mutex - const std::lock_guard lock(estimator->frame_mutex); - auto sld_win = estimator->getSelfSldWin(); - auto landmark_db = estimator->getLandmarkDB(); - if (params->enable_loop) { - loop_detector->updatebyLandmarkDB(estimator->getLandmarkDB()); - loop_detector->updatebySldWin(sld_win); - } - feature_tracker->updatebySldWin(sld_win); - feature_tracker->updatebyLandmarkDB(estimator->getLandmarkDB()); + std::unique_ptr viokf_rate_ptr_; + std::unique_ptr solver_rate_ptr_; + std::unique_ptr comm_rate_ptr_; + + + bool has_received_imu = false; + double last_imu_ts = 0; + bool need_solve = false; + std::set ready_drones; + std::map pgo_poses; + std::map> vins_poses; + + protected: + Swarm::Pose getMotionPredict(double stamp) const override { + return estimator->getMotionPredict(stamp).first.pose(); + } + + virtual void backendFrameCallback( + const D2Common::VisualImageDescArray &viokf) override { + if (params->estimation_mode < D2Common::SERVER_MODE) { + Guard guard(queue_lock); + viokf_queue.emplace(viokf); } + frame_count++; + }; - void processVIOKFThread() { - while(ros::ok()) { - if (!viokf_queue.empty()) { - Utility::TicToc estimator_timer; - if (viokf_queue.size() > params->warn_pending_frames) { - ROS_WARN("[D2VINS] Low efficient on D2VINS::estimator pending frames: %d", viokf_queue.size()); - } - D2Common::VisualImageDescArray viokf; - { - Guard guard(queue_lock); - viokf = viokf_queue.front(); - viokf_queue.pop(); - } - bool ret; - { - Utility::TicToc input; - ret = estimator->inputImage(viokf); - double input_time = input.toc(); - Utility::TicToc loop; - if (viokf.is_keyframe) { - addToLoopQueue(viokf); - } - updateOutModuleSldWinAndLandmarkDB(); - need_solve = true; - if (params->verbose || params->enable_perf_output) - printf("[D2VINS] input_time %.1fms, loop detector related takes %.1f ms\n", input_time, loop.toc()); - } + void processRemoteImage(VisualImageDescArray &frame_desc, + bool succ_track) override { + spdlog::debug( + "[D2VINS] processRemoteImage: frame_id {}, drone_id {}, matched_frame " + "{}, is_keyframe {}, succ_track {}, is_lazy_frame {}, sld_win_status " + "size {}", + frame_desc.frame_id, frame_desc.drone_id, frame_desc.matched_frame, + frame_desc.is_keyframe, succ_track, frame_desc.is_lazy_frame, + frame_desc.sld_win_status.size()); + ready_drones.insert(frame_desc.drone_id); + vins_poses[frame_desc.drone_id] = + std::make_pair(frame_desc.reference_frame_id, frame_desc.pose_drone); + if (params->estimation_mode != D2Common::SINGLE_DRONE_MODE && succ_track && + !frame_desc.is_lazy_frame && frame_desc.matched_frame < 0) { + estimator->inputRemoteImage(frame_desc); + } else { + if (params->estimation_mode != D2Common::SINGLE_DRONE_MODE && + frame_desc.sld_win_status.size() > 0) { + estimator->updateSldwin(frame_desc.drone_id, frame_desc.sld_win_status); + } + if (frame_desc.matched_frame < 0) { + VINSFrame frame(frame_desc); + estimator->getVisualizer().pubFrame(&frame); + } + } + D2Frontend::processRemoteImage(frame_desc, succ_track); + if (params->pub_visual_frame) { + visual_array_pub.publish(frame_desc.toROS()); + } + } - if (params->pub_visual_frame) { - visual_array_pub.publish(viokf.toROS()); - } - if (params->verbose || params->enable_perf_output) - printf("[D2VINS] estimator_timer_callback takes %.1f ms\n", estimator_timer.toc()); - bool discover_mode = false; - for (auto & id : ready_drones) { - if (pgo_poses.find(id) == pgo_poses.end() && !estimator->getState().hasDrone(id)) { - discover_mode = true; - break; - } - } - if (ret && D2FrontEnd::params->enable_network) { //Only send keyframes - if (params->lazy_broadcast_keyframe && !viokf.is_keyframe && !discover_mode) { - continue; - } - std::set nearbydrones = estimator->getNearbyDronesbyPGOData(vins_poses); - bool force_landmarks = false || discover_mode; - if (nearbydrones.size() > 0) { - force_landmarks = true; - if (params->verbose) { - printf("[D2VINS] Nearby drones: "); - for (auto & id : nearbydrones) { - printf("%d ", id); - } - printf("\n"); - } - } - printf("[D2VINS] force landmarks %d to broadcast\n", force_landmarks); - Utility::TicToc broadcast_timer; - loop_net->broadcastVisualImageDescArray(viokf, force_landmarks); - if (params->verbose || params->enable_perf_output) { - printf("[D2VINS] broadcastVisualImageDescArray takes %.1f ms\n", broadcast_timer.toc()); - } - } - } else { - usleep(1000); - } - } + void updateOutModuleSldWinAndLandmarkDB() { + // Frame related operations. Need to be protected by frame_mutex + const std::lock_guard lock(estimator->frame_mutex); + auto sld_win = estimator->getSelfSldWin(); + auto landmark_db = estimator->getLandmarkDB(); + if (params->enable_loop) { + loop_detector->updatebyLandmarkDB(estimator->getLandmarkDB()); + loop_detector->updatebySldWin(sld_win); } + feature_tracker->updatebySldWin(sld_win); + feature_tracker->updatebyLandmarkDB(estimator->getLandmarkDB()); + } - virtual void imuCallback(const sensor_msgs::Imu & imu) { - IMUData data(imu); - if(!has_received_imu) { - has_received_imu = true; - last_imu_ts = imu.header.stamp.toSec(); + void processVIOKFThread() { + //set thread name + prctl(PR_SET_NAME, "D2VINS::estimator", 0, 0, 0); + while (thread_viokf_running) { + if (!viokf_queue.empty()) { + Utility::TicToc estimator_timer; + if (viokf_queue.size() > params->warn_pending_frames) { + SPDLOG_WARN( + "[D2VINS] Low efficient on D2VINS::estimator pending frames: {}", + viokf_queue.size()); + } + D2Common::VisualImageDescArray viokf; + { + Guard guard(queue_lock); + viokf = viokf_queue.front(); + viokf_queue.pop(); + } + bool ret; + { + Utility::TicToc input; + ret = estimator->inputImage(viokf); + double input_time = input.toc(); + Utility::TicToc loop; + if (viokf.is_keyframe) { + addToLoopQueue(viokf); + } + updateOutModuleSldWinAndLandmarkDB(); + need_solve = true; + if (params->enable_perf_output) + SPDLOG_INFO( + "[D2VINS] input_time {:.1f}ms, loop detector related takes " + "{:.1f} ms", + input_time, loop.toc()); } - data.dt = imu.header.stamp.toSec() - last_imu_ts; - last_imu_ts = imu.header.stamp.toSec(); - estimator->inputImu(data); - } - void pgoSwarmFusedCallback(const swarm_msgs::swarm_fused & fused) { - if (params->estimation_mode == D2VINSConfig::SINGLE_DRONE_MODE) { - return; + if (params->pub_visual_frame) { + visual_array_pub.publish(viokf.toROS()); } - for (size_t i = 0; i < fused.ids.size(); i++) { - Swarm::Pose pose(fused.local_drone_position[i], fused.local_drone_rotation[i]); - pgo_poses[fused.ids[i]] = pose; - if (params->verbose) - printf("[D2VINS] PGO fused drone %d: %s\n", fused.ids[i], pose.toStr().c_str()); + if (params->enable_perf_output) + SPDLOG_INFO("[D2VINS] estimator_timer_callback takes {:.1f}ms", + estimator_timer.toc()); + bool discover_mode = false; + for (auto &id : ready_drones) { + if (pgo_poses.find(id) == pgo_poses.end() && + !estimator->getState().hasDrone(id)) { + discover_mode = true; + break; + } } - estimator->setPGOPoses(pgo_poses); + if (ret && D2FrontEnd::params->enable_network) { // Only send keyframes + if (params->lazy_broadcast_keyframe && !viokf.is_keyframe && + !discover_mode) { + continue; + } + std::set nearbydrones = + estimator->getNearbyDronesbyPGOData(vins_poses); + bool force_landmarks = false || discover_mode; + if (nearbydrones.size() > 0) { + force_landmarks = true; + spdlog::debug("[D2VINS] Nearby drones: "); + for (auto &id : nearbydrones) { + spdlog::debug("{}", id); + } + } + Utility::TicToc broadcast_timer; + loop_net->broadcastVisualImageDescArray(viokf, force_landmarks); + if (params->enable_perf_output) { + SPDLOG_INFO( + "[D2VINS] broadcastVisualImageDescArray takes %.1f ms\n", + broadcast_timer.toc()); + } + } + } + viokf_rate_ptr_->sleep(); } - - void distriburedTimerCallback(const ros::TimerEvent & e) { - estimator->solveinDistributedMode(); - updateOutModuleSldWinAndLandmarkDB(); + } + + virtual void imuCallback(const sensor_msgs::Imu &imu) { + IMUData data(imu); + if (!has_received_imu) { + has_received_imu = true; + last_imu_ts = imu.header.stamp.toSec(); } + data.dt = imu.header.stamp.toSec() - last_imu_ts; + last_imu_ts = imu.header.stamp.toSec(); + estimator->inputImu(data); + } - void solverThread() { - while (ros::ok()) { - if (need_solve) { - estimator->solveinDistributedMode(); - updateOutModuleSldWinAndLandmarkDB(); - if (!params->consensus_sync_to_start) { - need_solve = false; - } else { - usleep(0.5/params->estimator_timer_freq*1e6); - } - } else { - usleep(1000); - } - } + void pgoSwarmFusedCallback(const swarm_msgs::swarm_fused &fused) { + if (params->estimation_mode == D2Common::SINGLE_DRONE_MODE) { + return; } + for (size_t i = 0; i < fused.ids.size(); i++) { + Swarm::Pose pose(fused.local_drone_position[i], + fused.local_drone_rotation[i]); + pgo_poses[fused.ids[i]] = pose; + spdlog::debug("[D2VINS] PGO fused drone {}: {}", fused.ids[i], + pose.toStr()); + } + estimator->setPGOPoses(pgo_poses); + } + void distriburedTimerCallback(const ros::TimerEvent &e) { + estimator->solveinDistributedMode(); + updateOutModuleSldWinAndLandmarkDB(); + } - void Init(ros::NodeHandle & nh) { - D2Frontend::Init(nh); - initParams(nh); - estimator = new D2Estimator(params->self_id); - d2vins_net = new D2VINSNet(estimator, params->lcm_uri); - estimator->init(nh, d2vins_net); - visual_array_pub = nh.advertise("image_array_desc", 1); - imu_sub = nh.subscribe(params->imu_topic, 1000, &D2VINSNode::imuCallback, this, ros::TransportHints().tcpNoDelay()); //We need a big queue for IMU. - pgo_fused_sub = nh.subscribe("/d2pgo/swarm_fused", 1, &D2VINSNode::pgoSwarmFusedCallback, this, ros::TransportHints().tcpNoDelay()); - thread_viokf = std::thread([&] { - processVIOKFThread(); - printf("[D2VINS] processVIOKFThread exit.\n"); - }); - if (params->estimation_mode == D2VINSConfig::DISTRIBUTED_CAMERA_CONSENUS && params->consensus_sync_to_start) { - solver_timer = nh.createTimer(ros::Duration(1.0/params->estimator_timer_freq), &D2VINSNode::distriburedTimerCallback, this); - } else if (params->estimation_mode == D2VINSConfig::DISTRIBUTED_CAMERA_CONSENUS) { - thread_solver = std::thread([&] { - solverThread(); - }); - } - thread_comm = std::thread([&] { - ROS_INFO("Starting d2vins_net lcm."); - while(0 == d2vins_net->lcmHandle()) { - } - }); - ROS_INFO("D2VINS node %d initialized. Ready to start.", params->self_id); + void solverThread() { + //name thread + prctl(PR_SET_NAME, "D2VINS::solver", 0, 0, 0); + while (thread_solver_running) { + if (need_solve) { + estimator->solveinDistributedMode(); + updateOutModuleSldWinAndLandmarkDB(); + if (!params->consensus_sync_to_start) { + need_solve = false; + } + // else { + // usleep(0.5 / params->estimator_timer_freq * 1e6); + // } + } + solver_rate_ptr_->sleep(); } + } -public: - D2VINSNode(ros::NodeHandle & nh) { - Init(nh); + void Init(ros::NodeHandle &nh) { + D2Frontend::Init(nh); + initParams(nh); + estimator = new D2Estimator(params->self_id); + d2vins_net = new D2VINSNet(estimator, params->lcm_uri); + estimator->init(nh, d2vins_net); + visual_array_pub = + nh.advertise("image_array_desc", 1); + imu_sub = nh.subscribe( + params->imu_topic, 1000, &D2VINSNode::imuCallback, this, + ros::TransportHints().tcpNoDelay()); // We need a big queue for IMU. + pgo_fused_sub = nh.subscribe("/d2pgo/swarm_fused", 1, + &D2VINSNode::pgoSwarmFusedCallback, this, + ros::TransportHints().tcpNoDelay()); + // thread_viokf = std::thread([&] { + // processVIOKFThread(); + // SPDLOG_INFO("[D2VINS] processVIOKFThread exit."); + // }); + + thread_viokf_running = true; + viokf_rate_ptr_ = std::make_unique(params->estimator_timer_freq); + thread_viokf = std::thread([&] { + processVIOKFThread(); + SPDLOG_INFO("[D2VINS] processVIOKFThread exit."); + }); + if (params->estimation_mode == D2Common::DISTRIBUTED_CAMERA_CONSENUS && + params->consensus_sync_to_start) { + solver_timer = + nh.createTimer(ros::Duration(1.0 / params->estimator_timer_freq), + &D2VINSNode::distriburedTimerCallback, this); + } else if (params->estimation_mode == + D2Common::DISTRIBUTED_CAMERA_CONSENUS) { + thread_solver_running = true; + solver_rate_ptr_ = std::make_unique(params->estimator_timer_freq); + thread_solver = std::thread([&] { solverThread(); }); } + + thread_comm_running = true; + comm_rate_ptr_ = std::make_unique(1); + thread_comm = std::thread([&] { + SPDLOG_INFO("Starting d2vins_net lcm."); + while (thread_comm_running) { + //name thread + prctl(PR_SET_NAME, "D2VINS::lcm", 0, 0, 0); + while (0 == d2vins_net->lcmHandle()); + comm_rate_ptr_->sleep(); + } + }); + SPDLOG_INFO("D2VINS node {} initialized. Ready to start.", params->self_id); + } + + public: + D2VINSNode(ros::NodeHandle &nh) { Init(nh); } }; -int main(int argc, char **argv) -{ - cv::setNumThreads(1); - ros::init(argc, argv, "d2vins"); - ros::NodeHandle n("~"); - ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info); +int main(int argc, char **argv) { + spdlog::set_pattern("[%H:%M:%S][%^%l%$][%s,%!,L%#] %v"); + cv::setNumThreads(1); + ros::init(argc, argv, "d2vins"); + ros::NodeHandle n("~"); + ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, + ros::console::levels::Info); + // spdlog::set_level(spdlog::level::debug); - D2VINSNode d2vins(n); - ros::AsyncSpinner spinner(4); - spinner.start(); - ros::waitForShutdown(); - return 0; + D2VINSNode d2vins(n); + // ros::AsyncSpinner spinner(4); + // spinner.start(); + // ros::waitForShutdown(); + ros::spin(); + sleep(5); + d2vins.stopFrontend(); + d2vins.stopAllThread(); + return 0; } - diff --git a/d2vins/src/d2vins_params.cpp b/d2vins/src/d2vins_params.cpp index e0721696..0b9ae265 100644 --- a/d2vins/src/d2vins_params.cpp +++ b/d2vins/src/d2vins_params.cpp @@ -1,155 +1,180 @@ #include "d2vins_params.hpp" + #include -#include -#include -#include #include + +#include +#include +#include + +#include "factors/projectionOneFrameTwoCamFactor.h" #include "factors/projectionTwoFrameOneCamDepthFactor.h" #include "factors/projectionTwoFrameOneCamFactor.h" -#include "factors/projectionOneFrameTwoCamFactor.h" #include "factors/projectionTwoFrameTwoCamFactor.h" using namespace D2Common; namespace D2VINS { -D2VINSConfig * params = nullptr; - -void initParams(ros::NodeHandle & nh) { - params = new D2VINSConfig; - std::string vins_config_path; - nh.param("vins_config_path", vins_config_path, ""); - nh.param("lcm_uri", params->lcm_uri, "udpm://224.0.0.251:7667?ttl=1"); - nh.param("self_id", params->self_id, 0); - nh.param("enable_loop", params->enable_loop, true); - nh.param("main_id", params->main_id, 1); - params->init(vins_config_path); +D2VINSConfig* params = nullptr; + +void initParams(ros::NodeHandle& nh) { + params = new D2VINSConfig; + std::string vins_config_path; + nh.param("vins_config_path", vins_config_path, ""); + nh.param("lcm_uri", params->lcm_uri, + "udpm://224.0.0.251:7667?ttl=1"); + nh.param("self_id", params->self_id, 0); + nh.param("enable_loop", params->enable_loop, true); + nh.param("main_id", params->main_id, 1); + params->init(vins_config_path); } -void D2VINSConfig::init(const std::string & config_file) { - printf("[D2VINS::D2VINSConfig] readConfig from file %s\n", config_file.c_str()); - cv::FileStorage fsSettings; - try { - fsSettings.open(config_file.c_str(), cv::FileStorage::READ); - } catch(cv::Exception ex) { - std::cerr << "ERROR:" << ex.what() << " Can't open config file" << std::endl; - exit(-1); - } - - //Inputs - camera_num = fsSettings["num_of_cam"]; - IMU_FREQ = fsSettings["imu_freq"]; - max_imu_time_err = 1.5/IMU_FREQ; - frame_step = fsSettings["frame_step"]; - imu_topic = (std::string) fsSettings["imu_topic"]; - int _camconfig = fsSettings["camera_configuration"]; - camera_configuration = (CameraConfig) _camconfig; - - //Measurements - acc_n = fsSettings["acc_n"]; - acc_w = fsSettings["acc_w"]; - gyr_n = fsSettings["gyr_n"]; - gyr_w = fsSettings["gyr_w"]; - Eigen::Matrix noise = Eigen::Matrix::Zero(); - noise.block<3, 3>(0, 0) = (params->acc_n * params->acc_n) * Eigen::Matrix3d::Identity(); - noise.block<3, 3>(3, 3) = (params->gyr_n * params->gyr_n) * Eigen::Matrix3d::Identity(); - noise.block<3, 3>(6, 6) = (params->acc_n * params->acc_n) * Eigen::Matrix3d::Identity(); - noise.block<3, 3>(9, 9) = (params->gyr_n * params->gyr_n) * Eigen::Matrix3d::Identity(); - noise.block<3, 3>(12, 12) = (params->acc_w * params->acc_w) * Eigen::Matrix3d::Identity(); - noise.block<3, 3>(15, 15) = (params->gyr_w * params->gyr_w) * Eigen::Matrix3d::Identity(); - IntegrationBase::noise = noise; - - depth_sqrt_inf = fsSettings["depth_sqrt_inf"]; - IMUData::Gravity = Vector3d(0., 0., fsSettings["g_norm"]); - focal_length = D2FrontEnd::params->focal_length; - printf("[D2VINS::D2VINSConfig] VINS use focal length %.2f\n", focal_length); - - //Outputs - fsSettings["output_path"] >> output_folder; - debug_print_states = (int)fsSettings["debug_print_states"]; - enable_perf_output = (int)fsSettings["enable_perf_output"]; - debug_print_sldwin = (int)fsSettings["debug_print_sldwin"]; - debug_write_margin_matrix = (int)fsSettings["debug_write_margin_matrix"]; - verbose = (int) fsSettings["verbose"]; - print_network_status = (int) fsSettings["print_network_status"]; - - //Estimation - td_initial = fsSettings["td"]; - estimate_td = (int)fsSettings["estimate_td"]; - estimate_extrinsic = (int)fsSettings["estimate_extrinsic"]; - fuse_dep = (int) fsSettings["fuse_dep"]; - max_depth_to_fuse = fsSettings["max_depth_to_fuse"]; - min_depth_to_fuse = fsSettings["min_depth_to_fuse"]; - always_fixed_first_pose = (int) fsSettings["always_fixed_first_pose"]; - min_inv_dep = fsSettings["min_inv_dep"]; - max_solve_cnt = fsSettings["max_solve_cnt"]; - max_solve_measurements = fsSettings["max_solve_measurements"]; - min_measurements_per_keyframe = fsSettings["min_measurements_per_keyframe"]; - nearby_drone_dist = fsSettings["nearby_drone_dist"]; - //Multi-drone - estimation_mode = (ESTIMATION_MODE) (int) fsSettings["estimation_mode"]; - lazy_broadcast_keyframe = (int) fsSettings["lazy_broadcast_keyframe"]; - - //Initialiazation - init_method = (InitialMethod) (int)fsSettings["init_method"]; - depth_estimate_baseline = fsSettings["depth_estimate_baseline"]; - tri_max_err = fsSettings["tri_max_err"]; - - //Sliding window - max_sld_win_size = fsSettings["max_sld_win_size"]; - landmark_estimate_tracks = fsSettings["landmark_estimate_tracks"]; - min_solve_frames = fsSettings["min_solve_frames"]; - - //Outlier rejection - perform_outlier_rejection_num = fsSettings["perform_outlier_rejection_num"]; - landmark_outlier_threshold = fsSettings["thres_outlier"]; - - //Marginalization - margin_sparse_solver = (int)fsSettings["margin_sparse_solver"]; - enable_marginalization = (int)fsSettings["enable_marginalization"]; - remove_base_when_margin_remote = (int)fsSettings["remove_base_when_margin_remote"]; - margin_enable_fej = (int)fsSettings["margin_enable_fej"]; - - camera_extrinsics = D2FrontEnd::params->extrinsics; - - //Solver - solver_time = fsSettings["max_solver_time"]; - // options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;// ceres::DENSE_SCHUR; - // options.num_threads = 1; - // options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT;// ceres::DOGLEG; - // options.max_solver_time_in_seconds = solver_time; - ceres_options.linear_solver_type = ceres::DENSE_SCHUR; - ceres_options.num_threads = 1; - ceres_options.trust_region_strategy_type = ceres::DOGLEG; - ceres_options.max_solver_time_in_seconds = solver_time; - ceres_options.max_num_iterations = fsSettings["max_num_iterations"]; - - //Consenus Solver - consensus_config = new ConsensusSolverConfig; - consensus_config->ceres_options.linear_solver_type = ceres::DENSE_SCHUR; - if (!fsSettings["ceres_num_threads"].empty()) { - consensus_config->ceres_options.num_threads = (int)fsSettings["ceres_num_threads"]; - } else { - consensus_config->ceres_options.num_threads = 1; - } - consensus_config->ceres_options.trust_region_strategy_type = ceres::DOGLEG; - consensus_config->max_steps = fsSettings["consensus_max_steps"]; - consensus_config->ceres_options.max_num_iterations = ceres_options.max_num_iterations/consensus_config->max_steps; - consensus_config->ceres_options.max_solver_time_in_seconds = solver_time/consensus_config->max_steps; - consensus_config->self_id = self_id; - consensus_config->timout_wait_sync = fsSettings["timout_wait_sync"]; - consensus_config->rho_landmark = fsSettings["rho_landmark"]; - consensus_config->rho_frame_T = fsSettings["rho_frame_T"]; - consensus_config->rho_frame_theta = fsSettings["rho_frame_theta"]; - consensus_config->relaxation_alpha = fsSettings["relaxation_alpha"]; - consensus_config->sync_for_averaging = (int) fsSettings["consensus_sync_for_averaging"]; - consensus_sync_to_start = (int) fsSettings["consensus_sync_to_start"]; - - //Sqrt root information matrix - ProjectionTwoFrameOneCamFactor::sqrt_info = focal_length / 1.5 * Matrix2d::Identity(); - ProjectionOneFrameTwoCamFactor::sqrt_info = focal_length / 1.5 * Matrix2d::Identity(); - ProjectionTwoFrameTwoCamFactor::sqrt_info = focal_length / 1.5 * Matrix2d::Identity(); - ProjectionTwoFrameOneCamDepthFactor::sqrt_info = focal_length / 1.5 * Matrix3d::Identity(); - ProjectionTwoFrameOneCamDepthFactor::sqrt_info(2,2) = depth_sqrt_inf; +void D2VINSConfig::init(const std::string& config_file) { + printf("[D2VINS::D2VINSConfig] readConfig from file %s\n", + config_file.c_str()); + cv::FileStorage fsSettings; + try { + fsSettings.open(config_file.c_str(), cv::FileStorage::READ); + } catch (cv::Exception ex) { + std::cerr << "ERROR:" << ex.what() << " Can't open config file" + << std::endl; + exit(-1); + } + + // Inputs + camera_num = fsSettings["num_of_cam"]; + IMU_FREQ = fsSettings["imu_freq"]; + max_imu_time_err = 1.5 / IMU_FREQ; + frame_step = fsSettings["frame_step"]; + imu_topic = (std::string)fsSettings["imu_topic"]; + int _camconfig = fsSettings["camera_configuration"]; + camera_configuration = (CameraConfig)_camconfig; + + // Measurements + acc_n = fsSettings["acc_n"]; + acc_w = fsSettings["acc_w"]; + gyr_n = fsSettings["gyr_n"]; + gyr_w = fsSettings["gyr_w"]; + Eigen::Matrix noise = Eigen::Matrix::Zero(); + noise.block<3, 3>(0, 0) = + (params->acc_n * params->acc_n) * Eigen::Matrix3d::Identity(); + noise.block<3, 3>(3, 3) = + (params->gyr_n * params->gyr_n) * Eigen::Matrix3d::Identity(); + noise.block<3, 3>(6, 6) = + (params->acc_n * params->acc_n) * Eigen::Matrix3d::Identity(); + noise.block<3, 3>(9, 9) = + (params->gyr_n * params->gyr_n) * Eigen::Matrix3d::Identity(); + noise.block<3, 3>(12, 12) = + (params->acc_w * params->acc_w) * Eigen::Matrix3d::Identity(); + noise.block<3, 3>(15, 15) = + (params->gyr_w * params->gyr_w) * Eigen::Matrix3d::Identity(); + IntegrationBase::noise = noise; + + depth_sqrt_inf = fsSettings["depth_sqrt_inf"]; + IMUData::Gravity = Vector3d(0., 0., fsSettings["g_norm"]); + focal_length = D2FrontEnd::params->focal_length; + printf("[D2VINS::D2VINSConfig] VINS use focal length %.2f\n", focal_length); + + // Outputs + fsSettings["output_path"] >> output_folder; + debug_print_states = (int)fsSettings["debug_print_states"]; + enable_perf_output = (int)fsSettings["enable_perf_output"]; + debug_print_sldwin = (int)fsSettings["debug_print_sldwin"]; + debug_write_margin_matrix = (int)fsSettings["debug_write_margin_matrix"]; + verbose = (int)fsSettings["verbose"]; + print_network_status = (int)fsSettings["print_network_status"]; + + // Estimation + td_initial = fsSettings["td"]; + estimate_td = (int)fsSettings["estimate_td"]; + estimate_extrinsic = (int)fsSettings["estimate_extrinsic"]; + fuse_dep = (int)fsSettings["fuse_dep"]; + max_depth_to_fuse = fsSettings["max_depth_to_fuse"]; + min_depth_to_fuse = fsSettings["min_depth_to_fuse"]; + always_fixed_first_pose = (int)fsSettings["always_fixed_first_pose"]; + max_solve_cnt = fsSettings["max_solve_cnt"]; + max_solve_measurements = fsSettings["max_solve_measurements"]; + min_measurements_per_keyframe = fsSettings["min_measurements_per_keyframe"]; + nearby_drone_dist = fsSettings["nearby_drone_dist"]; + not_estimate_first_extrinsic = + (int)fsSettings["not_estimate_first_extrinsic"]; + + // Multi-drone + estimation_mode = (ESTIMATION_MODE)(int)fsSettings["estimation_mode"]; + lazy_broadcast_keyframe = (int)fsSettings["lazy_broadcast_keyframe"]; + + // Initialiazation + init_method = (InitialMethod)(int)fsSettings["init_method"]; + depth_estimate_baseline = fsSettings["depth_estimate_baseline"]; + tri_max_err = fsSettings["tri_max_err"]; + add_vel_ba_prior = (int)fsSettings["add_vel_ba_prior"]; + enable_sfm_initialization = (int)fsSettings["enable_sfm_initialization"]; + + // Sliding window + max_sld_win_size = fsSettings["max_sld_win_size"]; + landmark_estimate_tracks = fsSettings["landmark_estimate_tracks"]; + min_solve_frames = fsSettings["min_solve_frames"]; + + // Outlier rejection + perform_outlier_rejection_num = fsSettings["perform_outlier_rejection_num"]; + landmark_outlier_threshold = fsSettings["thres_outlier"]; + + // Marginalization + margin_sparse_solver = (int)fsSettings["margin_sparse_solver"]; + enable_marginalization = (int)fsSettings["enable_marginalization"]; + remove_base_when_margin_remote = + (int)fsSettings["remove_base_when_margin_remote"]; + margin_enable_fej = (int)fsSettings["margin_enable_fej"]; + + camera_extrinsics = D2FrontEnd::params->extrinsics; + + // Solver + solver_time = fsSettings["max_solver_time"]; + // options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;// + // ceres::DENSE_SCHUR; options.num_threads = 1; + // options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT;// + // ceres::DOGLEG; options.max_solver_time_in_seconds = solver_time; + ceres_options.linear_solver_type = ceres::DENSE_SCHUR; + ceres_options.num_threads = 1; + ceres_options.trust_region_strategy_type = ceres::DOGLEG; + ceres_options.max_solver_time_in_seconds = solver_time; + ceres_options.max_num_iterations = fsSettings["max_num_iterations"]; + + // Consenus Solver + consensus_config = new ConsensusSolverConfig; + consensus_config->ceres_options.linear_solver_type = ceres::DENSE_SCHUR; + if (!fsSettings["ceres_num_threads"].empty()) { + consensus_config->ceres_options.num_threads = + (int)fsSettings["ceres_num_threads"]; + } else { + consensus_config->ceres_options.num_threads = 1; + } + consensus_config->ceres_options.trust_region_strategy_type = ceres::DOGLEG; + consensus_config->max_steps = fsSettings["consensus_max_steps"]; + consensus_config->ceres_options.max_num_iterations = + ceres_options.max_num_iterations / consensus_config->max_steps; + consensus_config->ceres_options.max_solver_time_in_seconds = + solver_time / consensus_config->max_steps; + consensus_config->self_id = self_id; + consensus_config->timout_wait_sync = fsSettings["timout_wait_sync"]; + consensus_config->rho_landmark = fsSettings["rho_landmark"]; + consensus_config->rho_frame_T = fsSettings["rho_frame_T"]; + consensus_config->rho_frame_theta = fsSettings["rho_frame_theta"]; + consensus_config->relaxation_alpha = fsSettings["relaxation_alpha"]; + consensus_config->sync_for_averaging = + (int)fsSettings["consensus_sync_for_averaging"]; + consensus_sync_to_start = (int)fsSettings["consensus_sync_to_start"]; + + // Sqrt root information matrix + ProjectionTwoFrameOneCamFactor::sqrt_info = + focal_length / 1.5 * Matrix2d::Identity(); + ProjectionOneFrameTwoCamFactor::sqrt_info = + focal_length / 1.5 * Matrix2d::Identity(); + ProjectionTwoFrameTwoCamFactor::sqrt_info = + focal_length / 1.5 * Matrix2d::Identity(); + ProjectionTwoFrameOneCamDepthFactor::sqrt_info = + focal_length / 1.5 * Matrix3d::Identity(); + ProjectionTwoFrameOneCamDepthFactor::sqrt_info(2, 2) = depth_sqrt_inf; } -} \ No newline at end of file +} // namespace D2VINS \ No newline at end of file diff --git a/d2vins/src/d2vins_params.hpp b/d2vins/src/d2vins_params.hpp index d03c0554..377eed28 100644 --- a/d2vins/src/d2vins_params.hpp +++ b/d2vins/src/d2vins_params.hpp @@ -13,6 +13,7 @@ struct ConsensusSolverConfig; }; namespace D2VINS { using D2Common::CameraConfig; +using D2Common::ESTIMATION_MODE; struct D2VINSConfig { //Inputs std::string imu_topic; @@ -24,6 +25,9 @@ struct D2VINSConfig { double gyr_w = 0.0004; double focal_length = 460.0; double initial_pos_sqrt_info = 1000.0; + double initial_vel_sqrt_info = 100.0; + double initial_ba_sqrt_info = 5.0; + double initial_bg_sqrt_info = 10.0; double initial_yaw_sqrt_info = 10000.0; double initial_cam_pos_sqrt_info = 1000.0; double initial_cam_ang_sqrt_info = 10000.0; @@ -52,6 +56,11 @@ struct D2VINSConfig { InitialMethod init_method = INIT_POSE_PNP; double depth_estimate_baseline = 0.05; double tri_max_err = 0.1; + double mono_initial_tri_max_err = 0.05; + bool add_vel_ba_prior = false; + int solve_relative_pose_min_pts = 20; + double solve_relative_pose_min_parallex = 30.0/460.0; + bool enable_sfm_initialization = false; //Estimation bool estimate_td = false; @@ -65,19 +74,17 @@ struct D2VINSConfig { double process_input_timer = 100.0; double estimator_timer_freq = 10.0; int warn_pending_frames = 10; - enum ESTIMATION_MODE { - SINGLE_DRONE_MODE, //Not accept remote frame - SOLVE_ALL_MODE, //Each drone solve all the information - DISTRIBUTED_CAMERA_CONSENUS, //Distributed camera consensus - SERVER_MODE //In this mode receive all remote and solve them - } estimation_mode = SOLVE_ALL_MODE; + ESTIMATION_MODE estimation_mode; double estimate_extrinsic_vel_thres = 0.2; int max_solve_cnt = 10000; int max_solve_measurements = -1; + int min_solve_cnt = 10; + bool not_estimate_first_extrinsic = false; //Fuse depth bool fuse_dep = true; - double min_inv_dep = 1e-1; //10 meter away + double max_inv_dep = 1e1; //10 cm away + double default_inv_dep = 1e-1; double depth_sqrt_inf = 20.0; double max_depth_to_fuse = 5.; double min_depth_to_fuse = 0.3; diff --git a/d2vins/src/estimator/ParamResidualInfo.cpp b/d2vins/src/estimator/ParamResidualInfo.cpp index 494d9424..28a717a6 100644 --- a/d2vins/src/estimator/ParamResidualInfo.cpp +++ b/d2vins/src/estimator/ParamResidualInfo.cpp @@ -1,75 +1,76 @@ #include "ParamResidualInfo.hpp" -#include "d2vinsstate.hpp" + #include "../factors/prior_factor.h" +#include "d2vinsstate.hpp" namespace D2VINS { -PriorResInfo::PriorResInfo(PriorFactor * _factor) - :ResidualInfo(PriorResidual) { - cost_function = _factor; - factor = _factor; +PriorResInfo::PriorResInfo(PriorFactor* _factor) : ResidualInfo(PriorResidual) { + cost_function = _factor; + factor = _factor; } -bool PriorResInfo::relavant(const std::set & frame_ids) const { - //Prior relavant to all frames. - return true; +bool PriorResInfo::relavant(const std::set& frame_ids) const { + // Prior relavant to all frames. + return true; } -std::vector PriorResInfo::paramsList(D2State * state) const { - return factor->getKeepParams(); +std::vector PriorResInfo::paramsList(D2State* state) const { + return factor->getKeepParams(); } -ParamInfo createExtrinsic(D2EstimatorState * state, int camera_id) { - ParamInfo info; - info.pointer = state->getExtrinsicState(camera_id); - info.index = -1; - info.size = POSE_SIZE; - info.eff_size = POSE_EFF_SIZE; - info.type = EXTRINSIC; - info.id = camera_id; - info.data_copied = Map(info.pointer, info.size); - return info; +ParamInfo createExtrinsic(D2EstimatorState* state, int camera_id) { + ParamInfo info; + info.pointer = state->getExtrinsicState(camera_id); + info.index = -1; + info.size = POSE_SIZE; + info.eff_size = POSE_EFF_SIZE; + info.type = EXTRINSIC; + info.id = camera_id; + info.data_copied = Map(info.pointer, info.size); + return info; } -ParamInfo createLandmark(D2EstimatorState * state, int landmark_id, bool inv_dep_param) { - ParamInfo info; - info.pointer = state->getLandmarkState(landmark_id); - info.index = -1; - if (inv_dep_param) { - info.size = INV_DEP_SIZE; - info.eff_size = INV_DEP_SIZE; - } else { - info.size = POS_SIZE; - info.eff_size = POS_SIZE; - } - info.type = LANDMARK; - info.id = landmark_id; - info.data_copied = Map(info.pointer, info.size); - return info; +ParamInfo createLandmark(D2EstimatorState* state, int landmark_id, + bool inv_dep_param) { + ParamInfo info; + info.pointer = state->getLandmarkState(landmark_id); + info.index = -1; + if (inv_dep_param) { + info.size = INV_DEP_SIZE; + info.eff_size = INV_DEP_SIZE; + } else { + info.size = POS_SIZE; + info.eff_size = POS_SIZE; + } + info.type = LANDMARK; + info.id = landmark_id; + info.data_copied = Map(info.pointer, info.size); + return info; } -ParamInfo createSpeedBias(D2EstimatorState * state, FrameIdType id) { - ParamInfo info; - info.pointer = state->getSpdBiasState(id); - info.index = -1; - info.size = FRAME_SPDBIAS_SIZE; - info.eff_size = FRAME_SPDBIAS_SIZE; - info.type = SPEED_BIAS; - info.id = id; - info.data_copied = Map(info.pointer, info.size); - return info; +ParamInfo createSpeedBias(D2EstimatorState* state, FrameIdType id) { + ParamInfo info; + info.pointer = state->getSpdBiasState(id); + info.index = -1; + info.size = FRAME_SPDBIAS_SIZE; + info.eff_size = FRAME_SPDBIAS_SIZE; + info.type = SPEED_BIAS; + info.id = id; + info.data_copied = Map(info.pointer, info.size); + return info; } -ParamInfo createTd(D2EstimatorState * state, int camera_id) { - ParamInfo info; - info.pointer = state->getTdState(camera_id); - info.index = -1; - info.size = TD_SIZE; - info.eff_size = TD_SIZE; - info.type = TD; - info.id = camera_id; - info.data_copied = Map(info.pointer, info.size); - return info; +ParamInfo createTd(D2EstimatorState* state, int camera_id) { + ParamInfo info; + info.pointer = state->getTdState(camera_id); + info.index = -1; + info.size = TD_SIZE; + info.eff_size = TD_SIZE; + info.type = TD; + info.id = camera_id; + info.data_copied = Map(info.pointer, info.size); + return info; } -} \ No newline at end of file +} // namespace D2VINS \ No newline at end of file diff --git a/d2vins/src/estimator/d2estimator.cpp b/d2vins/src/estimator/d2estimator.cpp index 242dbe59..1d6b08a5 100644 --- a/d2vins/src/estimator/d2estimator.cpp +++ b/d2vins/src/estimator/d2estimator.cpp @@ -1,893 +1,990 @@ +#include "d2estimator.hpp" + +#include +#include + #include -#include "d2estimator.hpp" -#include "unistd.h" -#include "../factors/imu_factor.h" + #include "../factors/depth_factor.h" +#include "../factors/imu_factor.h" #include "../factors/prior_factor.h" +#include "../factors/projectionOneFrameTwoCamFactor.h" #include "../factors/projectionTwoFrameOneCamDepthFactor.h" #include "../factors/projectionTwoFrameOneCamFactor.h" -#include "../factors/projectionOneFrameTwoCamFactor.h" #include "../factors/projectionTwoFrameTwoCamFactor.h" -#include -#include -#include "marginalization/marginalization.hpp" -#include "solver/VINSConsenusSolver.hpp" #include "../network/d2vins_net.hpp" +#include "marginalization/marginalization.hpp" #include "solver/ConsensusSync.hpp" +#include "solver/VINSConsenusSolver.hpp" +#include "spdlog/spdlog.h" +#include "unistd.h" namespace D2VINS { -D2Estimator::D2Estimator(int drone_id): - self_id(drone_id), state(drone_id) { - sync_data_receiver = new SyncDataReceiver; -} - -void D2Estimator::init(ros::NodeHandle & nh, D2VINSNet * net) { - state.init(params->camera_extrinsics, params->td_initial); - visual.init(nh, this); - printf("[D2Estimator::init] init done estimator on drone %d\n", self_id); - for (auto cam_id : state.getAvailableCameraIds()) { - Swarm::Pose ext = state.getExtrinsic(cam_id); - printf("[D2VINS::D2Estimator] extrinsic %d: %s\n", cam_id, ext.toStr().c_str()); - } - vinsnet = net; - vinsnet->DistributedVinsData_callback = [&](DistributedVinsData msg) { - onDistributedVinsData(msg); - }; - vinsnet->DistributedSync_callback = [&](int drone_id, int signal, int64_t token) { - onSyncSignal(drone_id, signal, token); - }; - - imu_bufs[self_id] = IMUBuffer(); - if (params->estimation_mode == D2VINSConfig::DISTRIBUTED_CAMERA_CONSENUS) { - solver = new D2VINSConsensusSolver(this, &state, sync_data_receiver, *params->consensus_config, solve_token); - } else { - solver = new CeresSolver(&state, params->ceres_options); - } +D2Estimator::D2Estimator(int drone_id) : self_id(drone_id), state(drone_id) { + sync_data_receiver = new SyncDataReceiver; +} + +void D2Estimator::init(ros::NodeHandle &nh, D2VINSNet *net) { + state.init(params->camera_extrinsics, params->td_initial); + visual.init(nh, this); + SPDLOG_INFO("init done estimator on drone {}", self_id); + for (auto cam_id : state.getAvailableCameraIds()) { + Swarm::Pose ext = state.getExtrinsic(cam_id); + SPDLOG_INFO("extrinsic {}: {}", cam_id, ext.toStr()); + } + vinsnet = net; + vinsnet->DistributedVinsData_callback = [&](DistributedVinsData msg) { + onDistributedVinsData(msg); + }; + vinsnet->DistributedSync_callback = [&](int drone_id, int signal, + int64_t token) { + onSyncSignal(drone_id, signal, token); + }; + + imu_bufs[self_id] = IMUBuffer(); + if (params->estimation_mode == D2Common::DISTRIBUTED_CAMERA_CONSENUS) { + solver = new D2VINSConsensusSolver(this, &state, sync_data_receiver, + *params->consensus_config, solve_token); + } else { + solver = new CeresSolver(&state, params->ceres_options); + } } void D2Estimator::inputImu(IMUData data) { - IMUData last = data; - if (imu_bufs[self_id].size() > 0 ) { - last = imu_bufs[self_id].buf.back(); - } - imu_bufs[self_id].add(data); - if (!initFirstPoseFlag || solve_count == 0) { - return; - } - //Propagation current with last Bias. - auto last_frame = state.lastFrame(self_id); - std::lock_guard lock(imu_prop_lock); - data.propagation(last_prop_odom[params->self_id], last_frame.Ba, last_frame.Bg, last); - visual.pubIMUProp(last_prop_odom[params->self_id]); -} - -bool D2Estimator::tryinitFirstPose(VisualImageDescArray & frame) { - auto ret = imu_bufs[self_id].periodIMU(-1, frame.stamp + state.getTd(frame.drone_id)); - auto _imubuf = ret.first; - if (_imubuf.size() < params->init_imu_num) { - printf("[D2Estimator::tryinitFirstPose] not enough imu data %d/%d for init\n", _imubuf.size(), imu_bufs[self_id].size()); - return false; - } - auto mean_acc = _imubuf.mean_acc(); - auto q0 = Utility::g2R(mean_acc); - auto last_odom = Swarm::Odometry(frame.stamp, Swarm::Pose(q0, Vector3d::Zero())); - - //Easily use the average value as gyrobias now - //Also the ba with average acc - g - VINSFrame first_frame(frame, mean_acc - q0.inverse()*IMUData::Gravity, _imubuf.mean_gyro()); - first_frame.is_keyframe = true; - first_frame.odom = last_odom; - first_frame.imu_buf_index = ret.second; - first_frame.reference_frame_id = state.getReferenceFrameId(); - - state.addFrame(frame, first_frame); - - printf("\033[0;32m[D2VINS::D2Estimator] Initial firstPose %ld\n", frame.frame_id); - printf("[D2VINS::D2Estimator] Init pose with IMU: %s\n", last_odom.toStr().c_str()); - printf("[D2VINS::D2Estimator] Mean acc %.3f %.3f %.3f\n", mean_acc.x(), mean_acc.y(), mean_acc.z()); - printf("[D2VINS::D2Estimator] Gyro bias: %.3f %.3f %.3f\n", first_frame.Bg.x(), first_frame.Bg.y(), first_frame.Bg.z()); - printf("[D2VINS::D2Estimator] Acc bias: %.3f %.3f %.3f\033[0m\n\n", first_frame.Ba.x(), first_frame.Ba.y(), first_frame.Ba.z()); - - frame.reference_frame_id = state.getReferenceFrameId(); - frame.pose_drone = first_frame.odom.pose(); - frame.Ba = first_frame.Ba; - frame.Bg = first_frame.Bg; - frame.setTd(state.getTd(frame.drone_id)); - return true; -} - -std::pair D2Estimator::initialFramePnP(const VisualImageDescArray & frame, const Swarm::Pose & initial_pose) { - //Only use first image for initialization. - std::vector lm_positions_a, lm_3d_norm_b; - std::vector cam_extrinsics; - std::vector camera_indices, inliers; - //Compute pose with computePosePnPnonCentral - int i = 0; - for (auto image: frame.images) { - cam_extrinsics.push_back(image.extrinsic); - for (auto & lm: image.landmarks) { - auto & lm_id = lm.landmark_id; - if (state.hasLandmark(lm_id)) { - auto & est_lm = state.getLandmarkbyId(lm_id); - if (est_lm.flag >= LandmarkFlag::INITIALIZED) { - lm_positions_a.push_back(est_lm.position); - lm_3d_norm_b.push_back(lm.pt3d_norm); - camera_indices.push_back(lm.camera_index); - } - } - } - } - D2Common::Utility::TicToc tic; - auto pose_imu = D2FrontEnd::computePosePnPnonCentral(lm_positions_a, lm_3d_norm_b, cam_extrinsics, camera_indices, inliers); - bool success = inliers.size() > params->pnp_min_inliers; - if (frame.drone_id != self_id) { - printf("[D2VINS::D2Estimator@%d] PnP succ %d frame %ld@%d final %s inliers %d points %d time: %.2fms\n", self_id, success, - frame.frame_id, frame.drone_id, pose_imu.toStr().c_str(), inliers.size(), lm_positions_a.size(), tic.toc()); - } - return std::make_pair(success, pose_imu); -} - -VINSFrame * D2Estimator::addFrame(VisualImageDescArray & _frame) { - //First we init corresponding pose for with IMU - auto & last_frame = state.lastFrame(); - auto motion_predict = getMotionPredict(_frame.stamp); //Redo motion predict for get latest initial pose - VINSFrame frame(_frame, motion_predict.second, last_frame); - if (params->init_method == D2VINSConfig::INIT_POSE_IMU) { - frame.odom = motion_predict.first; + IMUData last = data; + if (imu_bufs[self_id].size() > 0) { + last = imu_bufs[self_id].buf.back(); + } + imu_bufs[self_id].add(data); + if (!initFirstPoseFlag || !isInitialized()) { + return; + } + // Propagation current with last Bias. + auto last_frame = state.lastFrame(self_id); + std::lock_guard lock(imu_prop_lock); + data.propagation(last_prop_odom[params->self_id], last_frame.Ba, + last_frame.Bg, last); + visual.pubIMUProp(last_prop_odom[params->self_id]); +} + +bool D2Estimator::tryinitFirstPose(VisualImageDescArray &frame) { + auto ret = imu_bufs[self_id].periodIMU( + -1, frame.stamp + state.getTd(frame.drone_id)); + auto _imubuf = ret.first; + if (_imubuf.size() < params->init_imu_num) { + SPDLOG_INFO("not enough imu data {}/{} for init", _imubuf.size(), + imu_bufs[self_id].size()); + return false; + } + auto mean_acc = _imubuf.mean_acc(); + auto q0 = Utility::g2R(mean_acc); + auto last_odom = + Swarm::Odometry(frame.stamp, Swarm::Pose(q0, Vector3d::Zero())); + + // Easily use the average value as gyrobias now + // Also the ba with average acc - g + VINSFrame first_frame(frame, mean_acc - q0.inverse() * IMUData::Gravity, + _imubuf.mean_gyro()); + first_frame.is_keyframe = true; + first_frame.odom = last_odom; + first_frame.imu_buf_index = ret.second; + first_frame.reference_frame_id = state.getReferenceFrameId(); + state.addFrame(frame, first_frame); + + SPDLOG_INFO("===============Initialization============="); + SPDLOG_INFO("Initial firstPose {}", frame.frame_id); + SPDLOG_INFO("Init pose with IMU: {}", last_odom.toStr()); + SPDLOG_INFO("Mean acc {:.6f} {:.6f} {:.6f}", mean_acc.x(), mean_acc.y(), + mean_acc.z()); + SPDLOG_INFO("Gyro bias: {:.6f} {:.6f} {:.6f}", first_frame.Bg.x(), + first_frame.Bg.y(), first_frame.Bg.z()); + SPDLOG_INFO("Acc bias: {:.6f} {:.6f} {:.6f}\n", first_frame.Ba.x(), + first_frame.Ba.y(), first_frame.Ba.z()); + SPDLOG_INFO("=========================================="); + + frame.reference_frame_id = state.getReferenceFrameId(); + frame.pose_drone = first_frame.odom.pose(); + frame.Ba = first_frame.Ba; + frame.Bg = first_frame.Bg; + frame.setTd(state.getTd(frame.drone_id)); + return true; +} + +std::pair D2Estimator::initialFramePnP( + const VisualImageDescArray &frame, const Swarm::Pose &initial_pose) { + // Only use first image for initialization. + std::vector lm_positions_a, lm_3d_norm_b; + std::vector cam_extrinsics; + std::vector camera_indices, inliers; + // Compute pose with computePosePnPnonCentral + int i = 0; + for (auto image : frame.images) { + cam_extrinsics.push_back(image.extrinsic); + for (auto &lm : image.landmarks) { + auto &lm_id = lm.landmark_id; + if (state.hasLandmark(lm_id)) { + auto &est_lm = state.getLandmarkbyId(lm_id); + if (est_lm.flag >= LandmarkFlag::INITIALIZED) { + lm_positions_a.push_back(est_lm.position); + lm_3d_norm_b.push_back(lm.pt3d_norm); + camera_indices.push_back(lm.camera_index); + } + } + } + } + D2Common::Utility::TicToc tic; + auto pose_imu = D2FrontEnd::computePosePnPnonCentral( + lm_positions_a, lm_3d_norm_b, cam_extrinsics, camera_indices, inliers); + bool success = inliers.size() > params->pnp_min_inliers; + if (frame.drone_id != self_id) { + SPDLOG_INFO( + "D{} PnP succ {} frame {}@{} final {} inliers {} points {} " + "time: {:.2f}ms", + self_id, success, frame.frame_id, frame.drone_id, pose_imu.toStr(), + inliers.size(), lm_positions_a.size(), tic.toc()); + } + return std::make_pair(success, pose_imu); +} + +VINSFrame *D2Estimator::addFrame(VisualImageDescArray &_frame) { + // First we init corresponding pose for with IMU + auto &last_frame = state.lastFrame(); + auto motion_predict = getMotionPredict( + _frame.stamp); // Redo motion predict for get latest initial pose + VINSFrame frame(_frame, motion_predict.second, last_frame); + if (params->init_method == D2VINSConfig::INIT_POSE_IMU) { + frame.odom = motion_predict.first; + } else { + auto odom_imu = motion_predict.first; + auto pnp_init = initialFramePnP(_frame, last_frame.odom.pose()); + if (!pnp_init.first) { + // Use IMU + SPDLOG_WARN("Initialization failed, use IMU instead."); } else { - auto odom_imu = motion_predict.first; - auto pnp_init = initialFramePnP(_frame, last_frame.odom.pose()); - if (!pnp_init.first) { - //Use IMU - printf("\033[0;31m[D2VINS::D2Estimator] Initialization failed, use IMU instead.\033[0m\n"); - } else { - odom_imu.pose() = pnp_init.second; - } - frame.odom = odom_imu; - } - frame.odom.stamp = _frame.stamp; - frame.reference_frame_id = state.getReferenceFrameId(); - - auto frame_ret = state.addFrame(_frame, frame); - //Clear old frames after add - if (params->estimation_mode != D2VINSConfig::DISTRIBUTED_CAMERA_CONSENUS) { - margined_landmarks = state.clearUselessFrames(); - } - _frame.setTd(state.getTd(_frame.drone_id)); - //Assign IMU and initialization to VisualImageDescArray for broadcasting. - _frame.imu_buf = motion_predict.second.first; - _frame.pose_drone = frame.odom.pose(); - _frame.Ba = frame.Ba; - _frame.Bg = frame.Bg; - _frame.reference_frame_id = frame.reference_frame_id; - - if (params->verbose || params->debug_print_states) { - printf("[D2VINS::D2Estimator] Initialize VINSFrame with %d: %s\n", - params->init_method, frame.toStr().c_str()); - } - return frame_ret; -} - -void D2Estimator::addRemoteImuBuf(int drone_id, const IMUBuffer & imu_) { - if (imu_bufs.find(drone_id) == imu_bufs.end()) { - imu_bufs[drone_id] = imu_; - printf("[D2Estimator::addRemoteImuBuf] Assign imu buf to drone %d cur_size %d\n", drone_id, imu_bufs[drone_id].size()); + odom_imu.pose() = pnp_init.second; + } + frame.odom = odom_imu; + } + frame.odom.stamp = _frame.stamp; + frame.reference_frame_id = state.getReferenceFrameId(); + + auto frame_ret = state.addFrame(_frame, frame); + // Clear old frames after add + if (params->estimation_mode != D2Common::DISTRIBUTED_CAMERA_CONSENUS) { + margined_landmarks = state.clearUselessFrames( + isInitialized()); // Only marginlization when solved + } + _frame.setTd(state.getTd(_frame.drone_id)); + // Assign IMU and initialization to VisualImageDescArray for broadcasting. + _frame.imu_buf = motion_predict.second.first; + _frame.pose_drone = frame.odom.pose(); + _frame.Ba = frame.Ba; + _frame.Bg = frame.Bg; + _frame.reference_frame_id = frame.reference_frame_id; + + spdlog::debug("Initialize VINSFrame with {}: {}", params->init_method, + frame.toStr().c_str()); + return frame_ret; +} + +void D2Estimator::addRemoteImuBuf(int drone_id, const IMUBuffer &imu_) { + if (imu_bufs.find(drone_id) == imu_bufs.end()) { + imu_bufs[drone_id] = imu_; + SPDLOG_INFO("Assign imu buf to drone {} cur_size {}", drone_id, + imu_bufs[drone_id].size()); + } else { + auto &_imu_buf = imu_bufs.at(drone_id); + auto t_last = _imu_buf.t_last; + bool add_first = true; + for (size_t i = 0; i < imu_.size(); i++) { + if (imu_[i].t > t_last) { + if (add_first) { + if ((imu_[i].t - t_last) > params->max_imu_time_err) { + SPDLOG_INFO("Add remote imu buffer {}: dt {:.2f}ms", drone_id, + (imu_[i].t - t_last) * 1000); + } + add_first = false; + } + _imu_buf.add(imu_[i]); + } + } + } +} + +VINSFrame *D2Estimator::addFrameRemote(const VisualImageDescArray &_frame) { + if (params->estimation_mode == D2Common::SOLVE_ALL_MODE || + params->estimation_mode == D2Common::SERVER_MODE) { + addRemoteImuBuf(_frame.drone_id, _frame.imu_buf); + } + int r_drone_id = _frame.drone_id; + VINSFrame vinsframe; + auto _imu = _frame.imu_buf; + if (state.size(r_drone_id) > 0) { + auto last_frame = state.lastFrame(r_drone_id); + if (params->estimation_mode == D2Common::SOLVE_ALL_MODE || + params->estimation_mode == D2Common::SERVER_MODE) { + auto &imu_buf = imu_bufs.at(_frame.drone_id); + auto ret = + imu_buf.periodIMU(last_frame.imu_buf_index, _frame.stamp + state.td); + auto _imu = ret.first; + if (fabs(_imu.size() / (_frame.stamp - last_frame.stamp) - + params->IMU_FREQ) > 15) { + SPDLOG_WARN( + "Remote IMU error freq: {:.3f} start_t " + "{:.3f}/{:.3f} end_t {:.3f}/{:.3f}", + _imu.size() / (_frame.stamp - last_frame.stamp), + last_frame.stamp + state.td, _imu[0].t, _frame.stamp + state.td, + _imu[_imu.size() - 1].t); + } + vinsframe = VINSFrame(_frame, ret, last_frame); } else { - auto & _imu_buf = imu_bufs.at(drone_id); - auto t_last = _imu_buf.t_last; - bool add_first = true; - for (size_t i = 0; i < imu_.size(); i++) { - if (imu_[i].t > t_last) { - if (add_first) { - if ((imu_[i].t - t_last) > params->max_imu_time_err) { - printf("\033[0;31m[D2VINS::D2Estimator] Add remote imu buffer %d: dt %.2fms\033[0m\n", drone_id, (imu_[i].t - t_last)*1000); - } - add_first = false; - } - _imu_buf.add(imu_[i]); - } - } - } -} - -VINSFrame * D2Estimator::addFrameRemote(const VisualImageDescArray & _frame) { - if (params->estimation_mode == D2VINSConfig::SOLVE_ALL_MODE || params->estimation_mode == D2VINSConfig::SERVER_MODE) { - addRemoteImuBuf(_frame.drone_id, _frame.imu_buf); - } - int r_drone_id = _frame.drone_id; - VINSFrame vinsframe; - auto _imu = _frame.imu_buf; - if (state.size(r_drone_id) > 0 ) { - auto last_frame = state.lastFrame(r_drone_id); - if (params->estimation_mode == D2VINSConfig::SOLVE_ALL_MODE || params->estimation_mode == D2VINSConfig::SERVER_MODE) { - auto & imu_buf = imu_bufs.at(_frame.drone_id); - auto ret = imu_buf.periodIMU(last_frame.imu_buf_index, _frame.stamp + state.td); - auto _imu = ret.first; - if (fabs(_imu.size()/(_frame.stamp - last_frame.stamp) - params->IMU_FREQ) > 15) { - printf("\033[0;31m[D2VINS::D2Estimator] Remote IMU error freq: %.3f start_t %.3f/%.3f end_t %.3f/%.3f\033[0m\n", - _imu.size()/(_frame.stamp - last_frame.stamp), last_frame.stamp + state.td, _imu[0].t, - _frame.stamp + state.td, _imu[_imu.size()-1].t); - } - vinsframe = VINSFrame(_frame, ret, last_frame); - } else { - vinsframe = VINSFrame(_frame, _frame.Ba, _frame.Bg); - } - if (_frame.reference_frame_id != state.getReferenceFrameId()) { - auto ego_last = last_frame.initial_ego_pose; - auto pose_local_cur = _frame.pose_drone; - auto pred_cur_pose = last_frame.odom.pose() * ego_last.inverse()*pose_local_cur; - vinsframe.odom.pose() = pred_cur_pose; - // std::cout << "ego_last " << ego_last.toStr() << std::endl; - // std::cout << "pose_local_cur " << pose_local_cur.toStr() << std::endl; - // std::cout << "last_frame " << last_frame.odom.pose().toStr() << std::endl; - // std::cout << "pred_cur_pose " << pred_cur_pose.toStr() << std::endl; - if (params->verbose) { - printf("[D2VINS::D2Estimator] Initial remoteframe %ld@drone%d with ego-motion: %s\n", - _frame.frame_id, r_drone_id, pred_cur_pose.toStr().c_str()); - } - } + vinsframe = VINSFrame(_frame, _frame.Ba, _frame.Bg); + } + if (_frame.reference_frame_id != state.getReferenceFrameId()) { + auto ego_last = last_frame.initial_ego_pose; + auto pose_local_cur = _frame.pose_drone; + auto pred_cur_pose = + last_frame.odom.pose() * ego_last.inverse() * pose_local_cur; + vinsframe.odom.pose() = pred_cur_pose; + spdlog::debug("Initial remoteframe {}@{} with ego-motion: {}", + _frame.frame_id, r_drone_id, pred_cur_pose.toStr()); + } + } else { + // Need to init the first frame. + vinsframe = VINSFrame(_frame, _frame.Ba, _frame.Bg); + auto pnp_init = initialFramePnP(_frame, Swarm::Pose::Identity()); + if (!pnp_init.first) { + // Use IMU + spdlog::debug("Initialization failed for remote {}@{}. will not add", + _frame.frame_id, _frame.drone_id); + return nullptr; } else { - //Need to init the first frame. - vinsframe = VINSFrame(_frame, _frame.Ba, _frame.Bg); - auto pnp_init = initialFramePnP(_frame, Swarm::Pose::Identity()); - if (!pnp_init.first) { - //Use IMU - if (params->verbose) { - printf("\033[0;31m[D2VINS::D2Estimator] Initialization failed for remote %d@%d. will not add\033[0m\n", _frame.frame_id, _frame.drone_id); - } - return nullptr; - } else { - if (params->verbose) { - printf("\033[0;32m[D2VINS::D2Estimator] Initial first remoteframe@drone%d with PnP: %s\033[0m\n", r_drone_id, pnp_init.second.toStr().c_str()); - } - if (_frame.reference_frame_id < state.getReferenceFrameId() && params->estimation_mode == D2VINSConfig::DISTRIBUTED_CAMERA_CONSENUS) { - //In this case, we merge the current map to the remote. - auto P_w_ki = _frame.pose_drone * pnp_init.second.inverse(); - P_w_ki.set_yaw_only(); - state.moveAllPoses(_frame.reference_frame_id, P_w_ki); - printf("[D2VINS::D2Estimator] Merge map to reference frame %d@%d RP: %s\n", - _frame.reference_frame_id, _frame.drone_id, P_w_ki.toStr().c_str()); - } else { - vinsframe.odom.pose() = pnp_init.second; - } - } - } - - auto frame_ret = state.addFrame(_frame, vinsframe); - if (params->verbose || params->debug_print_states) { - printf("[D2VINS::D2Estimator] Add Remote VINSFrame with %d: %s IMU %d iskeyframe %d/%d\n", - _frame.drone_id, vinsframe.toStr().c_str(), _frame.imu_buf.size(), vinsframe.is_keyframe, _frame.is_keyframe); - } - return frame_ret; -} - -void D2Estimator::addSldWinToFrame(VisualImageDescArray & frame) { - for (int i = 0; i < state.size(); i ++) { - frame.sld_win_status.push_back(state.getFrame(i).frame_id); - } -} - -void D2Estimator::inputRemoteImage(VisualImageDescArray & frame) { - const Guard lock(frame_mutex); - if (solve_count == 0 && params->estimation_mode == D2VINSConfig::DISTRIBUTED_CAMERA_CONSENUS) { - //In consenus mode, we require first to be local initialized before deal with remote - return; - } - if (frame.sld_win_status.size() > 0) { - //We need to update the sliding window. - updateSldwin(frame.drone_id, frame.sld_win_status); - } - auto frame_ptr = addFrameRemote(frame); - if (params->estimation_mode == D2VINSConfig::SERVER_MODE && state.size(frame.drone_id) >= params->min_solve_frames) { - state.clearUselessFrames(); - solveNonDistrib(); - } - visual.pubFrame(frame_ptr); -} - -bool D2Estimator::inputImage(VisualImageDescArray & _frame) { - //Guard - const Guard lock(frame_mutex); - if(!initFirstPoseFlag) { - printf("[D2VINS::D2Estimator] tryinitFirstPose imu buf %ld\n", imu_bufs[self_id].size()); - initFirstPoseFlag = tryinitFirstPose(_frame); - return initFirstPoseFlag; - } - - double t_imu_frame = _frame.stamp + state.td; - while (!imu_bufs[self_id].available(t_imu_frame)) { - //Wait for IMU - usleep(2000); - printf("[D2VINS::D2Estimator] wait for imu...\n"); - } - - auto frame = addFrame(_frame); - if (state.size() >= params->min_solve_frames && params->estimation_mode != D2VINSConfig::DISTRIBUTED_CAMERA_CONSENUS) { - solveNonDistrib(); - } - addSldWinToFrame(_frame); - frame_count ++; - updated = true; + spdlog::debug("Initial first remoteframe@{} with PnP: {}", r_drone_id, + pnp_init.second.toStr()); + if (_frame.reference_frame_id < state.getReferenceFrameId() && + params->estimation_mode == D2Common::DISTRIBUTED_CAMERA_CONSENUS) { + // In this case, we merge the current map to the remote. + auto P_w_ki = _frame.pose_drone * pnp_init.second.inverse(); + P_w_ki.set_yaw_only(); + state.moveAllPoses(_frame.reference_frame_id, P_w_ki); + SPDLOG_INFO("Merge map to reference frame {}@{} RP: {}", + _frame.reference_frame_id, _frame.drone_id, P_w_ki.toStr()); + } else { + vinsframe.odom.pose() = pnp_init.second; + } + } + } + + auto frame_ret = state.addFrame(_frame, vinsframe); + spdlog::debug("Add Remote VINSFrame with {}: {} IMU {} iskeyframe {}/{}", + _frame.drone_id, vinsframe.toStr(), _frame.imu_buf.size(), + vinsframe.is_keyframe, _frame.is_keyframe); + return frame_ret; +} + +void D2Estimator::addSldWinToFrame(VisualImageDescArray &frame) { + for (int i = 0; i < state.size(); i++) { + frame.sld_win_status.push_back(state.getFrame(i).frame_id); + } +} + +void D2Estimator::inputRemoteImage(VisualImageDescArray &frame) { + const Guard lock(frame_mutex); + if (!isInitialized() && + params->estimation_mode == D2Common::DISTRIBUTED_CAMERA_CONSENUS) { + // In consenus mode, we require first to be local initialized before + // deal with remote + return; + } + if (frame.sld_win_status.size() > 0) { + // We need to update the sliding window. + updateSldwin(frame.drone_id, frame.sld_win_status); + } + auto frame_ptr = addFrameRemote(frame); + if (params->estimation_mode == D2Common::SERVER_MODE && + state.size(frame.drone_id) >= params->min_solve_frames) { + state.clearUselessFrames(); + solveNonDistrib(); + } + visual.pubFrame(frame_ptr); +} + +bool D2Estimator::inputImage(VisualImageDescArray &_frame) { + // Guard + const Guard lock(frame_mutex); + if (!initFirstPoseFlag) { + SPDLOG_INFO("tryinitFirstPose imu buf: {}", imu_bufs[self_id].size()); + initFirstPoseFlag = tryinitFirstPose(_frame); + return initFirstPoseFlag; + } + + if (!isInitialized() && !_frame.is_keyframe && !_frame.is_stereo && + params->enable_sfm_initialization) { + // Do add when not solved and not keyframe + return false; + } + + double t_imu_frame = _frame.stamp + state.td; + while (!imu_bufs[self_id].available(t_imu_frame)) { + // Wait for IMU + usleep(2000); + SPDLOG_WARN("wait for imu..."); + } + + auto frame = addFrame(_frame); + if (state.size() >= params->min_solve_frames && + params->estimation_mode != D2Common::DISTRIBUTED_CAMERA_CONSENUS) { + solveNonDistrib(); + } + addSldWinToFrame(_frame); + frame_count++; + updated = true; + if (isInitialized()) { visual.pubFrame(frame); - return true; + } + return true; } void D2Estimator::setStateProperties() { - ceres::Problem & problem = solver->getProblem(); - auto pose_local_param = new PoseLocalParameterization; - //set LocalParameterization - for (auto & drone_id : state.availableDrones()) { - if (state.size(drone_id) > 0) { - for (size_t i = 0; i < state.size(drone_id); i ++) { - auto frame_a = state.getFrame(drone_id, i); - auto pointer = state.getPoseState(frame_a.frame_id); - if (problem.HasParameterBlock(pointer)) { - problem.SetParameterization(pointer, pose_local_param); - } - } - } + ceres::Problem &problem = solver->getProblem(); + auto pose_local_param = new PoseLocalParameterization; + // set LocalParameterization + for (auto &drone_id : state.availableDrones()) { + if (state.size(drone_id) > 0) { + for (size_t i = 0; i < state.size(drone_id); i++) { + auto frame_a = state.getFrame(drone_id, i); + auto pointer = state.getPoseState(frame_a.frame_id); + if (problem.HasParameterBlock(pointer)) { + problem.SetParameterization(pointer, pose_local_param); + } + } + } + } + + bool is_first = true; + for (auto cam_id : state.getAvailableCameraIds()) { + auto pointer = state.getExtrinsicState(cam_id); + if (is_first && params->not_estimate_first_extrinsic && + state.getCameraBelonging(cam_id) == self_id) { + if (problem.HasParameterBlock(pointer)) { + problem.SetParameterBlockConstant(pointer); + is_first = false; + } else { + SPDLOG_CRITICAL( + "first extrinsic {} not in problem. This should be a bug " + "or feature tracking is totally wrong. Will print sldwin " + "for debugging...", + cam_id); + state.printSldWin(keyframe_measurements); + } } - - for (auto cam_id: state.getAvailableCameraIds()) { - auto pointer = state.getExtrinsicState(cam_id); - if (!problem.HasParameterBlock(pointer)) { - continue; - } - int drone_id = state.getCameraBelonging(cam_id); - if (!params->estimate_extrinsic || state.size(drone_id) < params->max_sld_win_size || - state.lastFrame().odom.vel().norm() < params->estimate_extrinsic_vel_thres) { - problem.SetParameterBlockConstant(state.getExtrinsicState(cam_id)); - } - problem.SetParameterization(state.getExtrinsicState(cam_id), pose_local_param); + if (!problem.HasParameterBlock(pointer)) { + continue; } - - for (auto lm_id: used_landmarks) { - auto pointer = state.getLandmarkState(lm_id); - if (!problem.HasParameterBlock(pointer)) { - continue; - } - //Set minimun landmark distance - problem.SetParameterLowerBound(pointer, 0, params->min_inv_dep); + int drone_id = state.getCameraBelonging(cam_id); + if (!params->estimate_extrinsic || + state.size(drone_id) < params->max_sld_win_size || + state.lastFrame().odom.vel().norm() < + params->estimate_extrinsic_vel_thres) { + problem.SetParameterBlockConstant(state.getExtrinsicState(cam_id)); } + problem.SetParameterization(state.getExtrinsicState(cam_id), + pose_local_param); + } - if (!params->estimate_td || state.size() < params->max_sld_win_size || - state.lastFrame().odom.vel().norm() < params->estimate_extrinsic_vel_thres) { - // printf("[D2Estimator::setStateProperties@%d] set td to fixed sld_size %d/%d \n", self_id, state.size(), params->max_sld_win_size); - problem.SetParameterBlockConstant(state.getTdState(self_id)); + for (auto lm_id : used_landmarks) { + auto pointer = state.getLandmarkState(lm_id); + if (!problem.HasParameterBlock(pointer)) { + continue; } + } - if (!state.getPrior() || params->always_fixed_first_pose) { - //As we added prior for first pose, we do not need to fix it. - problem.SetParameterBlockConstant( - state.getPoseState(state.firstFrame(self_id).frame_id)); - } + if (!params->estimate_td || state.size() < params->max_sld_win_size || + state.lastFrame().odom.vel().norm() < + params->estimate_extrinsic_vel_thres) { + // printf("[D2Estimator::setStateProperties@%d] set td to fixed sld_size + // %d/%d \n", self_id, state.size(), params->max_sld_win_size); + problem.SetParameterBlockConstant(state.getTdState(self_id)); + } + + if (!state.getPrior() || params->always_fixed_first_pose) { + // As we added prior for first pose, we do not need to fix it. + problem.SetParameterBlockConstant( + state.getPoseState(state.firstFrame(self_id).frame_id)); + } } bool D2Estimator::isMain() const { - return self_id == params->main_id; //Temp code/ + return self_id == params->main_id; // Temp code/ } -void D2Estimator::onDistributedVinsData(const DistributedVinsData & dist_data) { - if (params->verbose) { - printf("[D2Estimator::onDistributedVinsData@%d] drone %d solver_id %d iteration %d reference_frame_id %ld \n", self_id, - dist_data.drone_id, dist_data.solver_token, dist_data.iteration_count, dist_data.reference_frame_id); - } - if (dist_data.reference_frame_id == state.getReferenceFrameId()) { - sync_data_receiver->add(dist_data); - } +void D2Estimator::onDistributedVinsData(const DistributedVinsData &dist_data) { + spdlog::debug("D{} drone {} solver_id {} iteration {} reference_frame_id {}", + self_id, dist_data.drone_id, dist_data.solver_token, + dist_data.iteration_count, dist_data.reference_frame_id); + if (dist_data.reference_frame_id == state.getReferenceFrameId()) { + sync_data_receiver->add(dist_data); + } } void D2Estimator::onSyncSignal(int drone_id, int signal, int64_t token) { - if (signal == DSolverReady || signal==DSolverNonDist) { - ready_drones.insert(drone_id); - if (params->verbose) { - // printf("[D2VINS::D2Estimator@%d] Drone %d is ready. Currently ready drone %ld\n", self_id, drone_id, ready_drones.size()); - } - } - if (signal == DSolverStart || (signal==DSolverNonDist && isMain())) { - //First drone start or claim non dist - ready_to_start = true; - solve_token = token; - // printf("[D2Estimator::onSyncSignal@%d] Start signal received from %d.\n", self_id, drone_id); - } - if (isMain() && ready_drones.size() == state.availableDrones().size()) { - ready_to_start = true; - if (params->verbose) { - // printf("[D2VINS::D2Estimator@%d] All drones are ready. Main will start the optimization\n", self_id); - } - } + if (signal == DSolverReady || signal == DSolverNonDist) { + ready_drones.insert(drone_id); + } + if (signal == DSolverStart || (signal == DSolverNonDist && isMain())) { + // First drone start or claim non dist + ready_to_start = true; + solve_token = token; + } + if (isMain() && ready_drones.size() == state.availableDrones().size()) { + ready_to_start = true; + } } void D2Estimator::sendDistributedVinsData(DistributedVinsData data) { - data.reference_frame_id = state.getReferenceFrameId(); - vinsnet->sendDistributedVinsData(data); + data.reference_frame_id = state.getReferenceFrameId(); + vinsnet->sendDistributedVinsData(data); } void D2Estimator::sendSyncSignal(SyncSignal data, int64_t token) { - vinsnet->sendSyncSignal((int) data, token); + vinsnet->sendSyncSignal((int)data, token); } bool D2Estimator::readyForStart() { - if (state.availableDrones().size() == 1) { - return true; - } - return ready_to_start; + if (state.availableDrones().size() == 1) { + return true; + } + return ready_to_start; } void D2Estimator::waitForStart() { - D2Common::Utility::TicToc timer; - while(!readyForStart()) { - sendSyncSignal(SyncSignal::DSolverReady, -1); - usleep(100); - if (timer.toc() > params->wait_for_start_timout) { - break; - } - } - double time = timer.toc(); - if (params->verbose) { - printf("[D2Estimator::waitForStart@%d] Wait for start time %f \n", self_id, timer.toc()); - } - if (time > 100) { - ROS_WARN("[D2Estimator::waitForStart@%d] Wait for start time long: %.1f \n", self_id, timer.toc()); + D2Common::Utility::TicToc timer; + while (!readyForStart()) { + sendSyncSignal(SyncSignal::DSolverReady, -1); + usleep(100); + if (timer.toc() > params->wait_for_start_timout) { + break; } + } + double time = timer.toc(); + spdlog::debug("D{} Wait for start time {:.f}", self_id, timer.toc()); + if (time > 100) { + SPDLOG_WARN("D{} Wait for start time long: {:.1f}", self_id, timer.toc()); + } } void D2Estimator::resetMarginalizer() { - if (marginalizer!=nullptr) { - delete marginalizer; - } - marginalizer = new Marginalizer(&state, state.getPrior()); - state.setMarginalizer(marginalizer); + if (marginalizer != nullptr) { + delete marginalizer; + } + marginalizer = new Marginalizer(&state, state.getPrior()); + state.setMarginalizer(marginalizer); } void D2Estimator::solveinDistributedMode() { - if (!updated) { - return; - } - updated = false; - D2Common::Utility::TicToc tic; - if (params->consensus_sync_to_start) { - if (true) { - ready_drones = std::set{self_id}; - if (params->verbose) { - printf("[D2VINS::D2Estimator@%d] ready, wait for start signal...\n", self_id); - } - waitForStart(); - if (isMain()) { - solve_token += 1; - sendSyncSignal(SyncSignal::DSolverStart, solve_token); - } - static_cast(solver)->setToken(solve_token); - if (params->verbose) { - printf("[D2VINS::D2Estimator@%d] All drones read start solving token %d...\n", self_id, solve_token); - } - ready_to_start = false; - } else { - //Claim not use a distribured solver. - sendSyncSignal(SyncSignal::DSolverNonDist, solve_token); - } + if (!updated) { + return; + } + updated = false; + D2Common::Utility::TicToc tic; + if (params->consensus_sync_to_start) { + if (true) { + ready_drones = std::set{self_id}; + spdlog::debug("D{} ready, wait for start signal...", self_id); + waitForStart(); + if (isMain()) { + solve_token += 1; + sendSyncSignal(SyncSignal::DSolverStart, solve_token); + } + static_cast(solver)->setToken(solve_token); + spdlog::debug("D{} All drones read start solving token {}...", self_id, + solve_token); + ready_to_start = false; } else { - if (params->verbose) { - printf("[D2VINS::D2Estimator@%d] async solve...\n", self_id); - } - } - - const Guard lock(frame_mutex); - //We do not check update, just do optimization everytime go here - if (state.size() < params->min_solve_frames) { - //We do not have enough frames to solve. - return; - } - - margined_landmarks = state.clearUselessFrames(); // clear in dist mode. - resetMarginalizer(); - state.preSolve(imu_bufs); - solver->reset(); - - setupImuFactors(); - setupLandmarkFactors(); - setupPriorFactor(); - if (params->enable_perf_output) { - printf("[D2VINS::solveDist: beforeSolve time cost %.1f ms\n", tic.toc()); - } - auto report = solver->solve(); - state.syncFromState(used_landmarks); - - //Now do some statistics - static double sum_time = 0; - static double sum_iteration = 0; - static double sum_cost = 0; - sum_time += report.total_time; - sum_iteration += report.total_iterations; - sum_cost += report.final_cost; - - if (params->enable_perf_output) { - printf("[D2VINS::solveDist@%d] average time %.1fms, average time of iter: %.1fms, average iteration %.3f, average cost %.3f\n", - self_id, sum_time*1000/solve_count, sum_time*1000/sum_iteration, sum_iteration/solve_count, sum_cost/solve_count); - } - - auto last_odom = state.lastFrame().odom; - printf("[D2VINS::solveDist@%d](%d) odom %s@ref%d landmarks %d/%d v_mea %d/%d drone_num %d opti_time %.1fms steps %d td %.1fms \n", - self_id, solve_count, last_odom.toStr().c_str(), state.getReferenceFrameId(), used_landmarks.size(), current_landmark_num, - current_measurement_num, params->max_solve_measurements, state.availableDrones().size(), report.total_time*1000, report.total_iterations, state.td*1000); - - // Reprogation - for (auto drone_id : state.availableDrones()) { - if (drone_id != self_id && params->estimation_mode == D2VINSConfig::DISTRIBUTED_CAMERA_CONSENUS) { - continue; - } - auto _imu = imu_bufs[self_id].tail(state.lastFrame(drone_id).stamp + state.td); - std::lock_guard lock(imu_prop_lock); - last_prop_odom[drone_id] = _imu.propagation(state.lastFrame(drone_id)); - } + // Claim not use a distribured solver. + sendSyncSignal(SyncSignal::DSolverNonDist, solve_token); + } + } else { + spdlog::debug("D{} async solve...", self_id); + } + + const Guard lock(frame_mutex); + // We do not check update, just do optimization everytime go here + if (state.size() < params->min_solve_frames) { + // We do not have enough frames to solve. + return; + } + + margined_landmarks = state.clearUselessFrames(); // clear in dist mode. + resetMarginalizer(); + state.preSolve(imu_bufs); + solver->reset(); + + setupImuFactors(); + setupLandmarkFactors(); + setupPriorFactor(); + if (params->enable_perf_output) { + SPDLOG_INFO("beforeSolve time cost {:.1f} ms", tic.toc()); + } + auto report = solver->solve(); + state.syncFromState(used_landmarks); + + // Now do some statistics + static double sum_time = 0; + static double sum_iteration = 0; + static double sum_cost = 0; + sum_time += report.total_time; + sum_iteration += report.total_iterations; + sum_cost += report.final_cost; + + if (params->enable_perf_output) { + SPDLOG_INFO( + "D{} average time {:.1f}ms, average time of iter: " + "{:.1f}ms, average iteration {:.3f}, average cost {:.3f}", + self_id, sum_time * 1000 / solve_count, sum_time * 1000 / sum_iteration, + sum_iteration / solve_count, sum_cost / solve_count); + } + + auto last_odom = state.lastFrame().odom; + SPDLOG_INFO( + "D{}({}) odom {}@ref{} landmarks {}/{} v_mea {}/{} drone_num {} " + "opti_time {:.1f}ms steps {} td {:.1f}ms", + self_id, solve_count, last_odom.toStr(), state.getReferenceFrameId(), + used_landmarks.size(), current_landmark_num, current_measurement_num, + params->max_solve_measurements, state.availableDrones().size(), + report.total_time * 1000, report.total_iterations, state.td * 1000); + + // Reprogation + for (auto drone_id : state.availableDrones()) { + if (drone_id != self_id && + params->estimation_mode == D2Common::DISTRIBUTED_CAMERA_CONSENUS) { + continue; + } + auto _imu = + imu_bufs[self_id].tail(state.lastFrame(drone_id).stamp + state.td); + std::lock_guard lock(imu_prop_lock); + last_prop_odom[drone_id] = _imu.propagation(state.lastFrame(drone_id)); + } - visual.postSolve(); + visual.postSolve(); - if (params->debug_print_states || params->debug_print_sldwin) { - state.printSldWin(keyframe_measurements); - } + if (params->debug_print_states || params->debug_print_sldwin) { + state.printSldWin(keyframe_measurements); + } - if (!report.succ) { - std::cout << report.message << std::endl; - exit(1); - } - // exit(0); - solve_count ++; - if (params->enable_perf_output) { - printf("[D2VINS::solveDist: total time cost %.1fms\n", tic.toc()); - } + if (!report.succ) { + std::cout << report.message << std::endl; + exit(1); + } + // exit(0); + solve_count++; + if (params->enable_perf_output) { + SPDLOG_INFO("[D2VINS::solveDist: total time cost {:.1f}ms", tic.toc()); + } } void D2Estimator::solveNonDistrib() { - resetMarginalizer(); - state.preSolve(imu_bufs); - solver->reset(); - setupImuFactors(); - setupLandmarkFactors(); - setupPriorFactor(); - setStateProperties(); - SolverReport report = solver->solve(); - state.syncFromState(used_landmarks); - - //Now do some statistics - static double sum_time = 0; - static double sum_iteration = 0; - static double sum_cost = 0; - sum_time += report.total_time; - sum_iteration += report.total_iterations; - sum_cost += report.final_cost; - - if (params->enable_perf_output) { - printf("[D2VINS] average time %.1fms, average time of iter: %.1fms, average iteration %.3f, average cost %.3f\n", - sum_time*1000/solve_count, sum_time*1000/sum_iteration, sum_iteration/solve_count, sum_cost/solve_count); - } - - if (params->estimation_mode < D2VINSConfig::SERVER_MODE) { - auto last_odom = state.lastFrame().odom; - printf("[D2VINS] solve_count %d landmarks %d odom %s td %.1fms opti_time %.1fms\n", solve_count, - current_landmark_num, last_odom.toStr().c_str(), state.td*1000, report.total_time*1000); + if (params->enable_sfm_initialization) { + if (state.numKeyframes() < params->min_solve_frames) { + SPDLOG_WARN("numKeyframes too less, skip optimization"); + return; } else { - printf("[D2VINS] solve_count %d landmarks %d td %.1fms opti_time %.1fms\n", solve_count, - current_landmark_num, state.td*1000, report.total_time*1000); - } - - // Reprogation - for (auto drone_id : state.availableDrones()) { - auto _imu = imu_bufs[self_id].tail(state.lastFrame(drone_id).stamp + state.td); - std::lock_guard lock(imu_prop_lock); - last_prop_odom[drone_id] = _imu.propagation(state.lastFrame(drone_id)); - } + if (!isInitialized()) { + SPDLOG_INFO("Initialization with {} keyframes", state.numKeyframes()); + if (state.monoInitialization()) { + SPDLOG_INFO("Mono initialization is success, turn to solve"); + } else { + SPDLOG_ERROR("Initialization failed, will try later\n"); + return; + } + } + } + } + resetMarginalizer(); + state.preSolve(imu_bufs); + solver->reset(); + setupImuFactors(); + setupLandmarkFactors(); + if (current_measurement_num < params->min_solve_cnt) { + SPDLOG_WARN("Landmark too less, skip optimization"); + return; + } + setupPriorFactor(); + setStateProperties(); + SolverReport report = solver->solve(); + state.syncFromState(used_landmarks); + + // Now do some statistics + static double sum_time = 0; + static double sum_iteration = 0; + static double sum_cost = 0; + sum_time += report.total_time; + sum_iteration += report.total_iterations; + sum_cost += report.final_cost; + + if (params->enable_perf_output) { + SPDLOG_INFO( + "average time {:.1f}ms, average time of iter: {:.1f}ms, " + "average iteration {:.3f}, average cost {:.3f}\n", + sum_time * 1000 / solve_count, sum_time * 1000 / sum_iteration, + sum_iteration / solve_count, sum_cost / solve_count); + } + + if (params->estimation_mode < D2Common::SERVER_MODE) { + auto last_odom = state.lastFrame().odom; + SPDLOG_INFO("C{} landmarks {} odom {} td {:.1f}ms opti_time {:.1f}ms", + solve_count, current_landmark_num, last_odom.toStr(), + state.td * 1000, report.total_time * 1000); + } else { + SPDLOG_INFO("C{} landmarks {} td {:1.f}ms opti_time {:.1f}ms", solve_count, + current_landmark_num, state.td * 1000, + report.total_time * 1000); + } + + // Reprogation + for (auto drone_id : state.availableDrones()) { + auto _imu = + imu_bufs[self_id].tail(state.lastFrame(drone_id).stamp + state.td); + std::lock_guard lock(imu_prop_lock); + last_prop_odom[drone_id] = _imu.propagation(state.lastFrame(drone_id)); + } - visual.postSolve(); + visual.postSolve(); - if (params->debug_print_states || params->debug_print_sldwin) { - state.printSldWin(keyframe_measurements); - } + if (params->debug_print_states || params->debug_print_sldwin) { + state.printSldWin(keyframe_measurements); + } - if (!report.succ) { - std::cout << report.message << std::endl; - exit(1); + if (!report.succ) { + std::cout << report.message << std::endl; + exit(1); + } + if (solve_count == 0) { + // Publish the initialized frames uisng visual.pubFrame + for (auto frame : state.getSldWin(self_id)) { + visual.pubFrame(frame); } + } + solve_count++; } -void D2Estimator::addIMUFactor(FrameIdType frame_ida, FrameIdType frame_idb, IntegrationBase* pre_integrations) { - IMUFactor* imu_factor = new IMUFactor(pre_integrations); - auto info = ImuResInfo::create(imu_factor, frame_ida, frame_idb); - solver->addResidual(info); - if (params->always_fixed_first_pose) { - //At this time we fix the first pose and ignore the margin of this imu factor to achieve better numerical stability - return; - } - marginalizer->addResidualInfo(info); - solve_count ++; +void D2Estimator::addIMUFactor(FrameIdType frame_ida, FrameIdType frame_idb, + IntegrationBase *pre_integrations) { + IMUFactor *imu_factor = new IMUFactor(pre_integrations); + auto info = ImuResInfo::create(imu_factor, frame_ida, frame_idb); + solver->addResidual(info); + if (params->always_fixed_first_pose) { + // At this time we fix the first pose and ignore the margin of this imu + // factor to achieve better numerical stability + return; + } + marginalizer->addResidualInfo(info); } void D2Estimator::setupImuFactors() { - if (state.size() > 1) { - for (size_t i = 0; i < state.size() - 1; i ++ ) { - auto & frame_a = state.getFrame(i); - auto & frame_b = state.getFrame(i + 1); - auto pre_integrations = frame_b.pre_integrations; //Prev to current - // printf("IMU Factor %d<->%d prev %d\n", frame_a.frame_id, frame_b.frame_id, frame_b.prev_frame_id); - assert(frame_b.prev_frame_id == frame_a.frame_id && "Wrong prev frame id"); - addIMUFactor(frame_a.frame_id, frame_b.frame_id, pre_integrations); - } - } - - // In non-distributed mode, we add IMU factor for each drone - if (params->estimation_mode == D2VINSConfig::SOLVE_ALL_MODE || params->estimation_mode == D2VINSConfig::SERVER_MODE) { - for (auto drone_id : state.availableDrones()) { - if (drone_id == self_id) { - continue; - } - if (state.size(drone_id) > 1) { - for (size_t i = 0; i < state.size(drone_id) - 1; i ++ ) { - auto & frame_a = state.getFrame(drone_id, i); - auto & frame_b = state.getFrame(drone_id, i + 1); - auto pre_integrations = frame_b.pre_integrations; //Prev to current - if (pre_integrations == nullptr) { - printf("\033[0;31m[D2VINS] Warning: frame %ld<->%ld@drone%d pre_integrations is nullptr.\033[0m\n", - frame_a.frame_id, frame_b.frame_id, drone_id); - continue; - } - assert(frame_b.prev_frame_id == frame_a.frame_id && "Wrong prev frame id on remote"); - addIMUFactor(frame_a.frame_id, frame_b.frame_id, pre_integrations); - } - } + if (state.size() > 1) { + for (size_t i = 0; i < state.size() - 1; i++) { + auto &frame_a = state.getFrame(i); + auto &frame_b = state.getFrame(i + 1); + auto pre_integrations = frame_b.pre_integrations; // Prev to current + // printf("IMU Factor %d<->%d prev %d\n", frame_a.frame_id, + // frame_b.frame_id, frame_b.prev_frame_id); + assert(frame_b.prev_frame_id == frame_a.frame_id && + "Wrong prev frame id"); + addIMUFactor(frame_a.frame_id, frame_b.frame_id, pre_integrations); + } + } + + // In non-distributed mode, we add IMU factor for each drone + if (params->estimation_mode == D2Common::SOLVE_ALL_MODE || + params->estimation_mode == D2Common::SERVER_MODE) { + for (auto drone_id : state.availableDrones()) { + if (drone_id == self_id) { + continue; + } + if (state.size(drone_id) > 1) { + for (size_t i = 0; i < state.size(drone_id) - 1; i++) { + auto &frame_a = state.getFrame(drone_id, i); + auto &frame_b = state.getFrame(drone_id, i + 1); + auto pre_integrations = frame_b.pre_integrations; // Prev to current + if (pre_integrations == nullptr) { + SPDLOG_WARN("frame {}<->{}@{} pre_integrations is nullptr.", + frame_a.frame_id, frame_b.frame_id, drone_id); + continue; + } + assert(frame_b.prev_frame_id == frame_a.frame_id && + "Wrong prev frame id on remote"); + addIMUFactor(frame_a.frame_id, frame_b.frame_id, pre_integrations); } + } } + } } bool D2Estimator::hasCommonLandmarkMeasurments() { - auto lms = state.availableLandmarkMeasurements(params->max_solve_cnt, params->max_solve_measurements); - for (auto lm : lms) { - if (lm.solver_id == -1 && lm.drone_id != self_id) { - // This is a internal only remote landmark - continue; - } - if (lm.solver_id > 0 && lm.solver_id != self_id) { - continue; - } - for (auto i = 0; i < lm.track.size(); i++) { - if (state.getFramebyId(lm.track[i].frame_id)->drone_id != self_id) { - return true; - } - } + auto lms = state.availableLandmarkMeasurements( + params->max_solve_cnt, params->max_solve_measurements); + for (auto lm : lms) { + if (lm.solver_id == -1 && lm.drone_id != self_id) { + // This is a internal only remote landmark + continue; + } + if (lm.solver_id > 0 && lm.solver_id != self_id) { + continue; + } + for (auto i = 0; i < lm.track.size(); i++) { + if (state.getFramebyId(lm.track[i].frame_id)->drone_id != self_id) { + return true; + } } - return false; + } + return false; } void D2Estimator::setupLandmarkFactors() { - used_landmarks.clear(); - auto lms = state.availableLandmarkMeasurements(params->max_solve_cnt, params->max_solve_measurements); - current_landmark_num = lms.size(); - current_measurement_num = 0; - auto loss_function = new ceres::HuberLoss(1.0); - keyframe_measurements.clear(); - if (params->verbose) { - printf("[D2VINS::setupLandmarkFactors] %d landmarks\n", lms.size()); - } - //We first count keyframe_measurements - for (auto lm : lms) { - LandmarkPerFrame firstObs = lm.track[0]; - keyframe_measurements[firstObs.frame_id] ++; - for (auto i = 1; i < lm.track.size(); i++) { - auto lm_per_frame = lm.track[i]; - keyframe_measurements[lm_per_frame.frame_id] ++; - } - } - //Check the measurements number of each keyframe - std::set ignore_frames; - for (auto it : keyframe_measurements) { - auto frame_id = it.first; - if (it.second < params->min_measurements_per_keyframe) { - auto frame = state.getFramebyId(frame_id); - if (frame->drone_id != self_id && params->estimation_mode == D2VINSConfig::DISTRIBUTED_CAMERA_CONSENUS) { - ignore_frames.insert(frame_id); - printf("\033[0;31m[D2VINS::D2Estimator@%d] frame_id %ld has only %d measurement, will be skip.\033[0m\n", - self_id, frame_id, it.second); - } else { - printf("\033[0;31m[D2VINS::D2Estimator@%d] frame_id %ld has only %d measurements\033[0m\n", + used_landmarks.clear(); + auto lms = state.availableLandmarkMeasurements( + params->max_solve_cnt, params->max_solve_measurements); + current_landmark_num = lms.size(); + current_measurement_num = 0; + auto loss_function = new ceres::HuberLoss(1.0); + keyframe_measurements.clear(); + spdlog::debug("{} landmarks", lms.size()); + // We first count keyframe_measurements + for (auto lm : lms) { + LandmarkPerFrame firstObs = lm.track[0]; + keyframe_measurements[firstObs.frame_id]++; + for (auto i = 1; i < lm.track.size(); i++) { + auto lm_per_frame = lm.track[i]; + keyframe_measurements[lm_per_frame.frame_id]++; + } + } + // Check the measurements number of each keyframe + std::set ignore_frames; + for (auto it : keyframe_measurements) { + auto frame_id = it.first; + if (it.second < params->min_measurements_per_keyframe) { + auto frame = state.getFramebyId(frame_id); + if (frame->drone_id != self_id && + params->estimation_mode == D2Common::DISTRIBUTED_CAMERA_CONSENUS) { + ignore_frames.insert(frame_id); + SPDLOG_WARN("D{} frame_id {} has only {} measurement, will be skip.", self_id, frame_id, it.second); - } - //Print a landmark report for this frame - // state.printLandmarkReport(frame_id); - } - } - - for (auto lm : lms) { - auto lm_id = lm.landmark_id; - LandmarkPerFrame firstObs = lm.track[0]; - if (ignore_frames.find(firstObs.frame_id) != ignore_frames.end()) { - continue; - } - auto base_camera_id = firstObs.camera_id; - auto mea0 = firstObs.measurement(); - state.getLandmarkbyId(lm_id).solver_flag = LandmarkSolverFlag::SOLVED; - if (firstObs.depth_mea && params->fuse_dep && - firstObs.depth < params->max_depth_to_fuse && - firstObs.depth > params->min_depth_to_fuse) { - auto f_dep = OneFrameDepth::Create(firstObs.depth); - auto info = DepthResInfo::create(f_dep, loss_function, firstObs.frame_id, lm_id); - marginalizer->addResidualInfo(info); - solver->addResidual(info); - used_landmarks.insert(lm_id); - } + } else { + SPDLOG_WARN("D{} frame_id {} has only {} measurements.", self_id, + frame_id, it.second); + } + // Print a landmark report for this frame + // state.printLandmarkReport(frame_id); + } + } + + for (auto lm : lms) { + auto lm_id = lm.landmark_id; + LandmarkPerFrame firstObs = lm.track[0]; + if (ignore_frames.find(firstObs.frame_id) != ignore_frames.end()) { + continue; + } + auto base_camera_id = firstObs.camera_id; + auto mea0 = firstObs.measurement(); + state.getLandmarkbyId(lm_id).solver_flag = LandmarkSolverFlag::SOLVED; + if (firstObs.depth_mea && params->fuse_dep && + firstObs.depth < params->max_depth_to_fuse && + firstObs.depth > params->min_depth_to_fuse) { + auto f_dep = OneFrameDepth::Create(firstObs.depth); + auto info = + DepthResInfo::create(f_dep, loss_function, firstObs.frame_id, lm_id); + marginalizer->addResidualInfo(info); + solver->addResidual(info); + used_landmarks.insert(lm_id); + } + current_measurement_num++; + for (auto i = 1; i < lm.track.size(); i++) { + auto lm_per_frame = lm.track[i]; + if (ignore_frames.find(lm_per_frame.frame_id) != ignore_frames.end()) { + continue; + } + auto mea1 = lm_per_frame.measurement(); + ResidualInfo *info = nullptr; + if (lm_per_frame.camera_id == base_camera_id) { + ceres::CostFunction *f_td = nullptr; + bool enable_depth_mea = false; + if (lm_per_frame.depth_mea && params->fuse_dep && + lm_per_frame.depth < params->max_depth_to_fuse && + lm_per_frame.depth > params->min_depth_to_fuse) { + enable_depth_mea = true; + f_td = new ProjectionTwoFrameOneCamDepthFactor( + mea0, mea1, firstObs.velocity, lm_per_frame.velocity, + firstObs.cur_td, lm_per_frame.cur_td, lm_per_frame.depth); + } else { + f_td = new ProjectionTwoFrameOneCamFactor( + mea0, mea1, firstObs.velocity, lm_per_frame.velocity, + firstObs.cur_td, lm_per_frame.cur_td); + } + if (firstObs.frame_id == lm_per_frame.frame_id) { + SPDLOG_WARN( + "landmarkid {} frame {}<->{}@{} is the same " + "camera id {}", + lm_per_frame.landmark_id, firstObs.frame_id, + lm_per_frame.frame_id, lm_id, base_camera_id); + continue; + } + info = LandmarkTwoFrameOneCamResInfo::create( + f_td, loss_function, firstObs.frame_id, lm_per_frame.frame_id, + lm_id, firstObs.camera_id, enable_depth_mea); + } else { + if (lm_per_frame.frame_id == firstObs.frame_id) { + auto f_td = new ProjectionOneFrameTwoCamFactor( + mea0, mea1, firstObs.velocity, lm_per_frame.velocity, + firstObs.cur_td, lm_per_frame.cur_td); + info = LandmarkOneFrameTwoCamResInfo::create( + f_td, loss_function, firstObs.frame_id, lm_id, firstObs.camera_id, + lm_per_frame.camera_id); + } else { + auto f_td = new ProjectionTwoFrameTwoCamFactor( + mea0, mea1, firstObs.velocity, lm_per_frame.velocity, + firstObs.cur_td, lm_per_frame.cur_td); + info = LandmarkTwoFrameTwoCamResInfo::create( + f_td, loss_function, firstObs.frame_id, lm_per_frame.frame_id, + lm_id, firstObs.camera_id, lm_per_frame.camera_id); + } + } + if (info != nullptr) { current_measurement_num++; - for (auto i = 1; i < lm.track.size(); i++) { - auto lm_per_frame = lm.track[i]; - if (ignore_frames.find(lm_per_frame.frame_id) != ignore_frames.end()) { - continue; - } - auto mea1 = lm_per_frame.measurement(); - ResidualInfo * info = nullptr; - if (lm_per_frame.camera_id == base_camera_id) { - ceres::CostFunction * f_td = nullptr; - bool enable_depth_mea = false; - if (lm_per_frame.depth_mea && params->fuse_dep && - lm_per_frame.depth < params->max_depth_to_fuse && - lm_per_frame.depth > params->min_depth_to_fuse) { - enable_depth_mea = true; - f_td = new ProjectionTwoFrameOneCamDepthFactor(mea0, mea1, firstObs.velocity, lm_per_frame.velocity, - firstObs.cur_td, lm_per_frame.cur_td, lm_per_frame.depth); - } else { - f_td = new ProjectionTwoFrameOneCamFactor(mea0, mea1, firstObs.velocity, lm_per_frame.velocity, - firstObs.cur_td, lm_per_frame.cur_td); - } - if (firstObs.frame_id == lm_per_frame.frame_id) { - printf("\033[0;31m[ [D2VINS::setupLandmarkFactors] Warning: landmarkid %ld frame %ld<->%ld@%ld is the same camera id %d.\033[0m\n", - lm_per_frame.landmark_id, firstObs.frame_id, lm_per_frame.frame_id, lm_id, base_camera_id); - continue; - } - info = LandmarkTwoFrameOneCamResInfo::create(f_td, loss_function, - firstObs.frame_id, lm_per_frame.frame_id, lm_id, firstObs.camera_id, enable_depth_mea); - } else { - if (lm_per_frame.frame_id == firstObs.frame_id) { - auto f_td = new ProjectionOneFrameTwoCamFactor(mea0, mea1, firstObs.velocity, - lm_per_frame.velocity, firstObs.cur_td, lm_per_frame.cur_td); - info = LandmarkOneFrameTwoCamResInfo::create(f_td, nullptr, - firstObs.frame_id, lm_id, firstObs.camera_id, lm_per_frame.camera_id); - } else { - auto f_td = new ProjectionTwoFrameTwoCamFactor(mea0, mea1, firstObs.velocity, - lm_per_frame.velocity, firstObs.cur_td, lm_per_frame.cur_td); - info = LandmarkTwoFrameTwoCamResInfo::create(f_td, loss_function, firstObs.frame_id, lm_per_frame.frame_id, lm_id, - firstObs.camera_id, lm_per_frame.camera_id); - } - } - if (info != nullptr) { - current_measurement_num++; - solver->addResidual(info); - marginalizer->addResidualInfo(info); - used_landmarks.insert(lm_id); - } - if (params->estimation_mode != D2VINSConfig::DISTRIBUTED_CAMERA_CONSENUS) { - solver->getProblem().SetParameterLowerBound(state.getLandmarkState(lm_id), 0, params->min_inv_dep); - } - } - } - if (params->verbose) { - printf("[D2VINS::setupLandmarkFactors@%d] %d landmarks %d measurements \n", self_id, lms.size(), current_measurement_num); + solver->addResidual(info); + marginalizer->addResidualInfo(info); + used_landmarks.insert(lm_id); + } } + } + spdlog::debug("D{} {} landmarks {} measurements {}", self_id, lms.size(), + current_measurement_num); } -const std::map & D2Estimator::getLandmarkDB() const { - return state.getLandmarkDB(); +const std::map &D2Estimator::getLandmarkDB() + const { + return state.getLandmarkDB(); } -const std::vector & D2Estimator::getSelfSldWin() const { - return state.getSldWin(self_id); +const std::vector &D2Estimator::getSelfSldWin() const { + return state.getSldWin(self_id); } void D2Estimator::setupPriorFactor() { - auto prior_factor = state.getPrior(); - if (prior_factor != nullptr) { - auto pfactor = new PriorFactor(*prior_factor); - auto info = PriorResInfo::create(pfactor); - solver->addResidual(info); - marginalizer->addResidualInfo(info); - } + auto prior_factor = state.getPrior(); + if (prior_factor != nullptr) { + auto pfactor = new PriorFactor(*prior_factor); + auto info = PriorResInfo::create(pfactor); + solver->addResidual(info); + marginalizer->addResidualInfo(info); + } } std::vector D2Estimator::getMarginedLandmarks() const { - return margined_landmarks; + return margined_landmarks; } Swarm::Odometry D2Estimator::getImuPropagation() { - std::lock_guard lock(imu_prop_lock); - return last_prop_odom.at(self_id); + std::lock_guard lock(imu_prop_lock); + return last_prop_odom.at(self_id); } Swarm::Odometry D2Estimator::getOdometry() const { - return getOdometry(self_id); + return getOdometry(self_id); } Swarm::Odometry D2Estimator::getOdometry(int drone_id) const { - // Attention! We output IMU stamp! - auto odom = state.lastFrame(drone_id).odom; - odom.stamp = odom.stamp + state.td; - return odom; + // Attention! We output IMU stamp! + auto odom = state.lastFrame(drone_id).odom; + odom.stamp = odom.stamp + state.td; + return odom; } - -D2EstimatorState & D2Estimator::getState() { - return state; -} +D2EstimatorState &D2Estimator::getState() { return state; } bool D2Estimator::isLocalFrame(FrameIdType frame_id) const { - return state.getFramebyId(frame_id)->drone_id == self_id; + return state.getFramebyId(frame_id)->drone_id == self_id; } -D2Visualization & D2Estimator::getVisualizer() { - return visual; -} +D2Visualization &D2Estimator::getVisualizer() { return visual; } -void D2Estimator::setPGOPoses(const std::map & poses) { - last_pgo_poses = poses; +void D2Estimator::setPGOPoses(const std::map &poses) { + last_pgo_poses = poses; } -std::set D2Estimator::getNearbyDronesbyPGOData(const std::map> & vins_poses) { - std::set nearby_drones; - if (last_pgo_poses.find(self_id) == last_pgo_poses.end()) { - return nearby_drones; - } - auto self_pose = last_pgo_poses.at(self_id); - for (auto p : last_pgo_poses) { - if (p.first == self_id) { - continue; - } - auto & pose = p.second; - auto dist = (pose.pos() - self_pose.pos()).norm(); - auto dist_yaw = std::abs(pose.yaw() -self_pose.yaw()); - if (dist < params->nearby_drone_dist && dist_yaw < params->nearby_drone_yaw_dist/57.3) { - nearby_drones.insert(p.first); - } - //Check using D2VINS pose - state.lock_state(); - if (state.size(p.first) > 0) { - auto d2vins_pose = state.lastFrame(p.first).odom.pose(); - dist = (d2vins_pose.pos() - self_pose.pos()).norm(); - dist_yaw = std::abs(d2vins_pose.yaw() -self_pose.yaw()); - } - state.unlock_state(); - if (dist < params->nearby_drone_dist && dist_yaw < params->nearby_drone_yaw_dist/57.3) { - nearby_drones.insert(p.first); - } - if (params->verbose) { - printf("[D2Estimator::getNearbyDronesbyPGOData] drone %d dist %.1f yaw %.1fdeg\n", p.first, dist, dist_yaw*57.3); - } - } - for (auto it: vins_poses) { - auto & pose = it.second.second; - auto dist = (pose.pos() - self_pose.pos()).norm(); - auto dist_yaw = std::abs(pose.yaw() -self_pose.yaw()); - if (dist < params->nearby_drone_dist && dist_yaw < params->nearby_drone_yaw_dist/57.3) { - nearby_drones.insert(it.first); - } - if (params->verbose) { - printf("[D2Estimator::getNearbyDronesbyPGOData] VINS Pose drone %d dist %.1f yaw %.1fdeg\n", it.first, dist, dist_yaw*57.3); - } - } +std::set D2Estimator::getNearbyDronesbyPGOData( + const std::map> &vins_poses) { + std::set nearby_drones; + if (last_pgo_poses.find(self_id) == last_pgo_poses.end()) { return nearby_drones; -} -std::pair> D2Estimator::getMotionPredict(double stamp) const { - if(!initFirstPoseFlag) { - return std::make_pair(Swarm::Odometry(), std::make_pair(IMUBuffer(), -1)); - } - const auto & last_frame = state.lastFrame(); - auto ret = imu_bufs.at(self_id).periodIMU(last_frame.imu_buf_index, stamp + state.td); - auto _imu = ret.first; - auto index = ret.second; - if (fabs(_imu.size()/(stamp - last_frame.stamp) - params->IMU_FREQ) > 15) { - printf("\033[0;31m[D2VINS::D2Estimator] Local IMU error freq: %.3f start_t %.3f/%.3f end_t %.3f/%.3f\033[0m\n", - _imu.size()/(stamp - last_frame.stamp), - last_frame.stamp + state.td, _imu[0].t, stamp + state.td, _imu[_imu.size()-1].t); - } - return std::make_pair(_imu.propagation(last_frame), ret); -} - -void D2Estimator::updateSldwin(int drone_id, const std::vector & sld_win) { - state.updateSldwin(drone_id, sld_win); -} - -} \ No newline at end of file + } + auto self_pose = last_pgo_poses.at(self_id); + for (auto p : last_pgo_poses) { + if (p.first == self_id) { + continue; + } + auto &pose = p.second; + auto dist = (pose.pos() - self_pose.pos()).norm(); + auto dist_yaw = std::abs(pose.yaw() - self_pose.yaw()); + if (dist < params->nearby_drone_dist && + dist_yaw < params->nearby_drone_yaw_dist / 57.3) { + nearby_drones.insert(p.first); + } + // Check using D2VINS pose + state.lock_state(); + if (state.size(p.first) > 0) { + auto d2vins_pose = state.lastFrame(p.first).odom.pose(); + dist = (d2vins_pose.pos() - self_pose.pos()).norm(); + dist_yaw = std::abs(d2vins_pose.yaw() - self_pose.yaw()); + } + state.unlock_state(); + if (dist < params->nearby_drone_dist && + dist_yaw < params->nearby_drone_yaw_dist / 57.3) { + nearby_drones.insert(p.first); + } + spdlog::debug("drone {} dist {:.1f} yaw {:.1f}deg", p.first, dist, + dist_yaw * 57.3); + } + for (auto it : vins_poses) { + auto &pose = it.second.second; + auto dist = (pose.pos() - self_pose.pos()).norm(); + auto dist_yaw = std::abs(pose.yaw() - self_pose.yaw()); + if (dist < params->nearby_drone_dist && + dist_yaw < params->nearby_drone_yaw_dist / 57.3) { + nearby_drones.insert(it.first); + } + spdlog::debug("VINS Pose drone {} dist {:.1f} yaw {:.1f}deg", it.first, + dist, dist_yaw * 57.3); + } + return nearby_drones; +} + +std::pair> +D2Estimator::getMotionPredict(double stamp) const { + if (!initFirstPoseFlag) { + return std::make_pair(Swarm::Odometry(), std::make_pair(IMUBuffer(), -1)); + } + const auto &last_frame = state.lastFrame(); + auto ret = imu_bufs.at(self_id).periodIMU(last_frame.imu_buf_index, + stamp + state.td); + auto _imu = ret.first; + auto index = ret.second; + if (fabs(_imu.size() / (stamp - last_frame.stamp) - params->IMU_FREQ) > 15) { + SPDLOG_WARN( + "Local IMU error freq: {:.3f} start_t {:.3f}/{:.3f} end_t " + "{:.3f}/{:.3f}", + _imu.size() / (stamp - last_frame.stamp), last_frame.stamp + state.td, + _imu[0].t, stamp + state.td, _imu[_imu.size() - 1].t); + } + return std::make_pair(_imu.propagation(last_frame), ret); +} + +void D2Estimator::updateSldwin(int drone_id, + const std::vector &sld_win) { + state.updateSldwin(drone_id, sld_win); +} + +bool D2Estimator::isInitialized() const { return solve_count > 0; } + +} // namespace D2VINS \ No newline at end of file diff --git a/d2vins/src/estimator/d2estimator.hpp b/d2vins/src/estimator/d2estimator.hpp index e1c85acf..694369d7 100644 --- a/d2vins/src/estimator/d2estimator.hpp +++ b/d2vins/src/estimator/d2estimator.hpp @@ -100,5 +100,6 @@ class D2Estimator { std::set getNearbyDronesbyPGOData(const std::map> & vins_poses); void setStateProperties(); virtual std::pair> getMotionPredict(double stamp) const; + bool isInitialized() const; }; } \ No newline at end of file diff --git a/d2vins/src/estimator/d2vinsstate.cpp b/d2vins/src/estimator/d2vinsstate.cpp index 1261c7a4..17b01b3d 100644 --- a/d2vins/src/estimator/d2vinsstate.cpp +++ b/d2vins/src/estimator/d2vinsstate.cpp @@ -1,657 +1,1014 @@ #include "d2vinsstate.hpp" -#include "../d2vins_params.hpp" + #include #include -#include "marginalization/marginalization.hpp" +#include + +#include + +#include "../d2vins_params.hpp" #include "../factors/prior_factor.h" +#include "marginalization/marginalization.hpp" using namespace Eigen; using D2Common::generateCameraId; namespace D2VINS { -D2EstimatorState::D2EstimatorState(int _self_id): - D2State(_self_id) -{ - sld_wins[self_id] = std::vector(); - if (params->estimation_mode != D2VINSConfig::SERVER_MODE) { - all_drones.insert(self_id); - } +D2EstimatorState::D2EstimatorState(int _self_id) : D2State(_self_id) { + sld_wins[self_id] = std::vector(); + if (params->estimation_mode != D2Common::SERVER_MODE) { + all_drones.insert(self_id); + } } std::vector D2EstimatorState::popFrame(int index) { - const Guard lock(state_lock); - //Remove from sliding window - auto frame_id = sld_wins[self_id].at(index)->frame_id; - if (params->verbose) { - printf("[D2VSIN::D2EstimatorState] remove frame %ld\n", frame_id); - } - sld_wins[self_id].erase(sld_wins[self_id].begin() + index); - return removeFrameById(frame_id); -} - -VINSFrame * D2EstimatorState::addVINSFrame(const VINSFrame & _frame) { - const Guard lock(state_lock); - auto * frame = new VINSFrame; - *frame = _frame; - frame_db[frame->frame_id] = frame; - _frame_pose_state[frame->frame_id] = new state_type[POSE_SIZE]; - _frame.odom.pose().to_vector(_frame_pose_state[frame->frame_id]); - frame->reference_frame_id = reference_frame_id; - all_drones.insert(_frame.drone_id); - return frame; -} - -std::vector D2EstimatorState::removeFrameById(FrameIdType frame_id, bool remove_base) { - const Guard lock(state_lock); - if (params->verbose) { - printf("[D2VSIN::D2EstimatorState] remove frame %ld remove base %d\n", frame_id, remove_base); - } - auto ret = lmanager.popFrame(frame_id, remove_base); - auto _frame = static_cast(frame_db.at(frame_id)); - if (_frame->pre_integrations) { - delete _frame->pre_integrations; - } - - delete _frame; - frame_db.erase(frame_id); - delete _frame_pose_state.at(frame_id); - _frame_pose_state.erase(frame_id); - if (_frame_spd_Bias_state.find(frame_id) != _frame_spd_Bias_state.end()) { - delete _frame_spd_Bias_state.at(frame_id); - _frame_spd_Bias_state.erase(frame_id); - } - return ret; + const Guard lock(state_lock); + // Remove from sliding window + auto frame_id = sld_wins[self_id].at(index)->frame_id; + if (params->verbose) { + printf("[D2VSIN::D2EstimatorState] remove frame %ld\n", frame_id); + } + sld_wins[self_id].erase(sld_wins[self_id].begin() + index); + return removeFrameById(frame_id); +} + +VINSFrame *D2EstimatorState::addVINSFrame(const VINSFrame &_frame) { + const Guard lock(state_lock); + auto *frame = new VINSFrame; + *frame = _frame; + frame_db[frame->frame_id] = frame; + _frame_pose_state[frame->frame_id] = new state_type[POSE_SIZE]; + _frame.odom.pose().to_vector(_frame_pose_state[frame->frame_id]); + frame->reference_frame_id = reference_frame_id; + all_drones.insert(_frame.drone_id); + return frame; +} + +std::vector D2EstimatorState::removeFrameById( + FrameIdType frame_id, bool remove_base) { + const Guard lock(state_lock); + if (params->verbose) { + printf("[D2VSIN::D2EstimatorState] remove frame %ld remove base %d\n", + frame_id, remove_base); + } + auto ret = lmanager.popFrame(frame_id, remove_base); + auto _frame = static_cast(frame_db.at(frame_id)); + if (_frame->pre_integrations) { + delete _frame->pre_integrations; + } + + delete _frame; + frame_db.erase(frame_id); + delete _frame_pose_state.at(frame_id); + _frame_pose_state.erase(frame_id); + if (_frame_spd_Bias_state.find(frame_id) != _frame_spd_Bias_state.end()) { + delete _frame_spd_Bias_state.at(frame_id); + _frame_spd_Bias_state.erase(frame_id); + } + return ret; } void D2EstimatorState::init(std::vector _extrinsic, double _td) { - for (int i = 0; i < _extrinsic.size(); i ++) { - auto pose = _extrinsic[i]; - auto cam_id = addCamera(pose, i, self_id); - local_camera_ids.push_back(cam_id); - } - td = _td; -} - -CamIdType D2EstimatorState::addCamera(const Swarm::Pose & pose, int camera_index, int drone_id, CamIdType camera_id) { - if (camera_id < 0) { - camera_id = generateCameraId(self_id, camera_index); - } - auto _p = new state_type[POSE_SIZE]; - pose.to_vector(_p); - _camera_extrinsic_state[camera_id] = _p; - extrinsic[camera_id] = pose; - camera_drone[camera_id] = drone_id; - return camera_id; + for (int i = 0; i < _extrinsic.size(); i++) { + auto pose = _extrinsic[i]; + auto cam_id = addCamera(pose, i, self_id); + local_camera_ids.push_back(cam_id); + } + td = _td; +} + +CamIdType D2EstimatorState::addCamera(const Swarm::Pose &pose, int camera_index, + int drone_id, CamIdType camera_id) { + if (camera_id < 0) { + camera_id = generateCameraId(self_id, camera_index); + } + auto _p = new state_type[POSE_SIZE]; + pose.to_vector(_p); + _camera_extrinsic_state[camera_id] = _p; + extrinsic[camera_id] = pose; + camera_drone[camera_id] = drone_id; + return camera_id; } std::vector D2EstimatorState::localCameraExtrinsics() const { - std::vector ret; - for (auto & camera_id : local_camera_ids) { - ret.push_back(extrinsic.at(camera_id)); - } - return ret; + std::vector ret; + for (auto &camera_id : local_camera_ids) { + ret.push_back(extrinsic.at(camera_id)); + } + return ret; } -size_t D2EstimatorState::size() const { - return size(self_id); -} +size_t D2EstimatorState::size() const { return size(self_id); } -VINSFrame & D2EstimatorState::getFrame(int index) { - return getFrame(self_id, index); +VINSFrame &D2EstimatorState::getFrame(int index) { + return getFrame(self_id, index); } -const VINSFrame & D2EstimatorState::getFrame(int index) const { - return getFrame(self_id, index); +const VINSFrame &D2EstimatorState::getFrame(int index) const { + return getFrame(self_id, index); } +VINSFrame &D2EstimatorState::firstFrame() { return firstFrame(self_id); } - -VINSFrame & D2EstimatorState::firstFrame() { - return firstFrame(self_id); -} - -const VINSFrame & D2EstimatorState::lastFrame() const { - return lastFrame(self_id); +const VINSFrame &D2EstimatorState::lastFrame() const { + return lastFrame(self_id); } -VINSFrame & D2EstimatorState::lastFrame() { - return lastFrame(self_id); -} +VINSFrame &D2EstimatorState::lastFrame() { return lastFrame(self_id); } -VINSFrame & D2EstimatorState::getFrame(int drone_id, int index) { - const Guard lock(state_lock); - return *sld_wins.at(drone_id)[index]; +VINSFrame &D2EstimatorState::getFrame(int drone_id, int index) { + const Guard lock(state_lock); + return *sld_wins.at(drone_id)[index]; } -const VINSFrame & D2EstimatorState::getFrame(int drone_id, int index) const { - const Guard lock(state_lock); - return *sld_wins.at(drone_id)[index]; +const VINSFrame &D2EstimatorState::getFrame(int drone_id, int index) const { + const Guard lock(state_lock); + return *sld_wins.at(drone_id)[index]; } - Swarm::Pose D2EstimatorState::getEstimatedPose(int drone_id, int index) const { - return getFrame(drone_id, index).odom.pose(); + return getFrame(drone_id, index).odom.pose(); } Swarm::Pose D2EstimatorState::getEstimatedPose(FrameIdType frame_id) const { - auto drone_id = getFrame(frame_id).drone_id; - return getFramebyId(frame_id)->odom.pose(); + auto drone_id = getFrame(frame_id).drone_id; + return getFramebyId(frame_id)->odom.pose(); } Swarm::Odometry D2EstimatorState::getEstimatedOdom(FrameIdType frame_id) const { - auto drone_id = getFrame(frame_id).drone_id; - return getFramebyId(frame_id)->odom; + auto drone_id = getFrame(frame_id).drone_id; + return getFramebyId(frame_id)->odom; } -VINSFrame & D2EstimatorState::firstFrame(int drone_id) { - const Guard lock(state_lock); - assert(sld_wins.at(drone_id).size() > 0 && "SLDWIN size must > 0 to call D2EstimatorState::firstFrame()"); - return *sld_wins.at(drone_id)[0]; +VINSFrame &D2EstimatorState::firstFrame(int drone_id) { + const Guard lock(state_lock); + assert(sld_wins.at(drone_id).size() > 0 && + "SLDWIN size must > 0 to call D2EstimatorState::firstFrame()"); + return *sld_wins.at(drone_id)[0]; } -const VINSFrame & D2EstimatorState::lastFrame(int drone_id) const { - const Guard lock(state_lock); - if (sld_wins.at(drone_id).size() == 0) { - printf("[D2VSIN::D2EstimatorState] lastFrame() sld_wins[%d].size() == 0\n", drone_id); - } - assert(sld_wins.at(drone_id).size() > 0 && "SLDWIN size must > 0 to call D2EstimatorState::lastFrame()"); - return *sld_wins.at(drone_id).back(); +const VINSFrame &D2EstimatorState::lastFrame(int drone_id) const { + const Guard lock(state_lock); + if (sld_wins.at(drone_id).size() == 0) { + printf("[D2VSIN::D2EstimatorState] lastFrame() sld_wins[%d].size() == 0\n", + drone_id); + } + assert(sld_wins.at(drone_id).size() > 0 && + "SLDWIN size must > 0 to call D2EstimatorState::lastFrame()"); + return *sld_wins.at(drone_id).back(); } -VINSFrame & D2EstimatorState::lastFrame(int drone_id) { - const Guard lock(state_lock); - assert(sld_wins.at(drone_id).size() > 0 && "SLDWIN size must > 0 to call D2EstimatorState::lastFrame()"); - return *sld_wins.at(drone_id).back(); +VINSFrame &D2EstimatorState::lastFrame(int drone_id) { + const Guard lock(state_lock); + assert(sld_wins.at(drone_id).size() > 0 && + "SLDWIN size must > 0 to call D2EstimatorState::lastFrame()"); + return *sld_wins.at(drone_id).back(); } -size_t D2EstimatorState::size(int drone_id) const { - const Guard lock(state_lock); - if (sld_wins.find(drone_id) == sld_wins.end()) { - return 0; - } - return sld_wins.at(drone_id).size(); +size_t D2EstimatorState::size(int drone_id) const { + const Guard lock(state_lock); + if (sld_wins.find(drone_id) == sld_wins.end()) { + return 0; + } + return sld_wins.at(drone_id).size(); } int D2EstimatorState::getPoseIndex(FrameIdType frame_id) const { - const Guard lock(state_lock); - return frame_indices.at(frame_id); -} - -double * D2EstimatorState::getTdState(int drone_id) { - return &td; + const Guard lock(state_lock); + return frame_indices.at(frame_id); } -double D2EstimatorState::getTd(int drone_id) { - return td; -} +double *D2EstimatorState::getTdState(int drone_id) { return &td; } +double D2EstimatorState::getTd(int drone_id) { return td; } -double * D2EstimatorState::getExtrinsicState(int cam_id) const { - if (_camera_extrinsic_state.find(cam_id) == _camera_extrinsic_state.end()) { - printf("[D2VINS::D2EstimatorState] Camera %d not found!\n"); - assert(false && "Camera_id not found"); - } - return _camera_extrinsic_state.at(cam_id); +double *D2EstimatorState::getExtrinsicState(int cam_id) const { + if (_camera_extrinsic_state.find(cam_id) == _camera_extrinsic_state.end()) { + printf("[D2VINS::D2EstimatorState] Camera %d not found!\n"); + assert(false && "Camera_id not found"); + } + return _camera_extrinsic_state.at(cam_id); } -double * D2EstimatorState::getSpdBiasState(FrameIdType frame_id) const { - return _frame_spd_Bias_state.at(frame_id); +double *D2EstimatorState::getSpdBiasState(FrameIdType frame_id) const { + return _frame_spd_Bias_state.at(frame_id); } -double * D2EstimatorState::getLandmarkState(LandmarkIdType landmark_id) const { - return lmanager.getLandmarkState(landmark_id); +double *D2EstimatorState::getLandmarkState(LandmarkIdType landmark_id) const { + return lmanager.getLandmarkState(landmark_id); } -FrameIdType D2EstimatorState::getLandmarkBaseFrame(LandmarkIdType landmark_id) const { - return lmanager.getLandmarkBaseFrame(landmark_id); +FrameIdType D2EstimatorState::getLandmarkBaseFrame( + LandmarkIdType landmark_id) const { + return lmanager.getLandmarkBaseFrame(landmark_id); } Swarm::Pose D2EstimatorState::getExtrinsic(CamIdType cam_id) const { - return extrinsic.at(cam_id); + return extrinsic.at(cam_id); } -PriorFactor * D2EstimatorState::getPrior() const { - return prior_factor; -} +PriorFactor *D2EstimatorState::getPrior() const { return prior_factor; } std::set D2EstimatorState::getAvailableCameraIds() const { - //Return all camera ids - std::set ids; - for (auto &it : _camera_extrinsic_state) { - ids.insert(it.first); - } - return ids; + // Return all camera ids + std::set ids; + for (auto &it : _camera_extrinsic_state) { + ids.insert(it.first); + } + return ids; } -std::vector D2EstimatorState::availableLandmarkMeasurements(int max_pts, int max_measurement) const { - std::set current_frames; - for (auto &it : sld_wins) { - for (auto &it2 : it.second) { - current_frames.insert(it2->frame_id); - } +std::vector D2EstimatorState::availableLandmarkMeasurements( + int max_pts, int max_measurement) const { + std::set current_frames; + for (auto &it : sld_wins) { + for (auto &it2 : it.second) { + current_frames.insert(it2->frame_id); } - return lmanager.availableMeasurements(max_pts, max_measurement, current_frames); + } + return lmanager.availableMeasurements(max_pts, max_measurement, + current_frames); } int D2EstimatorState::getCameraBelonging(CamIdType cam_id) const { - return camera_drone.at(cam_id); -} - -std::vector D2EstimatorState::clearUselessFrames() { - //If keyframe_only is true, then only remove keyframes. - const Guard lock(state_lock); - std::vector ret; - std::set clear_frames; //Frames in this set will be deleted. - std::set clear_key_frames; //Frames in this set will be MARGINALIZED and deleted. - - for (auto it : latest_remote_sld_wins) { - auto drone_id = it.first; - auto & latest_sld_win = it.second; - std::set sld_win_set{latest_sld_win.begin(), latest_sld_win.end()}; - auto & _sld_win = sld_wins.at(drone_id); - for (auto it : _sld_win) { - if (sld_win_set.find(it->frame_id) == sld_win_set.end()) { - clear_frames.insert(it->frame_id); - if (frame_db.at(it->frame_id)->is_keyframe) { - clear_key_frames.insert(it->frame_id); - } - } - } - } - - auto & self_sld_win = sld_wins[self_id]; - if (self_sld_win.size() >= params->min_solve_frames) { - int count_removed = 0; - int require_sld_win_size = params->max_sld_win_size; - int sld_win_size = self_sld_win.size(); - //We remove the second last non keyframe - if (sld_win_size > require_sld_win_size && !self_sld_win[sld_win_size - 3]->is_keyframe) { - clear_frames.insert(self_sld_win[sld_win_size - 3]->frame_id); - count_removed = 1; - //Here we attach the intergation base of the remove frame to the last frame - IntegrationBase * last_pre_int = self_sld_win[sld_win_size - 2]->pre_integrations; - auto remove_pre = self_sld_win[sld_win_size - 3]->pre_integrations; - remove_pre->push_back(last_pre_int); - self_sld_win[sld_win_size - 2]->pre_integrations = remove_pre; - self_sld_win[sld_win_size - 2]->prev_frame_id = self_sld_win[sld_win_size - 3]->prev_frame_id; - self_sld_win[sld_win_size - 3]->pre_integrations = nullptr; //To avoid delete - //then we delete the useless last_pre_int - delete last_pre_int; - } - if (sld_win_size - count_removed > require_sld_win_size) { - clear_key_frames.insert(self_sld_win[0]->frame_id); - clear_frames.insert(self_sld_win[0]->frame_id); + return camera_drone.at(cam_id); +} + +std::vector D2EstimatorState::clearUselessFrames( + bool marginalization) { + // If keyframe_only is true, then only remove keyframes. + const Guard lock(state_lock); + std::vector ret; + std::set clear_frames; // Frames in this set will be deleted. + std::set + clear_key_frames; // Frames in this set will be MARGINALIZED and deleted. + + for (auto it : latest_remote_sld_wins) { + auto drone_id = it.first; + auto &latest_sld_win = it.second; + std::set sld_win_set{latest_sld_win.begin(), + latest_sld_win.end()}; + auto &_sld_win = sld_wins.at(drone_id); + for (auto it : _sld_win) { + if (sld_win_set.find(it->frame_id) == sld_win_set.end()) { + clear_frames.insert(it->frame_id); + if (frame_db.at(it->frame_id)->is_keyframe) { + clear_key_frames.insert(it->frame_id); } - } - + } + } + } + + auto &self_sld_win = sld_wins[self_id]; + if (self_sld_win.size() >= params->min_solve_frames) { + int count_removed = 0; + int require_sld_win_size = params->max_sld_win_size; + int sld_win_size = self_sld_win.size(); + // We remove the second last non keyframe + if (sld_win_size > require_sld_win_size && + !self_sld_win[sld_win_size - 3]->is_keyframe) { + clear_frames.insert(self_sld_win[sld_win_size - 3]->frame_id); + count_removed = 1; + // Here we attach the intergation base of the remove frame to the last + // frame + IntegrationBase *last_pre_int = + self_sld_win[sld_win_size - 2]->pre_integrations; + auto remove_pre = self_sld_win[sld_win_size - 3]->pre_integrations; + remove_pre->push_back(last_pre_int); + self_sld_win[sld_win_size - 2]->pre_integrations = remove_pre; + self_sld_win[sld_win_size - 2]->prev_frame_id = + self_sld_win[sld_win_size - 3]->prev_frame_id; + self_sld_win[sld_win_size - 3]->pre_integrations = + nullptr; // To avoid delete + // then we delete the useless last_pre_int + delete last_pre_int; + } + if (sld_win_size - count_removed > require_sld_win_size) { + clear_key_frames.insert(self_sld_win[0]->frame_id); + clear_frames.insert(self_sld_win[0]->frame_id); + } + } + + if (marginalization) { if (params->enable_marginalization && clear_key_frames.size() > 0) { - if (marginalizer != nullptr) { - auto prior_return = marginalizer->marginalize(clear_key_frames); - if (prior_return!=nullptr) { - if (prior_factor!=nullptr) { - delete prior_factor; - } - prior_factor = prior_return; - } + if (marginalizer != nullptr) { + auto prior_return = marginalizer->marginalize(clear_key_frames); + if (prior_return != nullptr) { + if (prior_factor != nullptr) { + delete prior_factor; + } + prior_factor = prior_return; } + } } if (prior_factor != nullptr) { - //At this time, non-keyframes is also removed, so add them to remove set to avoid pointer issue. - std::vector keeps = prior_factor->getKeepParams(); - for (auto p : keeps) { - if (clear_frames.find(p.id)!=clear_frames.end()) { - if (params->verbose) - printf("[D2EstimatorState::clearFrame] Removed Frame %ld in prior is removed from prior\n", p.id); - prior_factor->removeFrame(p.id); - } + // At this time, non-keyframes is also removed, so add them to remove set + // to avoid pointer issue. + std::vector keeps = prior_factor->getKeepParams(); + for (auto p : keeps) { + if (clear_frames.find(p.id) != clear_frames.end()) { + if (params->verbose) + printf( + "[D2EstimatorState::clearFrame] Removed Frame %ld in prior is " + "removed from prior\n", + p.id); + prior_factor->removeFrame(p.id); } - } - - if (clear_frames.size() > 0 ) { - //Remove frames that are not in the new SLDWIN - for (auto & _it : sld_wins) { - auto & _sld_win = _it.second; - for (auto it = _sld_win.begin(); it != _sld_win.end();) { - if (clear_frames.find((*it)->frame_id) != clear_frames.end()) { - if (params->verbose) - printf("[D2EstimatorState::clearFrame] Remove Frame %ld is kf %d\n", (*it)->frame_id, (*it)->is_keyframe); - bool remove_base = false; - if (clear_key_frames.find((*it)->frame_id) != clear_key_frames.end() && - params->landmark_param == D2VINSConfig::LM_INV_DEP) { - //If the frame is a keyframe, then remove the base frame of it's related measurements. - //This is because the frame's related measurment's inv_dep is marginalized. - remove_base = params->remove_base_when_margin_remote == 1; - } - auto tmp = removeFrameById((*it)->frame_id, remove_base); - ret.insert(ret.end(), tmp.begin(), tmp.end()); - // delete *it; - it = _sld_win.erase(it); - } else { - ++it; - } - } + } + } + } + + if (clear_frames.size() > 0) { + // Remove frames that are not in the new SLDWIN + for (auto &_it : sld_wins) { + auto &_sld_win = _it.second; + for (auto it = _sld_win.begin(); it != _sld_win.end();) { + if (clear_frames.find((*it)->frame_id) != clear_frames.end()) { + if (params->verbose) + printf("[D2EstimatorState::clearFrame] Remove Frame %ld is kf %d\n", + (*it)->frame_id, (*it)->is_keyframe); + bool remove_base = false; + if (clear_key_frames.find((*it)->frame_id) != + clear_key_frames.end() && + params->landmark_param == D2VINSConfig::LM_INV_DEP) { + // If the frame is a keyframe, then remove the base frame of it's + // related measurements. This is because the frame's related + // measurment's inv_dep is marginalized. + remove_base = params->remove_base_when_margin_remote == 1; + } + auto tmp = removeFrameById((*it)->frame_id, remove_base); + ret.insert(ret.end(), tmp.begin(), tmp.end()); + // delete *it; + it = _sld_win.erase(it); + } else { + ++it; } - } - return ret; -} - -void D2EstimatorState::updateSldwin(int drone_id, const std::vector & sld_win) { - const Guard lock(state_lock); - if (params->verbose) { - printf("[D2VINS::D2EstimatorState] Update sld win for drone %d:", drone_id); - for (auto id : sld_win) { - printf("%ld ", id); + } + } + } + return ret; +} + +void D2EstimatorState::updateSldwin(int drone_id, + const std::vector &sld_win) { + const Guard lock(state_lock); + if (params->verbose) { + printf("[D2VINS::D2EstimatorState] Update sld win for drone %d:", drone_id); + for (auto id : sld_win) { + printf("%ld ", id); + } + printf("\n"); + } + if (sld_wins.find(drone_id) == sld_wins.end()) { + return; + } + latest_remote_sld_wins[drone_id] = sld_win; +} + +void D2EstimatorState::updateSldWinsIMU( + const std::map &remote_imu_bufs) { + if (params->estimation_mode == D2Common::DISTRIBUTED_CAMERA_CONSENUS || + params->estimation_mode == D2Common::SINGLE_DRONE_MODE) { + auto &_sld_win = sld_wins[self_id]; + for (size_t i = 0; i < _sld_win.size() - 1; i++) { + auto frame_a = _sld_win[i]; + auto frame_b = _sld_win[i + 1]; + if (frame_b->prev_frame_id != frame_a->frame_id) { + // Update IMU factor. + auto td = getTd(frame_a->drone_id); + auto ret = remote_imu_bufs.at(self_id).periodIMU(frame_a->imu_buf_index, + frame_b->stamp + td); + auto _imu_buf = ret.first; + if (frame_b->pre_integrations != nullptr) { + delete frame_b->pre_integrations; } - printf("\n"); - } - if (sld_wins.find(drone_id) == sld_wins.end()) { - return; - } - latest_remote_sld_wins[drone_id] = sld_win; -} - -void D2EstimatorState::updateSldWinsIMU(const std::map & remote_imu_bufs) { - if (params->estimation_mode == D2VINSConfig::DISTRIBUTED_CAMERA_CONSENUS || - params->estimation_mode == D2VINSConfig::SINGLE_DRONE_MODE) { - auto & _sld_win = sld_wins[self_id]; - for (size_t i = 0; i < _sld_win.size() - 1; i ++) { - auto frame_a = _sld_win[i]; - auto frame_b = _sld_win[i+1]; - if (frame_b->prev_frame_id != frame_a->frame_id) { - //Update IMU factor. - auto td = getTd(frame_a->drone_id); - auto ret = remote_imu_bufs.at(self_id).periodIMU(frame_a->imu_buf_index, frame_b->stamp + td); - auto _imu_buf = ret.first; - if (frame_b->pre_integrations != nullptr) { - delete frame_b->pre_integrations; - } - frame_b->pre_integrations = new IntegrationBase(_imu_buf, frame_a->Ba, frame_a->Bg); - frame_b->prev_frame_id = frame_a->frame_id; - frame_b->imu_buf_index = ret.second; - if (fabs(_imu_buf.size()/(frame_b->stamp - frame_a->stamp) - params->IMU_FREQ) > 10) { - printf("\033[0;31m[D2VINS::D2Estimator] Remote IMU error freq: %.3f in updateRemoteSldIMU \033[0m\n", - _imu_buf.size()/(frame_b->stamp - frame_a->stamp)); - } - printf("[D2VINS] Update IMU for %d<->%d\n", frame_a->frame_id, frame_b->frame_id); - } + frame_b->pre_integrations = + new IntegrationBase(_imu_buf, frame_a->Ba, frame_a->Bg); + frame_b->prev_frame_id = frame_a->frame_id; + frame_b->imu_buf_index = ret.second; + if (fabs(_imu_buf.size() / (frame_b->stamp - frame_a->stamp) - + params->IMU_FREQ) > 10) { + printf( + "\033[0;31m[D2VINS::D2Estimator] Remote IMU error freq: %.3f in " + "updateRemoteSldIMU \033[0m\n", + _imu_buf.size() / (frame_b->stamp - frame_a->stamp)); } - return; - } - for (auto & _it : sld_wins) { - auto drone_id = _it.first; - auto & _sld_win = _it.second; - for (size_t i = 0; i < _sld_win.size() - 1; i ++) { - auto frame_a = _sld_win[i]; - auto frame_b = _sld_win[i+1]; - if (frame_b->prev_frame_id != frame_a->frame_id) { - //Update IMU factor. - auto td = getTd(frame_a->drone_id); - auto ret = remote_imu_bufs.at(drone_id).periodIMU(frame_a->imu_buf_index, frame_b->stamp + td); - auto _imu_buf = ret.first; - frame_b->pre_integrations = new IntegrationBase(_imu_buf, frame_a->Ba, frame_a->Bg); - frame_b->prev_frame_id = frame_a->frame_id; - frame_b->imu_buf_index = ret.second; - if (frame_b->pre_integrations != nullptr) { - delete frame_b->pre_integrations; - } - if (fabs(_imu_buf.size()/(frame_b->stamp - frame_a->stamp) - params->IMU_FREQ) > 10) { - printf("\033[0;31m[D2VINS::D2Estimator] Remote IMU error freq: %.3f in updateRemoteSldIMU \033[0m\n", - _imu_buf.size()/(frame_b->stamp - frame_a->stamp)); - } - } + printf("[D2VINS] Update IMU for %d<->%d\n", frame_a->frame_id, + frame_b->frame_id); + } + } + return; + } + for (auto &_it : sld_wins) { + auto drone_id = _it.first; + auto &_sld_win = _it.second; + for (size_t i = 0; i < _sld_win.size() - 1; i++) { + auto frame_a = _sld_win[i]; + auto frame_b = _sld_win[i + 1]; + if (frame_b->prev_frame_id != frame_a->frame_id) { + // Update IMU factor. + auto td = getTd(frame_a->drone_id); + auto ret = remote_imu_bufs.at(drone_id).periodIMU( + frame_a->imu_buf_index, frame_b->stamp + td); + auto _imu_buf = ret.first; + frame_b->pre_integrations = + new IntegrationBase(_imu_buf, frame_a->Ba, frame_a->Bg); + frame_b->prev_frame_id = frame_a->frame_id; + frame_b->imu_buf_index = ret.second; + if (frame_b->pre_integrations != nullptr) { + delete frame_b->pre_integrations; } - } -} - - -VINSFrame * D2EstimatorState::addFrame(const VisualImageDescArray & images, const VINSFrame & _frame) { - const Guard lock(state_lock); - VINSFrame * frame = addVINSFrame(_frame); - if (_frame.drone_id != self_id) { - if (sld_wins.find(_frame.drone_id) == sld_wins.end()) { - printf("[D2VINS::D2EstimatorState] Add sld_win for remote drone %d\n", _frame.drone_id); - sld_wins[_frame.drone_id] = std::vector(); + if (fabs(_imu_buf.size() / (frame_b->stamp - frame_a->stamp) - + params->IMU_FREQ) > 10) { + printf( + "\033[0;31m[D2VINS::D2Estimator] Remote IMU error freq: %.3f in " + "updateRemoteSldIMU \033[0m\n", + _imu_buf.size() / (frame_b->stamp - frame_a->stamp)); } - sld_wins[_frame.drone_id].emplace_back(frame); - for (auto & img : images.images) { - if (extrinsic.find(img.camera_id) == extrinsic.end()) { - printf("[D2VINS::D2EstimatorState] Adding extrinsic of camera %d from drone@%d\n", img.camera_id, _frame.drone_id); - addCamera(img.extrinsic, img.camera_index, images.drone_id, img.camera_id); - } - } - } else { - sld_wins[self_id].emplace_back(frame); - } - if (params->estimation_mode == D2VINSConfig::DISTRIBUTED_CAMERA_CONSENUS && _frame.drone_id != self_id) { - //In this mode, the estimate state is always ego-motion and the bias is not been estimated on remote - _frame.odom.pose().to_vector(_frame_pose_state.at(frame->frame_id)); + } + } + } +} + +VINSFrame *D2EstimatorState::addFrame(const VisualImageDescArray &images, + const VINSFrame &_frame) { + const Guard lock(state_lock); + VINSFrame *frame = addVINSFrame(_frame); + if (_frame.drone_id != self_id) { + if (sld_wins.find(_frame.drone_id) == sld_wins.end()) { + SPDLOG_INFO("[D2VINS::D2EstimatorState] Add sld_win for remote drone {}", + _frame.drone_id); + sld_wins[_frame.drone_id] = std::vector(); + } + sld_wins[_frame.drone_id].emplace_back(frame); + for (auto &img : images.images) { + if (extrinsic.find(img.camera_id) == extrinsic.end()) { + SPDLOG_INFO( + "[D2VINS::D2EstimatorState] Adding extrinsic of camera {} from " + "drone@{}", + img.camera_id, _frame.drone_id); + addCamera(img.extrinsic, img.camera_index, images.drone_id, + img.camera_id); + } + } + } else { + sld_wins[self_id].emplace_back(frame); + } + if (params->estimation_mode == D2Common::DISTRIBUTED_CAMERA_CONSENUS && + _frame.drone_id != self_id) { + // In this mode, the estimate state is always ego-motion and the bias is not + // been estimated on remote + _frame.odom.pose().to_vector(_frame_pose_state.at(frame->frame_id)); + } else { + _frame_spd_Bias_state[frame->frame_id] = new state_type[FRAME_SPDBIAS_SIZE]; + frame->toVector(_frame_pose_state.at(frame->frame_id), + _frame_spd_Bias_state.at(frame->frame_id)); + } + + lmanager.addKeyframe(images, td); + spdlog::debug( + "[D2VINS::D2EstimatorState{}] add frame {}@{} ref {} iskeyframe {} with " + "{} images, current {} frame\n", + self_id, images.frame_id, _frame.drone_id, frame->reference_frame_id, + frame->is_keyframe, images.images.size(), sld_wins[self_id].size()); + // If first frame we need to add a prior here + if (size(images.drone_id) == 1 && + (images.drone_id == self_id || + params->estimation_mode == D2Common::SOLVE_ALL_MODE || + params->estimation_mode == D2Common::SERVER_MODE)) { + // Add a prior for first frame here + createPriorFactor4FirstFrame(frame); + } + return frame; +} + +void D2EstimatorState::createPriorFactor4FirstFrame(VINSFrame *frame) { + // Prior is in form of A \delta x = b + // A is a 6x6 matrix, A = diag([a_p, a_p, a_p, 0, 0, a_yaw]) + // b is zero vector + bool add_vel_ba_prior = params->add_vel_ba_prior; + int local_cam_num = params->camera_num; + SPDLOG_WARN("Add prior for first frame, extrinsic {}, {} and speed: {}", + params->estimate_extrinsic, local_cam_num, add_vel_ba_prior); + int Adim = POSE_EFF_SIZE + + (params->estimate_extrinsic ? POSE_EFF_SIZE * local_cam_num : 0) + + (add_vel_ba_prior ? FRAME_SPDBIAS_SIZE : 0); + Eigen::MatrixXd A = Eigen::MatrixXd::Zero(Adim, Adim); + A.block<3, 3>(0, 0) = + Eigen::Matrix3d::Identity() * params->initial_pos_sqrt_info; + A(5, 5) = params->initial_yaw_sqrt_info; + if (add_vel_ba_prior) { + A.block<3, 3>(POSE_EFF_SIZE, POSE_EFF_SIZE) = + Eigen::Matrix3d::Identity() * params->initial_vel_sqrt_info; + A.block<3, 3>(POSE_EFF_SIZE + 3, POSE_EFF_SIZE + 3) = + Eigen::Matrix3d::Identity() * params->initial_ba_sqrt_info; + A.block<3, 3>(POSE_EFF_SIZE + 6, POSE_EFF_SIZE + 6) = + Eigen::Matrix3d::Identity() * params->initial_bg_sqrt_info; + } + if (self_id == params->main_id) { + A = A * 100; + } + VectorXd b = VectorXd::Zero(Adim); + auto param_info = createFramePose(this, frame->frame_id); + param_info.index = 0; + int extrinsic_start_idx = POSE_EFF_SIZE; + std::vector need_fix_params{param_info}; + if (add_vel_ba_prior) { + auto param_info_spd_bias = createSpeedBias(this, frame->frame_id); + param_info_spd_bias.index = POSE_EFF_SIZE; + need_fix_params.emplace_back(param_info_spd_bias); + extrinsic_start_idx += FRAME_SPDBIAS_SIZE; + } + if (params->estimate_extrinsic) { + for (int i = 0; i < local_cam_num; i++) { + A.block<3, 3>(i * POSE_EFF_SIZE + extrinsic_start_idx, + i * POSE_EFF_SIZE + extrinsic_start_idx) = + Eigen::Matrix3d::Identity() * params->initial_cam_pos_sqrt_info; + A.block<3, 3>(i * POSE_EFF_SIZE + 3 + extrinsic_start_idx, + i * POSE_EFF_SIZE + 3 + extrinsic_start_idx) = + Eigen::Matrix3d::Identity() * params->initial_cam_ang_sqrt_info; + auto param_info = createExtrinsic(this, local_camera_ids[i]); + param_info.index = need_fix_params.back().index + extrinsic_start_idx; + need_fix_params.emplace_back(param_info); + } + } + prior_factor = new PriorFactor(need_fix_params, A, b); +} + +void D2EstimatorState::syncFromState( + const std::set &used_landmarks) { + const Guard lock(state_lock); + // copy state buffer to structs. + // First sync the poses + + for (auto it : _frame_pose_state) { + auto frame_id = it.first; + if (frame_db.find(frame_id) == frame_db.end()) { + SPDLOG_ERROR("[D2VINS::D2EstimatorState] Cannot find frame {}", frame_id); + } + auto frame = static_cast(frame_db.at(frame_id)); + if (params->estimation_mode == D2Common::DISTRIBUTED_CAMERA_CONSENUS && + frame->drone_id != self_id) { + frame->odom.pose() = Swarm::Pose(it.second); } else { - _frame_spd_Bias_state[frame->frame_id] = new state_type[FRAME_SPDBIAS_SIZE]; - frame->toVector(_frame_pose_state.at(frame->frame_id), _frame_spd_Bias_state.at(frame->frame_id)); - } - - lmanager.addKeyframe(images, td); - if (params->verbose) { - printf("[D2VINS::D2EstimatorState%d] add frame %ld@%d ref %d iskeyframe %d with %d images, current %ld frame\n", - self_id, images.frame_id, _frame.drone_id, frame->reference_frame_id, frame->is_keyframe, - images.images.size(), sld_wins[self_id].size()); - } - //If first frame we need to add a prior here - if (size(images.drone_id) == 1 && - (images.drone_id == self_id|| params->estimation_mode == D2VINSConfig::SOLVE_ALL_MODE || - params->estimation_mode == D2VINSConfig::SERVER_MODE)) { - //Add a prior for first frame here - createPriorFactor4FirstFrame(frame); - } - return frame; -} - -void D2EstimatorState::createPriorFactor4FirstFrame(VINSFrame * frame) { - //Prior is in form of A \delta x = b - //A is a 6x6 matrix, A = diag([a_p, a_p, a_p, 0, 0, a_yaw]) - //b is zero vector - int local_cam_num = params->camera_num; - printf("\033[0;32m[D2VINS::D2Estimator] Add prior for first frame and extrinsic %d: %d\033[0m\n", params->estimate_extrinsic, local_cam_num); - int Adim = POSE_EFF_SIZE + (params->estimate_extrinsic?POSE_EFF_SIZE*local_cam_num: 0); - printf("Admin %d\n", Adim); - Eigen::MatrixXd A = Eigen::MatrixXd::Zero(Adim, Adim); - A.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity() * params->initial_pos_sqrt_info; - A(5, 5) = params->initial_yaw_sqrt_info; - if (self_id == params->main_id) { - A = A * 100; - } - VectorXd b = VectorXd::Zero(Adim); - auto param_info = createFramePose(this, frame->frame_id); - param_info.index = 0; - std::vector need_fix_params{param_info}; - if (params->estimate_extrinsic) { - for (int i = 0; i < local_cam_num; i ++) { - A.block<3, 3>((i + 1)*POSE_EFF_SIZE, (i + 1)*POSE_EFF_SIZE) = Eigen::Matrix3d::Identity() * params->initial_cam_pos_sqrt_info; - A.block<3, 3>((i + 1)*POSE_EFF_SIZE + 3, (i + 1)*POSE_EFF_SIZE + 3) = Eigen::Matrix3d::Identity() * params->initial_cam_ang_sqrt_info; - auto param_info = createExtrinsic(this, local_camera_ids[i]); - param_info.index = need_fix_params.back().index + POSE_EFF_SIZE; - need_fix_params.emplace_back(param_info); - } - } - prior_factor = new PriorFactor(need_fix_params, A, b); -} - -void D2EstimatorState::syncFromState(const std::set & used_landmarks) { - const Guard lock(state_lock); - //copy state buffer to structs. - //First sync the poses - - for (auto it : _frame_pose_state) { - auto frame_id = it.first; - if (frame_db.find(frame_id) == frame_db.end()) { - printf("[D2VINS::D2EstimatorState] Cannot find frame %ld\033[0m\n", frame_id); - } - auto frame = static_cast(frame_db.at(frame_id)); - if (params->estimation_mode == D2VINSConfig::DISTRIBUTED_CAMERA_CONSENUS && frame->drone_id != self_id) { - frame->odom.pose() = Swarm::Pose(it.second); - }else { - frame->fromVector(it.second, _frame_spd_Bias_state.at(frame_id)); - } - } - for (auto it : _camera_extrinsic_state) { - auto cam_id = it.first; - extrinsic.at(cam_id).from_vector(_camera_extrinsic_state.at(cam_id)); - } - lmanager.syncState(this); - if (size() < params->max_sld_win_size ) { - //We only repropagte when sld win is smaller than max, means not full initialized. - printf("[D2VINS] not fully initialized, will repropagte IMU\n"); - repropagateIMU(); - } - outlierRejection(used_landmarks); + frame->fromVector(it.second, _frame_spd_Bias_state.at(frame_id)); + } + } + for (auto it : _camera_extrinsic_state) { + auto cam_id = it.first; + extrinsic.at(cam_id).from_vector(_camera_extrinsic_state.at(cam_id)); + } + lmanager.syncState(this); + if (size() < params->max_sld_win_size) { + // We only repropagte when sld win is smaller than max, means not full + // initialized. + SPDLOG_INFO("[D2VINS] not fully initialized, will repropagte IMU"); + repropagateIMU(); + } + outlierRejection(used_landmarks); } void D2EstimatorState::repropagateIMU() { - if (sld_wins[self_id].size() > 1) { - for (size_t i = 0; i < sld_wins[self_id].size() - 1; i ++) { - auto frame_a = sld_wins[self_id][i]; - auto frame_b = sld_wins[self_id][i+1]; - frame_b->pre_integrations->repropagate(frame_a->Ba, frame_a->Bg); - } - } - if (params->estimation_mode == D2VINSConfig::SOLVE_ALL_MODE) { - for (auto it : sld_wins) { - if (it.first == self_id) { - continue; - } - for (size_t i = 0; i < it.second.size() - 1; i ++) { - auto frame_a = it.second[i]; - auto frame_b = it.second[i+1]; - frame_b->pre_integrations->repropagate(frame_a->Ba, frame_a->Bg); - } - } - } -} - -void D2EstimatorState::moveAllPoses(int new_ref_frame_id, const Swarm::Pose & delta_pose) { - const Guard lock(state_lock); - reference_frame_id = new_ref_frame_id; - for (auto it: frame_db) { - auto frame_id = it.first; - auto frame = static_cast(it.second); - frame->moveByPose(new_ref_frame_id, delta_pose); - if (params->estimation_mode == D2VINSConfig::DISTRIBUTED_CAMERA_CONSENUS && frame->drone_id != self_id) { - frame->odom.pose().to_vector(_frame_pose_state.at(frame_id)); - } else { - frame->toVector(_frame_pose_state.at(frame_id), _frame_spd_Bias_state.at(frame_id)); - } - } - lmanager.moveByPose(delta_pose); - if (prior_factor != nullptr) { - prior_factor->moveByPose(delta_pose); + if (sld_wins[self_id].size() > 1) { + for (size_t i = 0; i < sld_wins[self_id].size() - 1; i++) { + auto frame_a = sld_wins[self_id][i]; + auto frame_b = sld_wins[self_id][i + 1]; + frame_b->pre_integrations->repropagate(frame_a->Ba, frame_a->Bg); + } + } + if (params->estimation_mode == D2Common::SOLVE_ALL_MODE) { + for (auto it : sld_wins) { + if (it.first == self_id) { + continue; + } + for (size_t i = 0; i < it.second.size() - 1; i++) { + auto frame_a = it.second[i]; + auto frame_b = it.second[i + 1]; + frame_b->pre_integrations->repropagate(frame_a->Ba, frame_a->Bg); + } + } + } +} + +void D2EstimatorState::moveAllPoses(int new_ref_frame_id, + const Swarm::Pose &delta_pose) { + const Guard lock(state_lock); + reference_frame_id = new_ref_frame_id; + for (auto it : frame_db) { + auto frame_id = it.first; + auto frame = static_cast(it.second); + frame->moveByPose(new_ref_frame_id, delta_pose); + if (params->estimation_mode == D2Common::DISTRIBUTED_CAMERA_CONSENUS && + frame->drone_id != self_id) { + frame->odom.pose().to_vector(_frame_pose_state.at(frame_id)); + } else { + frame->toVector(_frame_pose_state.at(frame_id), + _frame_spd_Bias_state.at(frame_id)); } + } + lmanager.moveByPose(delta_pose); + if (prior_factor != nullptr) { + prior_factor->moveByPose(delta_pose); + } } -void D2EstimatorState::outlierRejection(const std::set & used_landmarks) { - //Perform outlier rejection of landmarks - lmanager.outlierRejection(this, used_landmarks); +void D2EstimatorState::outlierRejection( + const std::set &used_landmarks) { + // Perform outlier rejection of landmarks + lmanager.outlierRejection(this, used_landmarks); } -void D2EstimatorState::preSolve(const std::map & remote_imu_bufs) { - // updateSldWinsIMU(remote_imu_bufs); Useless when IMU bufs are correctly set - lmanager.initialLandmarks(this); +void D2EstimatorState::preSolve( + const std::map &remote_imu_bufs) { + // updateSldWinsIMU(remote_imu_bufs); Useless when IMU bufs are correctly set + lmanager.initialLandmarks(this); } std::vector D2EstimatorState::getInitializedLandmarks() const { - return lmanager.getInitializedLandmarks(params->landmark_estimate_tracks); + return lmanager.getInitializedLandmarks(params->landmark_estimate_tracks); } -LandmarkPerId & D2EstimatorState::getLandmarkbyId(LandmarkIdType id) { - return lmanager.at(id); +LandmarkPerId &D2EstimatorState::getLandmarkbyId(LandmarkIdType id) { + return lmanager.at(id); } bool D2EstimatorState::hasLandmark(LandmarkIdType id) const { - return lmanager.hasLandmark(id); + return lmanager.hasLandmark(id); } bool D2EstimatorState::hasCamera(CamIdType frame_id) const { - return extrinsic.find(frame_id) != extrinsic.end(); + return extrinsic.find(frame_id) != extrinsic.end(); } -void D2EstimatorState::printSldWin(const std::map & keyframe_measurments) const { - const Guard lock(state_lock); - for (auto it : sld_wins) { - printf("=========SLDWIN@drone%d=========\n", it.first); - for (int i = 0; i < it.second.size(); i ++) { - int num_mea = 0; - if (keyframe_measurments.find(it.second[i]->frame_id) != keyframe_measurments.end()) { - num_mea = keyframe_measurments.at(it.second[i]->frame_id); - } - printf("index %d frame_id %ld measurements %d frame: %s\n", i, it.second[i]->frame_id, num_mea, it.second[i]->toStr().c_str()); - } - printf("========================\n"); +int D2EstimatorState::numKeyframes() const { + int ret = 0; + for (auto it : sld_wins) { + for (auto frame : it.second) { + if (frame->is_keyframe) { + ret++; + } } + } + return ret; } -const std::vector & D2EstimatorState::getSldWin(int drone_id) const { - return sld_wins.at(self_id); -} - -void D2EstimatorState::solveGyroscopeBias() { - Matrix3d A; - Vector3d b; - Vector3d delta_bg; - A.setZero(); - b.setZero(); - auto & sld_win = sld_wins[self_id]; - for (int i = 0; i < sld_win.size() - 1; i ++ ) { - auto frame_i = sld_win[i]; - auto frame_j = sld_win[i + 1]; - MatrixXd tmp_A(3, 3); - tmp_A.setZero(); - VectorXd tmp_b(3); - tmp_b.setZero(); - Eigen::Quaterniond q_ij(frame_i->R().transpose() * frame_j->R()); - tmp_A = frame_j->pre_integrations->jacobian.template block<3, 3>(O_R, O_BG); - tmp_b = 2 * (frame_j->pre_integrations->delta_q.inverse() * q_ij).vec(); - A += tmp_A.transpose() * tmp_A; - b += tmp_A.transpose() * tmp_b; - } - delta_bg = A.ldlt().solve(b); - printf("[D2EstimatorState] gyroscope bias initial calibration: "); - std::cout << delta_bg.transpose() << std::endl; - - for (int i = 0; i < sld_win.size() - 1; i++) { - auto frame_i = sld_win[i]; - auto frame_id = frame_i->frame_id; - frame_i->Bg += delta_bg; - frame_i->toVector(_frame_pose_state[frame_id], _frame_spd_Bias_state[frame_id]); +void D2EstimatorState::printSldWin( + const std::map &keyframe_measurments) const { + const Guard lock(state_lock); + for (auto it : sld_wins) { + printf("=========SLDWIN@drone%d=========\n", it.first); + for (int i = 0; i < it.second.size(); i++) { + int num_mea = 0; + if (keyframe_measurments.find(it.second[i]->frame_id) != + keyframe_measurments.end()) { + num_mea = keyframe_measurments.at(it.second[i]->frame_id); + } + printf("index %d frame_id %ld is_kf %d measurements %d frame: %s\n", i, + it.second[i]->frame_id, it.second[i]->is_keyframe, num_mea, + it.second[i]->toStr().c_str()); } + printf("========================\n"); + } +} - for (int i = 0; i < sld_win.size() - 1; i++) { - auto frame_i = sld_win[i]; - frame_i->pre_integrations->repropagate(frame_i->Ba, frame_i->Bg); - } +const std::vector &D2EstimatorState::getSldWin( + int drone_id) const { + return sld_wins.at(self_id); } void D2EstimatorState::updateEgoMotion() { - const Guard lock(state_lock); - auto & sld_win = sld_wins[self_id]; - for (int i = 0; i < sld_win.size() - 1; i++) { - auto frame_ptr = sld_win[i]; - auto frame_id = sld_win[i]->frame_id; - if (ego_motions.find(frame_id) == ego_motions.end()) { - //We need create ego motion for this frame use the data in sld_win - } + const Guard lock(state_lock); + auto &sld_win = sld_wins[self_id]; + for (int i = 0; i < sld_win.size() - 1; i++) { + auto frame_ptr = sld_win[i]; + auto frame_id = sld_win[i]->frame_id; + if (ego_motions.find(frame_id) == ego_motions.end()) { + // We need create ego motion for this frame use the data in sld_win } + } } void D2EstimatorState::printLandmarkReport(FrameIdType frame_id) const { - auto related_landmarks = lmanager.getRelatedLandmarks(frame_id); - printf("Related landmarks of frame %ld:\n"); - for (auto lm_id : related_landmarks) { - auto lm = lmanager.at(lm_id); - printf("landmark %ld: flag %d tracks %d solve_by_local %d\n", lm.landmark_id, lm.flag, - lm.track.size(), lm.shouldBeSolve(params->self_id)); - } - printf("===============================\n"); -} - - -} \ No newline at end of file + auto related_landmarks = lmanager.getRelatedLandmarks(frame_id); + printf("Related landmarks of frame %ld:\n"); + for (auto lm_id : related_landmarks) { + auto lm = lmanager.at(lm_id); + printf("landmark %ld: flag %d tracks %d solve_by_local %d\n", + lm.landmark_id, lm.flag, lm.track.size(), + lm.shouldBeSolve(params->self_id)); + } + printf("===============================\n"); +} + +void D2EstimatorState::setPose(FrameIdType frame_id, const Swarm::Pose &pose) { + const Guard lock(state_lock); + auto frame = static_cast(frame_db.at(frame_id)); + frame->odom.pose() = pose; + frame->odom.pose().to_vector(_frame_pose_state.at(frame_id)); +} + +void D2EstimatorState::setVelocity(FrameIdType frame_id, + const Vector3d &velocity) { + const Guard lock(state_lock); + auto frame = static_cast(frame_db.at(frame_id)); + frame->odom.vel() = velocity; + frame->toVector(_frame_pose_state.at(frame_id), + _frame_spd_Bias_state.at(frame_id)); +} + +void D2EstimatorState::setBias(FrameIdType frame_id, const Vector3d &Ba, + const Vector3d &Bg) { + const Guard lock(state_lock); + auto frame = static_cast(frame_db.at(frame_id)); + frame->Ba = Ba; + frame->Bg = Bg; + frame->toVector(_frame_pose_state.at(frame_id), + _frame_spd_Bias_state.at(frame_id)); + frame->pre_integrations->repropagate(Ba, Bg); +} + +bool D2EstimatorState::monoInitialization() { + // SFM + std::map keyframe_measurments; + printSldWin(keyframe_measurments); + + if (sld_wins.at(self_id).size() < 5) { + SPDLOG_WARN( + "monoInitialization: Not enough frames for mono initialization"); + return false; + } + auto sld_win = sld_wins.at(self_id); + const int camera_idx = + generateCameraId(self_id, 0); // Default use camera 0 for initialization. + auto sfm_poses = lmanager.SFMInitialization(sld_win, camera_idx); + if (sfm_poses.size() == 0) { + SPDLOG_WARN("monoInitialization: SFM initialization failed"); + return false; + } + + // Here we rotate the attitude to IMU but not translation + Swarm::Pose Tbc = extrinsic.at(camera_idx); + for (auto &[frame_id, pose] : sfm_poses) { + sfm_poses[frame_id].att() = pose.att() * Tbc.att().inverse(); + } + + // Then use these poses to perform gyro bias calibration + if (!solveGyroscopeBias(sld_win, sfm_poses, Tbc)) { + SPDLOG_WARN("monoInitialization: Gyroscope bias calibration failed"); + return false; + } + // Then use these poses to perform linear alignment + if (!LinearAlignment(sld_win, sfm_poses, Tbc)) { + SPDLOG_WARN("monoInitialization: Linear alignment failed"); + return false; + } + + SPDLOG_INFO("monoInitialization: Finished mono initialization"); + return true; +} + +bool D2EstimatorState::solveGyroscopeBias( + std::vector sld_win, + const std::map &sfm_poses, + Swarm::Pose extrinsic) { + // Migrating from VINS-Mono + Matrix3d A; + Vector3d b; + Vector3d delta_bg; + A.setZero(); + b.setZero(); + for (int i = 0; i < sld_win.size() - 1; i++) { + auto frame_i = sld_win[i]; + auto frame_j = sld_win[i + 1]; + MatrixXd tmp_A(3, 3); + tmp_A.setZero(); + VectorXd tmp_b(3); + tmp_b.setZero(); + Eigen::Quaterniond q0 = sfm_poses.at(frame_i->frame_id).att(); + Eigen::Quaterniond q1 = sfm_poses.at(frame_j->frame_id).att(); + + Eigen::Quaterniond q_ij(q0.inverse() * q1); + tmp_A = frame_j->pre_integrations->jacobian.template block<3, 3>(O_R, O_BG); + tmp_b = 2 * (frame_j->pre_integrations->delta_q.inverse() * q_ij).vec(); + A += tmp_A.transpose() * tmp_A; + b += tmp_A.transpose() * tmp_b; + } + delta_bg = A.ldlt().solve(b); + printf("[D2EstimatorState] gyroscope bias initial calibration: "); + std::cout << delta_bg.transpose() << std::endl; + + for (int i = 1; i < sld_win.size(); i++) { + auto frame_i = sld_win[i]; + auto frame_id = frame_i->frame_id; + setBias(frame_id, frame_i->Ba, frame_i->Bg + delta_bg); + } + return true; +} + +bool D2EstimatorState::LinearAlignment( + std::vector sld_win, + const std::map &sfm_poses, + Swarm::Pose extrinsic) { + // Migrating from VINS-Mono + int all_frame_count = sld_win.size(); + int n_state = all_frame_count * 3 + 3 + 1; + Eigen::Vector3d g; + Eigen::VectorXd x; + + Eigen::MatrixXd A{n_state, n_state}; + A.setZero(); + Eigen::VectorXd b{n_state}; + b.setZero(); + + int i = 0; + for (int i = 0; i < sld_win.size() - 1; i++) { + auto frame_i = sld_win[i]; + auto frame_j = sld_win[i + 1]; + Swarm::Pose pose_i = sfm_poses.at(frame_i->frame_id); + Swarm::Pose pose_j = sfm_poses.at(frame_j->frame_id); + Eigen::Matrix3d R_i = pose_i.R(); + Eigen::Matrix3d R_j = pose_j.R(); + Eigen::Vector3d T_i = pose_i.pos(); + Eigen::Vector3d T_j = pose_j.pos(); + + MatrixXd tmp_A(6, 10); + tmp_A.setZero(); + VectorXd tmp_b(6); + tmp_b.setZero(); + + double dt = frame_j->pre_integrations->sum_dt; + + tmp_A.block<3, 3>(0, 0) = -dt * Matrix3d::Identity(); + tmp_A.block<3, 3>(0, 6) = + R_i.transpose() * dt * dt / 2 * Matrix3d::Identity(); + tmp_A.block<3, 1>(0, 9) = R_i.transpose() * (T_j - T_i) / 100.0; + tmp_b.block<3, 1>(0, 0) = frame_j->pre_integrations->delta_p + + R_i.transpose() * R_j * extrinsic.pos() - + extrinsic.pos(); + tmp_A.block<3, 3>(3, 0) = -Matrix3d::Identity(); + tmp_A.block<3, 3>(3, 3) = R_i.transpose() * R_j; + tmp_A.block<3, 3>(3, 6) = R_i.transpose() * dt * Matrix3d::Identity(); + tmp_b.block<3, 1>(3, 0) = frame_j->pre_integrations->delta_v; + + Matrix cov_inv = Matrix::Zero(); + cov_inv.setIdentity(); + + MatrixXd r_A = tmp_A.transpose() * cov_inv * tmp_A; + VectorXd r_b = tmp_A.transpose() * cov_inv * tmp_b; + + A.block<6, 6>(i * 3, i * 3) += r_A.topLeftCorner<6, 6>(); + b.segment<6>(i * 3) += r_b.head<6>(); + + A.bottomRightCorner<4, 4>() += r_A.bottomRightCorner<4, 4>(); + b.tail<4>() += r_b.tail<4>(); + + A.block<6, 4>(i * 3, n_state - 4) += r_A.topRightCorner<6, 4>(); + A.block<4, 6>(n_state - 4, i * 3) += r_A.bottomLeftCorner<4, 6>(); + } + A = A * 1000.0; + b = b * 1000.0; + x = A.ldlt().solve(b); + double s = x(n_state - 1) / 100.0; + g = x.segment<3>(n_state - 4); + spdlog::debug( + "LinearAlignment: Scale: {:.3f} g_norm: {:.3f} g {:.3f} {:.3f} {:.3f}", s, + g.norm(), g.x(), g.y(), g.z()); + if (fabs(g.norm() - IMUData::Gravity.norm()) > 1.0 || s < 0) { + SPDLOG_WARN( + "LinearAlignment Failed. Scale or gnorm wrong: g {:.3f} {:.3f} {:.3f} " + "s {:.3f}", + g.x(), g.y(), g.z(), s); + return false; + } + RefineGravity(sld_win, sfm_poses, extrinsic, g, x); + s = (x.tail<1>())(0) / 100.0; + (x.tail<1>())(0) = s; + if (s < 0.0) { + SPDLOG_WARN("LinearAlignment Failed in RefineGravity. Scale wrong"); + return false; + } + + // Recover camera poses and IMU poses using the scale + Eigen::Vector3d pos0 = Eigen::Vector3d::Zero(); + Quaterniond q0 = Utility::g2R(g); + double yaw = quat2eulers(q0 * sfm_poses.at(sld_win[0]->frame_id).att()).z(); + q0 = eulers2quat(Eigen::Vector3d{0, 0, -yaw}) * q0; + g = q0 * g; + SPDLOG_INFO("G final {:.4f} {:.4f} {:.4f}", g.x(), g.y(), g.z()); + + for (int i = 0; i < sld_win.size(); i++) { + auto frame = sld_win[i]; + Swarm::Pose imu_pose = sfm_poses.at(frame->frame_id); + if (i == 0) { + pos0 = s * imu_pose.pos() - imu_pose.att() * extrinsic.pos(); + } + imu_pose.pos() = + q0 * (imu_pose.pos() * s - imu_pose.att() * extrinsic.pos() - pos0); + imu_pose.att() = q0 * imu_pose.att(); + // Set the pose of the frame + Vector3d vel = imu_pose.R() * x.segment<3>(i * 3); + setPose(frame->frame_id, imu_pose); + setVelocity(frame->frame_id, vel); + SPDLOG_INFO("LinearAlignment: F{} IMU_pose {} vel {:.4f} {:.4f} {:.4f}", + sld_win[i]->frame_id, imu_pose.toStr(), vel.x(), vel.y(), + vel.z()); + } + return true; +} + +MatrixXd TangentBasis(Vector3d &g0) { + Vector3d b, c; + Vector3d a = g0.normalized(); + Vector3d tmp(0, 0, 1); + if (a == tmp) tmp << 1, 0, 0; + b = (tmp - a * (a.transpose() * tmp)).normalized(); + c = a.cross(b); + MatrixXd bc(3, 2); + bc.block<3, 1>(0, 0) = b; + bc.block<3, 1>(0, 1) = c; + return bc; +} + +void D2EstimatorState::RefineGravity( + std::vector sld_win, + const std::map &sfm_poses, Swarm::Pose extrinsic, + Vector3d &g, VectorXd &x) { + Vector3d g0 = g.normalized() * IMUData::Gravity.norm(); + Vector3d lx, ly; + // VectorXd x; + int all_frame_count = sld_win.size(); + int n_state = all_frame_count * 3 + 2 + 1; + + MatrixXd A{n_state, n_state}; + A.setZero(); + VectorXd b{n_state}; + b.setZero(); + + for (int k = 0; k < 4; k++) { + MatrixXd lxly(3, 2); + lxly = TangentBasis(g0); + int i = 0; + for (int i = 0; i < sld_win.size() - 1; i++) { + auto frame_i = sld_win[i]; + auto frame_j = sld_win[i + 1]; + Swarm::Pose pose_i = sfm_poses.at(frame_i->frame_id); + Swarm::Pose pose_j = sfm_poses.at(frame_j->frame_id); + Eigen::Matrix3d R_i = pose_i.R(); + Eigen::Matrix3d R_j = pose_j.R(); + Eigen::Vector3d T_i = pose_i.pos(); + Eigen::Vector3d T_j = pose_j.pos(); + + MatrixXd tmp_A(6, 9); + tmp_A.setZero(); + VectorXd tmp_b(6); + tmp_b.setZero(); + + double dt = frame_j->pre_integrations->sum_dt; + + tmp_A.block<3, 3>(0, 0) = -dt * Matrix3d::Identity(); + tmp_A.block<3, 2>(0, 6) = + R_i.transpose() * dt * dt / 2 * Matrix3d::Identity() * lxly; + tmp_A.block<3, 1>(0, 8) = R_i.transpose() * (T_j - T_i) / 100.0; + tmp_b.block<3, 1>(0, 0) = frame_j->pre_integrations->delta_p + + R_i.transpose() * R_j * extrinsic.pos() - + extrinsic.pos() - + R_i.transpose() * dt * dt / 2 * g0; + + tmp_A.block<3, 3>(3, 0) = -Matrix3d::Identity(); + tmp_A.block<3, 3>(3, 3) = R_i.transpose() * R_j; + tmp_A.block<3, 2>(3, 6) = + R_i.transpose() * dt * Matrix3d::Identity() * lxly; + tmp_b.block<3, 1>(3, 0) = + frame_j->pre_integrations->delta_v - + R_i.transpose() * dt * Matrix3d::Identity() * g0; + + Matrix cov_inv = Matrix::Zero(); + // cov.block<6, 6>(0, 0) = IMU_cov[i + 1]; + // MatrixXd cov_inv = cov.inverse(); + cov_inv.setIdentity(); + + MatrixXd r_A = tmp_A.transpose() * cov_inv * tmp_A; + VectorXd r_b = tmp_A.transpose() * cov_inv * tmp_b; + + A.block<6, 6>(i * 3, i * 3) += r_A.topLeftCorner<6, 6>(); + b.segment<6>(i * 3) += r_b.head<6>(); + + A.bottomRightCorner<3, 3>() += r_A.bottomRightCorner<3, 3>(); + b.tail<3>() += r_b.tail<3>(); + + A.block<6, 3>(i * 3, n_state - 3) += r_A.topRightCorner<6, 3>(); + A.block<3, 6>(n_state - 3, i * 3) += r_A.bottomLeftCorner<3, 6>(); + } + A = A * 1000.0; + b = b * 1000.0; + x = A.ldlt().solve(b); + VectorXd dg = x.segment<2>(n_state - 3); + g0 = (g0 + lxly * dg).normalized() * IMUData::Gravity.norm(); + } + g = g0; + SPDLOG_INFO( + "RefineGravity: scale {:.3f} g_norm: {:.3f} g {:.3f} {:.3f} {:.3f}", + x(n_state - 1), g.norm(), g.x(), g.y(), g.z()); +} +} // namespace D2VINS diff --git a/d2vins/src/estimator/d2vinsstate.hpp b/d2vins/src/estimator/d2vinsstate.hpp index bd884254..76dd52b0 100644 --- a/d2vins/src/estimator/d2vinsstate.hpp +++ b/d2vins/src/estimator/d2vinsstate.hpp @@ -34,7 +34,11 @@ class D2EstimatorState : public D2State { void outlierRejection(const std::set & used_landmarks); void updateSldWinsIMU(const std::map & remote_imu_bufs); void createPriorFactor4FirstFrame(VINSFrame * frame); - void solveGyroscopeBias(); + bool solveGyroscopeBias(std::vector sld_win, const std::map& sfm_poses, Swarm::Pose extrinsic); + bool LinearAlignment(std::vector sld_win, + const std::map& sfm_poses, Swarm::Pose extrinsic); + void RefineGravity(std::vector sld_win, + const std::map& sfm_poses, Swarm::Pose extrinsic, Vector3d &g, VectorXd &x); public: state_type td = 0.0; D2EstimatorState(int _self_id); @@ -64,12 +68,13 @@ class D2EstimatorState : public D2State { std::vector localCameraExtrinsics() const; //Frame operations - std::vector clearUselessFrames(); + std::vector clearUselessFrames(bool marginalization=true); VINSFrame * addFrame(const VisualImageDescArray & images, const VINSFrame & _frame); void updateSldwin(int drone_id, const std::vector & sld_win); virtual void moveAllPoses(int new_ref_frame_id, const Swarm::Pose & delta_pose) override; const std::vector & getSldWin(int drone_id) const; VINSFrame * addVINSFrame(const VINSFrame & _frame); + //Frame access VINSFrame & getFrame(int index); const VINSFrame & getFrame(int index) const; @@ -91,6 +96,11 @@ class D2EstimatorState : public D2State { void syncFromState(const std::set & used_landmarks); void preSolve(const std::map & remote_imu_bufs); void repropagateIMU(); + void setPose(FrameIdType frame_id, const Swarm::Pose & pose); + void setVelocity(FrameIdType frame_id, const Vector3d & velocity); + void setBias(FrameIdType frame_id, const Vector3d & ba, const Vector3d & bg); + + int numKeyframes() const; //Debug void printSldWin(const std::map & keyframe_measurments) const; @@ -104,5 +114,7 @@ class D2EstimatorState : public D2State { void updateEgoMotion(); void printLandmarkReport(FrameIdType frame_id) const; + bool monoInitialization(); + }; } \ No newline at end of file diff --git a/d2vins/src/estimator/landmark_manager.cpp b/d2vins/src/estimator/landmark_manager.cpp index 3a1c5e37..4fce20bf 100644 --- a/d2vins/src/estimator/landmark_manager.cpp +++ b/d2vins/src/estimator/landmark_manager.cpp @@ -1,395 +1,767 @@ #include "landmark_manager.hpp" -#include "d2vinsstate.hpp" + +#include + #include "../d2vins_params.hpp" +#include "../factors/reprojection3d.h" +#include "../utils/solve_5pts.h" +#include "d2frontend/utils.h" +#include "d2vinsstate.hpp" +#include "spdlog/spdlog.h" namespace D2VINS { -double triangulatePoint3DPts(const std::vector poses, const std::vector &points, Vector3d &point_3d); +double triangulatePoint3DPts(const std::vector poses, + const std::vector &points, + Vector3d &point_3d); -void D2LandmarkManager::addKeyframe(const VisualImageDescArray & images, double td) { - const Guard lock(state_lock); - for (auto & image : images.images) { - for (auto lm : image.landmarks) { - if (lm.landmark_id < 0) { - //Do not use unmatched features. - continue; - } - lm.cur_td = td; - updateLandmark(lm); - if (landmark_state.find(lm.landmark_id) == landmark_state.end()) { - if (params->landmark_param == D2VINSConfig::LM_INV_DEP) { - landmark_state[lm.landmark_id] = new state_type[INV_DEP_SIZE]; - } else { - landmark_state[lm.landmark_id] = new state_type[POS_SIZE]; - } - } +void D2LandmarkManager::addKeyframe(const VisualImageDescArray &images, + double td) { + const Guard lock(state_lock); + for (auto &image : images.images) { + for (auto lm : image.landmarks) { + if (lm.landmark_id < 0) { + // Do not use unmatched features. + continue; + } + lm.cur_td = td; + updateLandmark(lm); + if (landmark_state.find(lm.landmark_id) == landmark_state.end()) { + if (params->landmark_param == D2VINSConfig::LM_INV_DEP) { + landmark_state[lm.landmark_id] = new state_type[INV_DEP_SIZE]; + } else { + landmark_state[lm.landmark_id] = new state_type[POS_SIZE]; } + } } + } } -std::vector D2LandmarkManager::availableMeasurements(int max_pts, int max_solve_measurements, const std::set & current_frames) const { - std::map current_landmark_num; - std::map result_landmark_num; - std::map> current_assoicated_landmarks; - bool exit = false; - std::set ret_ids_set; - std::vector ret_set; - for (auto frame_id : current_frames) { - current_landmark_num[frame_id] = 0; - result_landmark_num[frame_id] = 0; +std::vector D2LandmarkManager::availableMeasurements( + int max_pts, int max_solve_measurements, + const std::set ¤t_frames) const { + std::map current_landmark_num; + std::map result_landmark_num; + std::map> + current_assoicated_landmarks; + bool exit = false; + std::set ret_ids_set; + std::vector ret_set; + for (auto frame_id : current_frames) { + current_landmark_num[frame_id] = 0; + result_landmark_num[frame_id] = 0; + } + int count_measurements = 0; + if (max_solve_measurements <= 0) { + max_solve_measurements = 1000000; + } + while (!exit) { + // found the frame with minimum landmarks in current frames + if (current_landmark_num.size() == 0) { + exit = true; } - int count_measurements = 0; - if (max_solve_measurements <= 0) { - max_solve_measurements = 1000000; + auto it = + min_element(current_landmark_num.begin(), current_landmark_num.end(), + [](decltype(current_landmark_num)::value_type &l, + decltype(current_landmark_num)::value_type &r) -> bool { + return l.second < r.second; + }); + auto frame_id = it->first; + // Add the a landmark in its related landmarks with highest score + if (related_landmarks.find(frame_id) == related_landmarks.end()) { + // Remove the frame from current_landmark_num + current_landmark_num.erase(frame_id); + continue; } - while (!exit) { - //found the frame with minimum landmarks in current frames - if (current_landmark_num.size() == 0) { - exit = true; - } - auto it = min_element(current_landmark_num.begin(), current_landmark_num.end(), - [](decltype(current_landmark_num)::value_type& l, decltype(current_landmark_num)::value_type& r) -> - bool { return l.second < r.second; }); - auto frame_id = it->first; - //Add the a landmark in its related landmarks with highest score - if (related_landmarks.find(frame_id) == related_landmarks.end()) { - //Remove the frame from current_landmark_num - current_landmark_num.erase(frame_id); - continue; - } - auto frame_related_landmarks = related_landmarks.at(frame_id); - //Find the landmark with highest score - LandmarkIdType lm_best; - double score_best = -10000; - bool found = false; - for (auto & itre : frame_related_landmarks) { - LandmarkIdType lm_id = itre.first; - if (landmark_db.find(lm_id) == landmark_db.end() || ret_ids_set.find(lm_id) != ret_ids_set.end()) { - //The landmark is not in the database or has been added - continue; - } - auto & lm = landmark_db.at(lm_id); - if (lm.track.size() >= params->landmark_estimate_tracks && - lm.flag >= LandmarkFlag::INITIALIZED) { - if (lm.scoreForSolve(params->self_id) > score_best) { - score_best = lm.scoreForSolve(params->self_id); - lm_best = lm_id; - found = true; - } - } - } - if (found) { - auto & lm = landmark_db.at(lm_best); - ret_set.emplace_back(lm); - ret_ids_set.insert(lm_best); - count_measurements += lm.track.size(); - //Add the frame to current_landmark_num - for (auto track: lm.track) { - auto frame_id = track.frame_id; - current_assoicated_landmarks[frame_id].insert(lm_best); - //We count the landmark numbers, but not the measurements - current_landmark_num[frame_id] = current_assoicated_landmarks[frame_id].size(); - result_landmark_num[frame_id] = current_landmark_num[frame_id]; - } - if (ret_set.size() >= max_pts || count_measurements >= max_solve_measurements) { - exit = true; - } - } else { - //Remove the frame from current_landmark_num - current_landmark_num.erase(frame_id); + auto frame_related_landmarks = related_landmarks.at(frame_id); + // Find the landmark with highest score + LandmarkIdType lm_best; + double score_best = -10000; + bool found = false; + for (auto &itre : frame_related_landmarks) { + LandmarkIdType lm_id = itre.first; + if (landmark_db.find(lm_id) == landmark_db.end() || + ret_ids_set.find(lm_id) != ret_ids_set.end()) { + // The landmark is not in the database or has been added + continue; + } + auto &lm = landmark_db.at(lm_id); + if (lm.track.size() >= params->landmark_estimate_tracks && + lm.flag >= LandmarkFlag::INITIALIZED) { + if (lm.scoreForSolve(params->self_id) > score_best) { + score_best = lm.scoreForSolve(params->self_id); + lm_best = lm_id; + found = true; } + } } - if (params->verbose) { - printf("[D2VINS::D2LandmarkManager] Found %ld(total %ld) landmarks measure %d/%d in %ld frames\n", ret_set.size(), landmark_db.size(), - count_measurements, max_solve_measurements, result_landmark_num.size()); + if (found) { + auto &lm = landmark_db.at(lm_best); + ret_set.emplace_back(lm); + ret_ids_set.insert(lm_best); + count_measurements += lm.track.size(); + // Add the frame to current_landmark_num + for (auto track : lm.track) { + auto frame_id = track.frame_id; + current_assoicated_landmarks[frame_id].insert(lm_best); + // We count the landmark numbers, but not the measurements + current_landmark_num[frame_id] = + current_assoicated_landmarks[frame_id].size(); + result_landmark_num[frame_id] = current_landmark_num[frame_id]; + } + if (ret_set.size() >= max_pts || + count_measurements >= max_solve_measurements) { + exit = true; + } + } else { + // Remove the frame from current_landmark_num + current_landmark_num.erase(frame_id); } - return ret_set; + } + if (params->verbose) { + printf( + "[D2VINS::D2LandmarkManager] Found %ld(total %ld) landmarks " + "measure %d/%d in %ld frames\n", + ret_set.size(), landmark_db.size(), count_measurements, + max_solve_measurements, result_landmark_num.size()); + } + return ret_set; } -double * D2LandmarkManager::getLandmarkState(LandmarkIdType landmark_id) const { - const Guard lock(state_lock); - return landmark_state.at(landmark_id); +double *D2LandmarkManager::getLandmarkState(LandmarkIdType landmark_id) const { + const Guard lock(state_lock); + return landmark_state.at(landmark_id); } - -void D2LandmarkManager::moveByPose(const Swarm::Pose & delta_pose) { - const Guard lock(state_lock); - for (auto it: landmark_db) { - auto & lm = it.second; - if (lm.flag != LandmarkFlag::UNINITIALIZED) { - lm.position = delta_pose * lm.position; - } +void D2LandmarkManager::moveByPose(const Swarm::Pose &delta_pose) { + const Guard lock(state_lock); + for (auto it : landmark_db) { + auto &lm = it.second; + if (lm.flag != LandmarkFlag::UNINITIALIZED) { + lm.position = delta_pose * lm.position; } + } } -void D2LandmarkManager::initialLandmarkState(LandmarkPerId & lm, const D2EstimatorState * state) { - const Guard lock(state_lock); - LandmarkPerFrame lm_first; - lm_first = lm.track[0]; - auto lm_id = lm.landmark_id; - auto pt3d_n = lm_first.pt3d_norm; - auto firstFrame = *state->getFramebyId(lm_first.frame_id); - // printf("[D2VINS::D2LandmarkManager] Try initial landmark %ld dep %d tracks %ld\n", lm_id, - // lm.track[0].depth_mea && lm.track[0].depth > params->min_depth_to_fuse && lm.track[0].depth < params->max_depth_to_fuse, - // lm.track.size()); - if (lm_first.depth_mea && lm_first.depth > params->min_depth_to_fuse && lm_first.depth < params->max_depth_to_fuse) { - //Use depth to initial - auto ext = state->getExtrinsic(lm_first.camera_id); - //Note in depth mode, pt3d = (u, v, w), depth is distance since we use unitsphere - Vector3d pos = pt3d_n * lm_first.depth; - pos = firstFrame.odom.pose()*ext*pos; - lm.position = pos; +void D2LandmarkManager::initialLandmarkState(LandmarkPerId &lm, + const D2EstimatorState *state) { + const Guard lock(state_lock); + LandmarkPerFrame lm_first; + lm_first = lm.track[0]; + auto lm_id = lm.landmark_id; + auto pt3d_n = lm_first.pt3d_norm; + auto firstFrame = *state->getFramebyId(lm_first.frame_id); + // printf("[D2VINS::D2LandmarkManager] Try initial landmark %ld dep %d + // tracks %ld\n", lm_id, + // lm.track[0].depth_mea && lm.track[0].depth > + // params->min_depth_to_fuse && lm.track[0].depth < + // params->max_depth_to_fuse, lm.track.size()); + if (lm_first.depth_mea && lm_first.depth > params->min_depth_to_fuse && + lm_first.depth < params->max_depth_to_fuse) { + // Use depth to initial + auto ext = state->getExtrinsic(lm_first.camera_id); + // Note in depth mode, pt3d = (u, v, w), depth is distance since we use + // unitsphere + Vector3d pos = pt3d_n * lm_first.depth; + pos = firstFrame.odom.pose() * ext * pos; + lm.position = pos; + if (params->landmark_param == D2VINSConfig::LM_INV_DEP) { + *landmark_state[lm_id] = 1 / lm_first.depth; + if (params->debug_print_states) { + printf( + "[D2VINS::D2LandmarkManager] Initialize landmark %ld by " + "depth measurement position %.3f %.3f %.3f inv_dep %.3f\n", + lm_id, pos.x(), pos.y(), pos.z(), 1 / lm_first.depth); + } + } else { + memcpy(landmark_state[lm_id], lm.position.data(), + sizeof(state_type) * POS_SIZE); + } + lm.flag = LandmarkFlag::INITIALIZED; + } else if (lm.track.size() >= params->landmark_estimate_tracks || + lm.isMultiCamera()) { + // Initialize by motion. + std::vector poses; + std::vector points; + auto ext_base = state->getExtrinsic(lm_first.camera_id); + Eigen::Vector3d _min = (firstFrame.odom.pose() * ext_base).pos(); + Eigen::Vector3d _max = (firstFrame.odom.pose() * ext_base).pos(); + for (auto &it : lm.track) { + auto frame = *state->getFramebyId(it.frame_id); + auto ext = state->getExtrinsic(it.camera_id); + auto cam_pose = frame.odom.pose() * ext; + poses.push_back(cam_pose); + points.push_back(it.pt3d_norm); + _min = _min.cwiseMin((frame.odom.pose() * ext).pos()); + _max = _max.cwiseMax((frame.odom.pose() * ext).pos()); + } + if ((_max - _min).norm() > params->depth_estimate_baseline) { + // Initialize by triangulation + Vector3d point_3d(0., 0., 0.); + double tri_err = triangulatePoint3DPts(poses, points, point_3d); + if (tri_err < params->tri_max_err) { + lm.position = point_3d; if (params->landmark_param == D2VINSConfig::LM_INV_DEP) { - *landmark_state[lm_id] = 1/lm_first.depth; + auto ptcam = (firstFrame.odom.pose() * ext_base).inverse() * point_3d; + auto inv_dep = 1 / ptcam.norm(); + if (inv_dep < params->max_inv_dep) { + lm.flag = LandmarkFlag::INITIALIZED; + *landmark_state[lm_id] = inv_dep; + if (params->debug_print_states) { + SPDLOG_INFO( + "Landmark {} " + "tracks {} baseline {:.2f} by tri. P {:.3f} " + "{:.3f} {:.3f} inv_dep {:.3f} err {:.3f}\n", + lm_id, lm.track.size(), (_max - _min).norm(), point_3d.x(), + point_3d.y(), point_3d.z(), inv_dep, tri_err); + } + } else { + lm.flag = LandmarkFlag::INITIALIZED; + *landmark_state[lm_id] = params->default_inv_dep; if (params->debug_print_states) { - printf("[D2VINS::D2LandmarkManager] Initialize landmark %ld by depth measurement position %.3f %.3f %.3f inv_dep %.3f\n", - lm_id, pos.x(), pos.y(), pos.z(), 1/lm_first.depth); + SPDLOG_WARN( + "Initialize failed too far away: landmark " + "{} tracks {} baseline {:.2f} by " + "triangulation position {:.3f} {:.3f} {:.3f} " + "inv_dep {:.3f}", + lm_id, lm.track.size(), (_max - _min).norm(), point_3d.x(), + point_3d.y(), point_3d.z(), inv_dep); } + } + if (params->debug_print_states) { + for (auto &it : lm.track) { + auto frame = *state->getFramebyId(it.frame_id); + auto ext = state->getExtrinsic(it.camera_id); + auto cam_pose = frame.odom.pose() * ext; + auto reproject_pos = cam_pose.inverse() * point_3d; + reproject_pos.normalize(); + SPDLOG_INFO( + "Frame {} camera_id {} index {} cam pose: {}" + "pt3d norm {:.3f} {:.3f} {:.3f} reproject " + "{:.3f} {:.3f} {:.3f}", + it.frame_id, it.camera_id, it.camera_index, + cam_pose.toStr().c_str(), it.pt3d_norm.x(), it.pt3d_norm.y(), + it.pt3d_norm.z(), reproject_pos.x(), reproject_pos.y(), + reproject_pos.z()); + } + } } else { - memcpy(landmark_state[lm_id], lm.position.data(), sizeof(state_type)*POS_SIZE); + lm.flag = LandmarkFlag::INITIALIZED; + memcpy(landmark_state[lm_id], lm.position.data(), + sizeof(state_type) * POS_SIZE); } - lm.flag = LandmarkFlag::INITIALIZED; - } else if (lm.track.size() >= params->landmark_estimate_tracks || lm.isMultiCamera()) { - //Initialize by motion. - std::vector poses; - std::vector points; - auto ext_base = state->getExtrinsic(lm_first.camera_id); - Eigen::Vector3d _min = (firstFrame.odom.pose()*ext_base).pos(); - Eigen::Vector3d _max = (firstFrame.odom.pose()*ext_base).pos(); - for (auto & it: lm.track) { - auto frame = *state->getFramebyId(it.frame_id); - auto ext = state->getExtrinsic(it.camera_id); - auto cam_pose = frame.odom.pose()*ext; - poses.push_back(cam_pose); - points.push_back(it.pt3d_norm); - _min = _min.cwiseMin((frame.odom.pose()*ext).pos()); - _max = _max.cwiseMax((frame.odom.pose()*ext).pos()); + // Some debug code + } else { + if (params->debug_print_states) { + SPDLOG_WARN( + "Initialize " + "failed too large triangle error: landmark {} " + "tracks {} baseline {:.2f} by triangulation position " + "{:.3f} {:.3f} {:.3f}", + lm_id, lm.track.size(), (_max - _min).norm(), point_3d.x(), + point_3d.y(), point_3d.z()); } - if ((_max - _min).norm() > params->depth_estimate_baseline) { - //Initialize by triangulation - Vector3d point_3d(0., 0., 0.); - double tri_err = triangulatePoint3DPts(poses, points, point_3d); - // printf("Lm %ld tri err %.3f thres %.3f\n", lm_id, tri_err*params->focal_length, params->tri_max_err*params->focal_length); - if (tri_err < params->tri_max_err) { - lm.position = point_3d; - if (params->landmark_param == D2VINSConfig::LM_INV_DEP) { - auto ptcam = (firstFrame.odom.pose()*ext_base).inverse()*point_3d; - auto inv_dep = 1/ptcam.norm(); - if (inv_dep > params->min_inv_dep) { - lm.flag = LandmarkFlag::INITIALIZED; - *landmark_state[lm_id] = inv_dep; - if (params->debug_print_states) { - printf("[D2VINS::D2LandmarkManager] Landmark %ld tracks %ld baseline %.2f by tri. P %.3f %.3f %.3f inv_dep %.3f err %.3f\n", - lm_id, lm.track.size(), (_max - _min).norm(), point_3d.x(), point_3d.y(), point_3d.z(), inv_dep, tri_err); - } - } else { - lm.flag = LandmarkFlag::INITIALIZED; - *landmark_state[lm_id] = params->min_inv_dep; - if (params->debug_print_states) { - printf("\033[0;31m [D2VINS::D2LandmarkManager] Initialize failed too far away: landmark %ld tracks %ld baseline %.2f by triangulation position %.3f %.3f %.3f inv_dep %.3f \033[0m\n", - lm_id, lm.track.size(), (_max - _min).norm(), point_3d.x(), point_3d.y(), point_3d.z(), inv_dep); - } - } - // for (auto & it: lm.track) { - // auto frame = *state->getFramebyId(it.frame_id); - // auto ext = state->getExtrinsic(it.camera_id); - // auto cam_pose = frame.odom.pose()*ext; - // auto reproject_pos = cam_pose.inverse()*point_3d; - // reproject_pos.normalize(); - // printf("Frame %ld camera_id %d index %d cam pose: %s pt3d norm %.3f %.3f %.3f reproject %.3f %.3f %.3f\n", - // it.frame_id, it.camera_id, it.camera_index, cam_pose.toStr().c_str(), it.pt3d_norm.x(), it.pt3d_norm.y(), it.pt3d_norm.z(), - // reproject_pos.x(), reproject_pos.y(), reproject_pos.z()); - // } - } else { - lm.flag = LandmarkFlag::INITIALIZED; - memcpy(landmark_state[lm_id], lm.position.data(), sizeof(state_type)*POS_SIZE); - } - // Some debug code - } else { - if (params->debug_print_states) { - printf("\033[0;31m [D2VINS::D2LandmarkManager] Initialize failed too large triangle error: landmark %ld tracks %ld baseline %.2f by triangulation position %.3f %.3f %.3f\033[0m\n", - lm_id, lm.track.size(), (_max - _min).norm(), point_3d.x(), point_3d.y(), point_3d.z()); - } - } - } else { - if (params->debug_print_states) { - printf("\033[0;31m [D2VINS::D2LandmarkManager] Initialize failed too short baseline: landmark %ld tracks %ld baseline %.2f\033[0m\n", - lm_id, lm.track.size(), (_max - _min).norm()); - } + } + } else { + if (params->debug_print_states) { + SPDLOG_WARN( + "Initialize " + "failed too short baseline: landmark {} tracks {} " + "baseline {:.2f}", + lm_id, lm.track.size(), (_max - _min).norm()); + } + } + } +} + +void D2LandmarkManager::initialLandmarks(const D2EstimatorState *state) { + const Guard lock(state_lock); + int inited_count = 0; + for (auto &it : landmark_db) { + auto &lm = it.second; + auto lm_id = it.first; + // Set to unsolved + lm.solver_flag = LandmarkSolverFlag::UNSOLVED; + if (lm.flag < LandmarkFlag::ESTIMATED) { + if (lm.track.size() == 0) { + SPDLOG_ERROR( + "Initialize landmark " + "{} failed, no track.", + lm_id); + continue; + } + initialLandmarkState(lm, state); + inited_count += 1; + } else if (lm.flag == LandmarkFlag::ESTIMATED) { + // Extracting depth from estimated pos + inited_count += 1; + if (params->landmark_param == D2VINSConfig::LM_INV_DEP) { + auto lm_per_frame = landmark_db.at(lm_id).track[0]; + auto firstFrame = state->getFramebyId(lm_per_frame.frame_id); + auto ext = state->getExtrinsic(lm_per_frame.camera_id); + Vector3d pos_cam = + (firstFrame->odom.pose() * ext).inverse() * lm.position; + *landmark_state[lm_id] = 1.0 / pos_cam.norm(); + } else { + memcpy(landmark_state[lm_id], lm.position.data(), + sizeof(state_type) * POS_SIZE); + } + } + } + + spdlog::debug("Total {} initialized {}", landmark_db.size(), inited_count); +} + +void D2LandmarkManager::outlierRejection( + const D2EstimatorState *state, + const std::set &used_landmarks) { + const Guard lock(state_lock); + int remove_count = 0; + int total_count = 0; + if (estimated_landmark_size < params->perform_outlier_rejection_num) { + return; + } + for (auto &it : landmark_db) { + auto &lm = it.second; + auto lm_id = it.first; + if (lm.flag == LandmarkFlag::ESTIMATED && + used_landmarks.find(lm_id) != used_landmarks.end()) { + double err_sum = 0; + double err_cnt = 0; + int count_err_track = 0; + total_count++; + for (auto it = lm.track.begin() + 1; it != lm.track.end();) { + auto pose = state->getFramebyId(it->frame_id)->odom.pose(); + auto ext = state->getExtrinsic(it->camera_id); + auto pt3d_n = it->pt3d_norm; + Vector3d pos_cam = (pose * ext).inverse() * lm.position; + pos_cam.normalize(); + // Compute reprojection error + Vector3d reproj_error = pt3d_n - pos_cam; + if (reproj_error.norm() * params->focal_length > + params->landmark_outlier_threshold) { + count_err_track += 1; + ++it; + } else { + ++it; } + err_sum += reproj_error.norm(); + err_cnt += 1; + } + lm.num_outlier_tracks = count_err_track; + if (err_cnt > 0) { + double reproj_err = err_sum / err_cnt; + if (reproj_err * params->focal_length > + params->landmark_outlier_threshold) { + remove_count++; + lm.flag = LandmarkFlag::OUTLIER; + spdlog::debug( + "remove LM {} inv_dep/dep " + "{:.2f}/{:.2f} pos {:.2f} {:.2f} {:.2f} reproj_error " + "{:.2f}", + lm_id, *landmark_state[lm_id], 1. / (*landmark_state[lm_id]), + lm.position.x(), lm.position.y(), lm.position.z(), + reproj_err * params->focal_length); + } + } } + } + spdlog::debug("outlierRejection remove {}/{} landmarks", remove_count, + total_count); } -void D2LandmarkManager::initialLandmarks(const D2EstimatorState * state) { - const Guard lock(state_lock); - int inited_count = 0; - for (auto & it: landmark_db) { - auto & lm = it.second; - auto lm_id = it.first; - //Set to unsolved - lm.solver_flag = LandmarkSolverFlag::UNSOLVED; - if (lm.flag < LandmarkFlag::ESTIMATED) { - if (lm.track.size() == 0) { - printf("\033[0;31m[D2VINS::D2LandmarkManager] Initialize landmark %ld failed, no track.\033[0m\n", lm_id); - continue; - } - initialLandmarkState(lm, state); - inited_count += 1; - } else if(lm.flag == LandmarkFlag::ESTIMATED) { - //Extracting depth from estimated pos - inited_count += 1; - if (params->landmark_param == D2VINSConfig::LM_INV_DEP) { - auto lm_per_frame = landmark_db.at(lm_id).track[0]; - auto firstFrame = state->getFramebyId(lm_per_frame.frame_id); - auto ext = state->getExtrinsic(lm_per_frame.camera_id); - Vector3d pos_cam = (firstFrame->odom.pose()*ext).inverse()*lm.position; - *landmark_state[lm_id] = 1.0/pos_cam.norm(); - } else { - memcpy(landmark_state[lm_id], lm.position.data(), sizeof(state_type)*POS_SIZE); - } +void D2LandmarkManager::syncState(const D2EstimatorState *state) { + const Guard lock(state_lock); + // Sync inverse depth to 3D positions + estimated_landmark_size = 0; + for (auto it : landmark_state) { + auto lm_id = it.first; + auto &lm = landmark_db.at(lm_id); + if (lm.solver_flag == LandmarkSolverFlag::SOLVED) { + if (params->landmark_param == D2VINSConfig::LM_INV_DEP) { + auto inv_dep = *it.second; + if (inv_dep < 0) { + spdlog::debug("[Warn] small inv dep {:.2f} found", inv_dep); + lm.flag = LandmarkFlag::OUTLIER; + continue; + } + if (inv_dep > params->max_inv_dep) { + inv_dep = params->default_inv_dep; } + auto lm_per_frame = lm.track[0]; + const auto &firstFrame = state->getFramebyId(lm_per_frame.frame_id); + auto ext = state->getExtrinsic(lm_per_frame.camera_id); + auto pt3d_n = lm_per_frame.pt3d_norm; + Vector3d pos = pt3d_n / inv_dep; + pos = firstFrame->odom.pose() * ext * pos; + lm.position = pos; + lm.flag = LandmarkFlag::ESTIMATED; + // spdlog::debug( + // "update LM {:d} inv_dep/dep " + // "{:.2f}/{:.2f} depmea {:d} {:.2f} pt3d_n {:.2f} {:.2f} " + // "{:.2f} pos " + // "{:.2f} {:.2f} {:.2f} baseFrame {:d} pose {} extrinsic {}", + // lm_id, inv_dep, 1. / inv_dep, lm_per_frame.depth_mea, + // lm_per_frame.depth, pt3d_n.x(), pt3d_n.y(), pt3d_n.z(), + // pos.x(), pos.y(), pos.z(), lm_per_frame.frame_id, + // firstFrame->odom.pose().toStr().c_str(), + // ext.toStr().c_str()); + } else { + lm.position.x() = it.second[0]; + lm.position.y() = it.second[1]; + lm.position.z() = it.second[2]; + lm.flag = LandmarkFlag::ESTIMATED; + } + estimated_landmark_size++; } + } +} + +void D2LandmarkManager::removeLandmark(const LandmarkIdType &id) { + landmark_db.erase(id); + landmark_state.erase(id); +} + +double triangulatePoint3DPts(const std::vector poses, + const std::vector &points, + Vector3d &point_3d) { + MatrixXd design_matrix(poses.size() * 2, 4); + assert(poses.size() > 0 && poses.size() == points.size() && + "We at least have 2 poses and number of pts and poses must equal"); + for (unsigned int i = 0; i < poses.size(); i++) { + double norm = points[i].norm(); + double p0x = points[i][0] / norm; + double p0y = points[i][1] / norm; + double p0z = points[i][2] / norm; + Eigen::Matrix pose; + auto R0 = poses[i].R(); + auto t0 = poses[i].pos(); + pose.leftCols<3>() = R0.transpose(); + pose.rightCols<1>() = -R0.transpose() * t0; + design_matrix.row(i * 2) = p0x * pose.row(2) - p0z * pose.row(0); + design_matrix.row(i * 2 + 1) = p0y * pose.row(2) - p0z * pose.row(1); + } + Vector4d triangulated_point; + triangulated_point = + design_matrix.jacobiSvd(Eigen::ComputeFullV).matrixV().rightCols<1>(); + point_3d(0) = triangulated_point(0) / triangulated_point(3); + point_3d(1) = triangulated_point(1) / triangulated_point(3); + point_3d(2) = triangulated_point(2) / triangulated_point(3); - if (params->debug_print_states) { - printf("[D2VINS::D2LandmarkManager] Total %d initialized %d\n", - landmark_db.size(), inited_count); + double sum_err = 0; + double err_pose_0 = 0.0; + for (unsigned int i = 0; i < poses.size(); i++) { + auto reproject_pos = poses[i].inverse() * point_3d; + reproject_pos.normalize(); + Vector3d err = points[i].normalized() - reproject_pos; + if (i == 0) { + err_pose_0 = err.norm(); } + sum_err += err.norm(); + } + return sum_err / points.size() + err_pose_0; } -void D2LandmarkManager::outlierRejection(const D2EstimatorState * state, const std::set & used_landmarks) { - const Guard lock(state_lock); - int remove_count = 0; - int total_count = 0; - if (estimated_landmark_size < params->perform_outlier_rejection_num) { - return; +std::map D2LandmarkManager::SFMInitialization( + const std::vector frames, int camera_idx) { + spdlog::debug("SFMInitialization with camera {}", camera_idx); + + // TODO: Add camera param? or consider multi-camera case + // Here we assume we are using mono camera + // First we init with solve 5 points use last two frames + std::map initial; + assert(frames.size() > 2); + auto last_frame = frames[frames.size() - 1]; + VINSFrame *head_frame_for_match = nullptr; + Swarm::Pose relative_pose; + // Start the scale with solve5pts + for (size_t i = 0; i < frames.size() - 1; i++) { + head_frame_for_match = frames[i]; + if (SolveRelativePose5Pts(relative_pose, camera_idx, last_frame->frame_id, + head_frame_for_match->frame_id)) { + initial[last_frame->frame_id] = Swarm::Pose::Identity(); + initial[head_frame_for_match->frame_id] = relative_pose; + SPDLOG_INFO("Frame_id {} PnP result: {}", last_frame->frame_id, + initial[last_frame->frame_id].toStr()); + SPDLOG_INFO("Frame_id {} PnP result: {}", head_frame_for_match->frame_id, + relative_pose.toStr()); + break; + } else { + continue; } - for (auto & it: landmark_db) { - auto & lm = it.second; - auto lm_id = it.first; - if(lm.flag == LandmarkFlag::ESTIMATED && used_landmarks.find(lm_id)!=used_landmarks.end()) { - double err_sum = 0; - double err_cnt = 0; - int count_err_track = 0; - total_count ++; - for (auto it = lm.track.begin() + 1; it != lm.track.end();) { - auto pose = state->getFramebyId(it->frame_id)->odom.pose(); - auto ext = state->getExtrinsic(it->camera_id); - auto pt3d_n = it->pt3d_norm; - Vector3d pos_cam = (pose*ext).inverse()*lm.position; - pos_cam.normalize(); - //Compute reprojection error - Vector3d reproj_error = pt3d_n - pos_cam; - if (reproj_error.norm() * params->focal_length > params->landmark_outlier_threshold) { - count_err_track += 1; - // printf("[outlierRejection] remove outlier track LM %d frame %ld inv_dep/dep %.2f/%.2f reproj_err %.2f/%.2f\n", - // lm_id, it->frame_id, *landmark_state[lm_id], 1./(*landmark_state[lm_id]), reproj_error.norm() * params->focal_length, - // params->landmark_outlier_threshold); - // //Remove the track - // it = lm.track.erase(it); - ++it; - } else { - ++it; - } - err_sum += reproj_error.norm(); - err_cnt += 1; - } - lm.num_outlier_tracks = count_err_track; - if (err_cnt > 0) { - double reproj_err = err_sum/err_cnt; - if (reproj_err*params->focal_length > params->landmark_outlier_threshold) { - remove_count ++; - lm.flag = LandmarkFlag::OUTLIER; - if (params->verbose) { - printf("[outlierRejection] remove LM %d inv_dep/dep %.2f/%.2f pos %.2f %.2f %.2f reproj_error %.2f\n", - lm_id, *landmark_state[lm_id], 1./(*landmark_state[lm_id]), lm.position.x(), lm.position.y(), lm.position.z(), reproj_err*params->focal_length); - } - } - } - } + } + if (initial.size() == 0) { + SPDLOG_WARN("SFMInitialization failed"); + return initial; + } + + // First triangulation + auto last_triangluation_pts = + triangulationFrames(last_frame->frame_id, initial[last_frame->frame_id], + head_frame_for_match->frame_id, + initial[head_frame_for_match->frame_id], camera_idx); + + // Recursive triangluation + for (int i = frames.size() - 2; i >= 0; i--) { + auto frame = frames[i]; + if (frame->frame_id == head_frame_for_match->frame_id) { + continue; + } + Swarm::Pose pose = Swarm::Pose::Identity(); + // Found the landmarks observerd by frame and in points3d + if (InitFramePoseWithPts(pose, last_triangluation_pts, frame->frame_id, + camera_idx)) { + // Triangulate all the common points + initial[frame->frame_id] = pose; + last_triangluation_pts = triangulationFrames(initial, camera_idx, 2); + SPDLOG_INFO("{} points initialized", last_triangluation_pts.size()); + } else { + return std::map(); } - printf("[D2VINS::D2LandmarkManager] outlierRejection remove %d/%d landmarks\n", remove_count, total_count); + } + + // Now re-triangluation all points + auto initial_pts = triangulationFrames(initial, camera_idx, 3); + auto ret = PerformBA(initial, last_frame, head_frame_for_match, initial_pts, + camera_idx); + return ret; } -void D2LandmarkManager::syncState(const D2EstimatorState * state) { - const Guard lock(state_lock); - //Sync inverse depth to 3D positions - estimated_landmark_size = 0; - for (auto it : landmark_state) { - auto lm_id = it.first; - auto & lm = landmark_db.at(lm_id); - if (lm.solver_flag == LandmarkSolverFlag::SOLVED) { - if (params->landmark_param == D2VINSConfig::LM_INV_DEP) { - auto inv_dep = *it.second; - if (inv_dep < 0) { - printf("[Warn] negative inv dep %.2f found\n", inv_dep); - } - if (inv_dep < params->min_inv_dep) { - inv_dep = params->min_inv_dep; - } - auto lm_per_frame = lm.track[0]; - const auto & firstFrame = state->getFramebyId(lm_per_frame.frame_id); - auto ext = state->getExtrinsic(lm_per_frame.camera_id); - auto pt3d_n = lm_per_frame.pt3d_norm; - Vector3d pos = pt3d_n / inv_dep; - pos = firstFrame->odom.pose()*ext*pos; - lm.position = pos; - lm.flag = LandmarkFlag::ESTIMATED; - if (params->debug_print_states) { - printf("[D2VINS::D2LandmarkManager] update LM %d inv_dep/dep %.2f/%.2f depmea %d %.2f pt3d_n %.2f %.2f %.2f pos %.2f %.2f %.2f baseFrame %ld pose %s extrinsic %s\n", - lm_id, inv_dep, 1./inv_dep, lm_per_frame.depth_mea, lm_per_frame.depth, - pt3d_n.x(), pt3d_n.y(), pt3d_n.z(), - pos.x(), pos.y(), pos.z(), - lm_per_frame.frame_id, firstFrame->odom.pose().toStr().c_str(), ext.toStr().c_str()); - } - } else { - lm.position.x() = it.second[0]; - lm.position.y() = it.second[1]; - lm.position.z() = it.second[2]; - lm.flag = LandmarkFlag::ESTIMATED; - } - estimated_landmark_size ++; - } +const std::map D2LandmarkManager::PerformBA( + const std::map &initial, VINSFrame *last_frame, + VINSFrame *head_frame_for_match, + std::map initial_pts, int camera_idx) const { + SPDLOG_INFO("{} points initialized. Now start BA", initial_pts.size()); + std::map ret; + + std::map c_translation, c_rotation; + std::map points; + + ceres::Problem problem; + ceres::LocalParameterization *local_parameterization = + new ceres::EigenQuaternionParameterization(); + ceres::HuberLoss *loss = new ceres::HuberLoss(1.0); + + for (auto it : initial) { + // Initial states + auto frame_id = it.first; + c_translation[frame_id] = new double[3]; + c_rotation[frame_id] = new double[4]; + Eigen::Map pos(c_translation[frame_id]); + Eigen::Map q(c_rotation[frame_id]); + pos = it.second.pos(); + q = it.second.att(); + problem.AddParameterBlock(c_rotation[frame_id], 4, local_parameterization); + problem.AddParameterBlock(c_translation[frame_id], 3); + if (frame_id == last_frame->frame_id) { + problem.SetParameterBlockConstant(c_rotation[frame_id]); + } + if (frame_id == last_frame->frame_id || + frame_id == head_frame_for_match->frame_id) { + // For scale + problem.SetParameterBlockConstant(c_translation[frame_id]); } + } + + for (auto &it : landmark_db) { + auto &lm = it.second; + auto lm_id = it.first; + std::vector poses; + std::vector pt3d_norms; + if (initial_pts.count(lm_id) == 0 || + lm.track.size() < params->landmark_estimate_tracks) { + continue; + } + points[lm_id] = new double[3]; + Eigen::Map pt3d(points[lm_id]); + pt3d = initial_pts.at(lm_id); + + for (auto &it : lm.track) { + if (it.camera_id == camera_idx && initial.count(it.frame_id) > 0) { + const auto &pt3d_norm = it.pt3d_norm; + ceres::CostFunction *cost_function = ReprojectionError3D::Create( + pt3d_norm.x() / pt3d_norm.z(), pt3d_norm.y() / pt3d_norm.z()); + problem.AddResidualBlock(cost_function, loss, c_rotation[it.frame_id], + c_translation[it.frame_id], points.at(lm_id)); + } + } + } + + ceres::Solver::Options options; + options.linear_solver_type = ceres::DENSE_SCHUR; + options.max_solver_time_in_seconds = 0.2; + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + SPDLOG_INFO("Finish solve BA in {:.2f}ms. rpt {}", + summary.total_time_in_seconds * 1000.0, summary.BriefReport()); + Swarm::Pose pose0_inv = Swarm::Pose::Identity(); + bool is_pose0_set = false; + for (auto it : initial) { + // Initial states + auto frame_id = it.first; + Eigen::Map pos(c_translation[frame_id]); + Eigen::Map quat(c_rotation[frame_id]); + Swarm::Pose camera_pose(pos, quat); + if (!is_pose0_set) { + pose0_inv = camera_pose.inverse(); + is_pose0_set = true; + } + ret[frame_id] = pose0_inv * camera_pose; + SPDLOG_INFO("SfM init {}: Cam {}", frame_id, ret[frame_id].toStr()); + } + return ret; } -void D2LandmarkManager::removeLandmark(const LandmarkIdType & id) { - landmark_db.erase(id); - landmark_state.erase(id); +bool D2LandmarkManager::InitFramePoseWithPts( + Swarm::Pose &ret, + std::map &last_triangluation_pts, + FrameIdType frame_id, int camera_idx) { + auto landmark_ids = getRelatedLandmarks(frame_id); + std::vector points_3d; + std::vector points_undist; + std::vector landmark_ids_used; + for (auto lm_id : landmark_ids) { + if (last_triangluation_pts.find(lm_id) != last_triangluation_pts.end()) { + const auto &pt3d = last_triangluation_pts.at(lm_id); + points_3d.emplace_back(pt3d.x(), pt3d.y(), pt3d.z()); + auto lm = landmark_db.at(lm_id); + auto lm_per_frame = lm.at(frame_id); + cv::Point2f pt_undist( + lm_per_frame.pt3d_norm.x() / lm_per_frame.pt3d_norm.z(), + lm_per_frame.pt3d_norm.y() / lm_per_frame.pt3d_norm.z()); + points_undist.push_back(pt_undist); + landmark_ids_used.push_back(lm_id); + } + } + + // Then use cv::solvePnPRansac to solve the pose of frame + if (points_undist.size() < 5) { + SPDLOG_ERROR( + "PnP failed in " + "SFMInitialization on {}, " + "only {} pts", + frame_id, points_3d.size()); + return false; + } + cv::Mat rvec, tvec; + D2FrontEnd::PnPInitialFromCamPose(ret, rvec, tvec); + cv::Mat K = cv::Mat::eye(3, 3, CV_64F); // Use undist point, so identity + std::vector inliers; + cv::solvePnPRansac(points_3d, points_undist, K, cv::Mat(), rvec, tvec, true, + 100, 0.01, 0.99, inliers); + // Convert to eigen + ret = D2FrontEnd::PnPRestoCamPose(rvec, tvec); + // Print result + int num_inliers = 0; + for (size_t i = 0; i < inliers.size(); i++) { + if (inliers[i]) { + num_inliers++; + } else { + last_triangluation_pts.erase(landmark_ids_used[i]); + } + } + SPDLOG_INFO("Frame_id {} PnP result: {} inlier {}/{}", frame_id, ret.toStr(), + num_inliers, points_undist.size()); + return true; } -double triangulatePoint3DPts(const std::vector poses, const std::vector &points, Vector3d &point_3d) { - MatrixXd design_matrix(poses.size()*2, 4); - assert(poses.size() > 0 && poses.size() == points.size() && "We at least have 2 poses and number of pts and poses must equal"); - for (unsigned int i = 0; i < poses.size(); i ++) { - double p0x = points[i][0]; - double p0y = points[i][1]; - double p0z = points[i][2]; - Eigen::Matrix pose; - auto R0 = poses[i].R(); - auto t0 = poses[i].pos(); - pose.leftCols<3>() = R0.transpose(); - pose.rightCols<1>() = -R0.transpose() * t0; - design_matrix.row(i*2) = p0x * pose.row(2) - p0z*pose.row(0); - design_matrix.row(i*2+1) = p0y * pose.row(2) - p0z*pose.row(1); +bool D2LandmarkManager::SolveRelativePose5Pts(Swarm::Pose &ret, int camera_idx, + FrameIdType frame1_id, + FrameIdType frame2_id) { + // Get their landmarks and find the common + auto common_lm = findCommonLandmarkPerFrames(frame1_id, frame2_id); + + // Solve with 5 pts method + std::vector> corres; + double sum_parallex = 0.0; + for (auto &it : common_lm) { + auto &lm1 = it.first; + auto &lm2 = it.second; + // Check if the camera index is the same, if not we will skip this + // landmark + if (lm1.camera_id != camera_idx || lm2.camera_id != camera_idx) { + continue; } - Vector4d triangulated_point; - triangulated_point = - design_matrix.jacobiSvd(Eigen::ComputeFullV).matrixV().rightCols<1>(); - point_3d(0) = triangulated_point(0) / triangulated_point(3); - point_3d(1) = triangulated_point(1) / triangulated_point(3); - point_3d(2) = triangulated_point(2) / triangulated_point(3); - - double sum_err = 0; - double err_pose_0 = 0.0; - for (unsigned int i = 0; i < poses.size(); i ++) { - auto reproject_pos = poses[i].inverse()*point_3d; - reproject_pos.normalize(); - Vector3d err = points[i] - reproject_pos; - if (i == 0) { - err_pose_0 = err.norm(); - } - sum_err += err.norm(); + // Use pt3d_norm + corres.emplace_back(lm1.pt3d_norm, lm2.pt3d_norm); + sum_parallex += (lm1.pt3d_norm - lm2.pt3d_norm).norm(); + // Draw the arrow of the points using pt2d, it's already cv::Point2f + } + if (corres.size() < params->solve_relative_pose_min_pts || + sum_parallex / corres.size() < params->solve_relative_pose_min_parallex) { + SPDLOG_WARN( + "Solve 5 pts failed, only {} " + "pts, parallex {:.2f}", + corres.size(), sum_parallex / corres.size()); + return false; + } + utils::MotionEstimator estimator; + Matrix3d R; + Vector3d T; + if (!estimator.solveRelativeRT(corres, R, T)) { + SPDLOG_WARN("Solve 5 pts failed"); + return false; + } + ret = Swarm::Pose(R, T); + SPDLOG_INFO( + "Frame {}-{} Solve 5 pts with {} pts result: " + "{}", + frame1_id, frame2_id, corres.size(), ret.toStr()); + return true; +} + +std::map D2LandmarkManager::triangulationFrames( + FrameIdType frame1_id, const Swarm::Pose &pose1, FrameIdType frame2_id, + const Swarm::Pose &pose2, int camera_idx) { + auto common_lm = findCommonLandmarkPerFrames(frame1_id, frame2_id); + // triangluate these points use the pose + std::map points3d; + for (auto &it : common_lm) { + auto &lm1 = it.first; + auto &lm2 = it.second; + if (lm1.camera_id != camera_idx || lm2.camera_id != camera_idx) { + continue; } - return sum_err/ points.size() + err_pose_0; + std::vector points{lm1.pt3d_norm, lm2.pt3d_norm}; + std::vector poses{pose1, pose2}; + // Perform triangulation + Vector3d point_3d(0., 0., 0.); + auto ret = triangulatePoint3DPts(poses, points, point_3d); + if (ret < params->mono_initial_tri_max_err) + points3d[lm1.landmark_id] = point_3d; + } + return points3d; } + +std::map D2LandmarkManager::triangulationFrames( + const std::map &frame_poses, int camera_idx, + int min_tracks) { + std::map ret; + for (auto &it : landmark_db) { + auto &lm = it.second; + auto lm_id = it.first; + std::vector poses; + std::vector pt3d_norms; + if (lm.track.size() < min_tracks) { + continue; + } + for (auto &it : lm.track) { + if (it.camera_id == camera_idx && frame_poses.count(it.frame_id) > 0) { + poses.emplace_back(frame_poses.at(it.frame_id)); + pt3d_norms.emplace_back(it.pt3d_norm); + } + } + if (poses.size() < min_tracks) { + continue; + } + // Perform triangulation + Vector3d point_3d(0., 0., 0.); + auto err = triangulatePoint3DPts(poses, pt3d_norms, point_3d); + if (point_3d.norm() > 1e-2 && err < params->mono_initial_tri_max_err) { + ret[lm.landmark_id] = point_3d; + } + } + return ret; } + +} // namespace D2VINS diff --git a/d2vins/src/estimator/landmark_manager.hpp b/d2vins/src/estimator/landmark_manager.hpp index 0e38f6aa..8db1f9e2 100644 --- a/d2vins/src/estimator/landmark_manager.hpp +++ b/d2vins/src/estimator/landmark_manager.hpp @@ -1,23 +1,48 @@ #pragma once #include + #include "d2frontend/d2landmark_manager.h" namespace D2VINS { class D2EstimatorState; class D2LandmarkManager : public D2FrontEnd::LandmarkManager { - std::map landmark_state; + std::map landmark_state; int estimated_landmark_size = 0; - void initialLandmarkState(LandmarkPerId & lm, const D2EstimatorState * state); -public: - virtual void addKeyframe(const VisualImageDescArray & images, double td); - std::vector availableMeasurements(int max_pts, int max_solve_measurements, const std::set & current_frames) const; - double * getLandmarkState(LandmarkIdType landmark_id) const; - void initialLandmarks(const D2EstimatorState * state); - void syncState(const D2EstimatorState * state); - void outlierRejection(const D2EstimatorState * state, const std::set & used_landmarks); - void moveByPose(const Swarm::Pose & delta_pose); - virtual void removeLandmark(const LandmarkIdType & id) override; + void initialLandmarkState(LandmarkPerId &lm, const D2EstimatorState *state); + + public: + virtual void addKeyframe(const VisualImageDescArray &images, double td); + std::vector + availableMeasurements(int max_pts, int max_solve_measurements, + const std::set ¤t_frames) const; + double *getLandmarkState(LandmarkIdType landmark_id) const; + void initialLandmarks(const D2EstimatorState *state); + void syncState(const D2EstimatorState *state); + void outlierRejection(const D2EstimatorState *state, + const std::set &used_landmarks); + void moveByPose(const Swarm::Pose &delta_pose); + virtual void removeLandmark(const LandmarkIdType &id) override; + std::map + SFMInitialization(const std::vector frames, int camera_idx); + std::map + triangulationFrames(FrameIdType frame1_id, const Swarm::Pose &frame1, + FrameIdType frame2_id, const Swarm::Pose &frame2, + int camera_idx); + std::map + triangulationFrames(const std::map &frame_poses, + int camera_idx, int min_tracks); + bool SolveRelativePose5Pts(Swarm::Pose &ret, int camera_idx, + FrameIdType frame1_id, FrameIdType frame2_id); + bool InitFramePoseWithPts( + Swarm::Pose &ret, + std::map &last_triangluation_pts, + FrameIdType frame_id, int camera_idx); + const std::map + PerformBA(const std::map &initial, + VINSFrame *last_frame, VINSFrame *head_frame_for_match, + std::map initial_pts, + int camera_idx) const; }; -} \ No newline at end of file +} // namespace D2VINS \ No newline at end of file diff --git a/d2vins/src/estimator/marginalization/marginalization.cpp b/d2vins/src/estimator/marginalization/marginalization.cpp index 5ceca626..00d332ad 100644 --- a/d2vins/src/estimator/marginalization/marginalization.cpp +++ b/d2vins/src/estimator/marginalization/marginalization.cpp @@ -1,247 +1,297 @@ #include "marginalization.hpp" -#include "../d2vinsstate.hpp" + #include -#include "../../factors/prior_factor.h" + #include "../../factors/imu_factor.h" +#include "../../factors/prior_factor.h" #include "../../factors/projectionTwoFrameOneCamFactor.h" +#include "../d2vinsstate.hpp" using namespace D2Common; namespace D2VINS { void Marginalizer::addResidualInfo(ResidualInfo* info) { - residual_info_list.push_back(info); + residual_info_list.push_back(info); } -VectorXd Marginalizer::evaluate(SparseMat & J, int eff_residual_size, int eff_param_size) { - //Then evaluate all residuals - //Setup Jacobian - //row: sort by residual_info_list - //col: sort by params_list - int cul_res_size = 0; - std::vector> triplet_list; - VectorXd residual_vec(eff_residual_size); - for (auto info : residual_info_list) { - if (params->margin_enable_fej) { - //In this case, we need to evaluate the residual with the FEJ state - auto params = info->paramsList(state); - if (last_prior!=nullptr) { - last_prior->replacetoPrevLinearizedPoints(params); - } - info->Evaluate(params, true); - } else { - info->Evaluate(state); - } - auto params = info->paramsList(state); - auto residual_size = info->residualSize(); - residual_vec.segment(cul_res_size, residual_size) = info->residuals; - if (std::isnan(info->residuals.maxCoeff()) || std::isnan(info->residuals.minCoeff())) { - printf("\033[0;31m[D2VINS::Marginalizer] Residual type %d residuals is nan:\033[0m\n", - info->residual_type); - std::cout << info->residuals.transpose() << std::endl; - continue; - } - for (auto param_blk_i = 0; param_blk_i < params.size(); param_blk_i ++) { - auto & J_blk = info->jacobians[param_blk_i]; - //Place this J to row: cul_res_size, col: param_indices - auto i0 = cul_res_size; - auto j0 = _params.at(params[param_blk_i].pointer).index; - auto param_size = params[param_blk_i].size; - auto blk_eff_param_size = params[param_blk_i].eff_size; - if (std::isnan(J_blk.maxCoeff()) || std::isnan(J_blk.minCoeff())) { - printf("\033[0;31m[D2VINS::Marginalizer] Residual type %d param_blk %d jacobians is nan\033[0m:\n", - info->residual_type, param_blk_i); - std::cout << J_blk << std::endl; - continue; - } - - for (auto i = 0; i < residual_size; i ++) { - for (auto j = 0; j < blk_eff_param_size; j ++) { - //We only copy the eff param part, that is: on tangent space. - triplet_list.push_back(Eigen::Triplet(i0 + i, j0 + j, J_blk(i, j))); - } - } +VectorXd Marginalizer::evaluate(SparseMat& J, int eff_residual_size, + int eff_param_size) { + // Then evaluate all residuals + // Setup Jacobian + // row: sort by residual_info_list + // col: sort by params_list + int cul_res_size = 0; + std::vector> triplet_list; + VectorXd residual_vec(eff_residual_size); + for (auto info : residual_info_list) { + if (params->margin_enable_fej) { + // In this case, we need to evaluate the residual with the FEJ state + auto params = info->paramsList(state); + if (last_prior != nullptr) { + last_prior->replacetoPrevLinearizedPoints(params); + } + info->Evaluate(params, true); + } else { + info->Evaluate(state); + } + auto params = info->paramsList(state); + auto residual_size = info->residualSize(); + residual_vec.segment(cul_res_size, residual_size) = info->residuals; + if (std::isnan(info->residuals.maxCoeff()) || + std::isnan(info->residuals.minCoeff())) { + printf( + "\033[0;31m[D2VINS::Marginalizer] Residual type %d residuals is " + "nan:\033[0m\n", + info->residual_type); + std::cout << info->residuals.transpose() << std::endl; + continue; + } + for (auto param_blk_i = 0; param_blk_i < params.size(); param_blk_i++) { + auto& J_blk = info->jacobians[param_blk_i]; + // Place this J to row: cul_res_size, col: param_indices + auto i0 = cul_res_size; + auto j0 = _params.at(params[param_blk_i].pointer).index; + auto param_size = params[param_blk_i].size; + auto blk_eff_param_size = params[param_blk_i].eff_size; + if (std::isnan(J_blk.maxCoeff()) || std::isnan(J_blk.minCoeff())) { + printf( + "\033[0;31m[D2VINS::Marginalizer] Residual type %d param_blk %d " + "jacobians is nan\033[0m:\n", + info->residual_type, param_blk_i); + std::cout << J_blk << std::endl; + continue; + } + + for (auto i = 0; i < residual_size; i++) { + for (auto j = 0; j < blk_eff_param_size; j++) { + // We only copy the eff param part, that is: on tangent space. + triplet_list.push_back( + Eigen::Triplet(i0 + i, j0 + j, J_blk(i, j))); } - cul_res_size += residual_size; + } } - J.setFromTriplets(triplet_list.begin(), triplet_list.end()); - return residual_vec; + cul_res_size += residual_size; + } + J.setFromTriplets(triplet_list.begin(), triplet_list.end()); + return residual_vec; } int Marginalizer::filterResiduals() { - int eff_residual_size = 0; - for (auto it = residual_info_list.begin(); it != residual_info_list.end();) { - if ((*it)->relavant(remove_frame_ids)) { - eff_residual_size += (*it)->residualSize(); - auto param_list = (*it)->paramsList(state); - for (auto param_ : param_list) { - if (_params.find(param_.pointer) == _params.end()) { - _params[param_.pointer] = param_; - //Check if param should be remove - bool is_remove = false; - auto & param = _params[param_.pointer]; - if ((param.type == POSE || param.type == SPEED_BIAS) && remove_frame_ids.find(param.id) != remove_frame_ids.end()) { - param.is_remove = true; - } - if (param.type == LANDMARK) { - FrameIdType base_frame_id = state->getLandmarkBaseFrame(param.id); - param.is_remove = false; - if (remove_frame_ids.find(base_frame_id) != remove_frame_ids.end()) { - param.is_remove = true; - } else { - if (params->landmark_param == D2VINSConfig::LM_INV_DEP) { - // printf("[D2VINS::Marginalizer::filterResiduals] landmark %d base frame %d not in remove_frame_ids %ld but will be remove\n", param.id, base_frame_id, *remove_frame_ids.begin()); - param.is_remove = params->remove_base_when_margin_remote; - } - } - } - } + int eff_residual_size = 0; + for (auto it = residual_info_list.begin(); it != residual_info_list.end();) { + if ((*it)->relavant(remove_frame_ids)) { + eff_residual_size += (*it)->residualSize(); + auto param_list = (*it)->paramsList(state); + for (auto param_ : param_list) { + if (_params.find(param_.pointer) == _params.end()) { + _params[param_.pointer] = param_; + // Check if param should be remove + bool is_remove = false; + auto& param = _params[param_.pointer]; + if ((param.type == POSE || param.type == SPEED_BIAS) && + remove_frame_ids.find(param.id) != remove_frame_ids.end()) { + param.is_remove = true; + } + if (param.type == LANDMARK) { + FrameIdType base_frame_id = state->getLandmarkBaseFrame(param.id); + param.is_remove = false; + if (remove_frame_ids.find(base_frame_id) != + remove_frame_ids.end()) { + param.is_remove = true; + } else { + if (params->landmark_param == D2VINSConfig::LM_INV_DEP) { + // printf("[D2VINS::Marginalizer::filterResiduals] landmark %d + // base frame %d not in remove_frame_ids %ld but will be + // remove\n", param.id, base_frame_id, + // *remove_frame_ids.begin()); + param.is_remove = params->remove_base_when_margin_remote; + } } - it++; - } else { - it = residual_info_list.erase(it); + } } + } + it++; + } else { + it = residual_info_list.erase(it); } - return eff_residual_size; + } + return eff_residual_size; } -void Marginalizer::covarianceEstimation(const SparseMat & H) { - //This covariance estimation is for debug only. It does not estimate the real covariance in marginalization module. - //To make it estimate real cov, we need to use full jacobian instead part of the problem. - auto cov = Utility::inverse(H); - printf("covarianceEstimation\n"); - for (auto param: params_list) { - if (param.type == POSE) { - //Print pose covariance - printf("[D2VINS::Marginalizer::covarianceEstimation] pose %d covariance:\n", param.id); - std::cout << cov.block(param.index, param.index, POSE_EFF_SIZE, POSE_EFF_SIZE) << std::endl; - } - if (param.type == EXTRINSIC) { - //Print extrinsic covariance - printf("[D2VINS::Marginalizer::covarianceEstimation] extrinsic %d covariance:\n", param.id); - std::cout << cov.block(param.index, param.index, POSE_EFF_SIZE, POSE_EFF_SIZE) << std::endl; - } - if (param.type == SPEED_BIAS) { - //Print speed bias covariance - printf("[D2VINS::Marginalizer::covarianceEstimation] speed bias %d covariance:\n", param.id); - std::cout << cov.block(param.index, param.index, FRAME_SPDBIAS_SIZE, FRAME_SPDBIAS_SIZE) << std::endl; - } +void Marginalizer::covarianceEstimation(const SparseMat& H) { + // This covariance estimation is for debug only. It does not estimate the real + // covariance in marginalization module. To make it estimate real cov, we need + // to use full jacobian instead part of the problem. + auto cov = Utility::inverse(H); + printf("covarianceEstimation\n"); + for (auto param : params_list) { + if (param.type == POSE) { + // Print pose covariance + printf( + "[D2VINS::Marginalizer::covarianceEstimation] pose %d covariance:\n", + param.id); + std::cout << cov.block(param.index, param.index, POSE_EFF_SIZE, + POSE_EFF_SIZE) + << std::endl; + } + if (param.type == EXTRINSIC) { + // Print extrinsic covariance + printf( + "[D2VINS::Marginalizer::covarianceEstimation] extrinsic %d " + "covariance:\n", + param.id); + std::cout << cov.block(param.index, param.index, POSE_EFF_SIZE, + POSE_EFF_SIZE) + << std::endl; } + if (param.type == SPEED_BIAS) { + // Print speed bias covariance + printf( + "[D2VINS::Marginalizer::covarianceEstimation] speed bias %d " + "covariance:\n", + param.id); + std::cout << cov.block(param.index, param.index, FRAME_SPDBIAS_SIZE, + FRAME_SPDBIAS_SIZE) + << std::endl; + } + } } -void Marginalizer::showDeltaXofschurComplement(std::vector keep_params_list, const SparseMatrix & A, const Matrix & b) { - Eigen::SimplicialLLT> solver; - solver.compute(A); - VectorXd _b = solver.solve(b); - for (auto & param : keep_params_list) { - printf("param id %d type %d size %d \n", param.id, param.type, param.size); - std::cout << "A^-1.b:" << _b.segment(param.index, param.eff_size).transpose() << std::endl; - } +void Marginalizer::showDeltaXofschurComplement( + std::vector keep_params_list, const SparseMatrix& A, + const Matrix& b) { + Eigen::SimplicialLLT> solver; + solver.compute(A); + VectorXd _b = solver.solve(b); + for (auto& param : keep_params_list) { + printf("param id %d type %d size %d \n", param.id, param.type, param.size); + std::cout << "A^-1.b:" + << _b.segment(param.index, param.eff_size).transpose() + << std::endl; + } } -PriorFactor * Marginalizer::marginalize(std::set _remove_frame_ids) { - Utility::TicToc tic; - remove_frame_ids = _remove_frame_ids; - //Clear previous states - params_list.clear(); - _params.clear(); - //We first remove all factors that does not evolved frame - if (params->verbose) { - for (auto frame_id: remove_frame_ids) { - printf("[D2VINS::Marginalizer::marginalize] remove frame %d\n", frame_id); - } +PriorFactor* Marginalizer::marginalize( + std::set _remove_frame_ids) { + Utility::TicToc tic; + remove_frame_ids = _remove_frame_ids; + // Clear previous states + params_list.clear(); + _params.clear(); + // We first remove all factors that does not evolved frame + if (params->verbose) { + for (auto frame_id : remove_frame_ids) { + printf("[D2VINS::Marginalizer::marginalize] remove frame %d\n", frame_id); } + } - auto eff_residual_size = filterResiduals(); - - sortParams(); //sort the parameters - if (keep_block_size == 0 || remove_state_dim == 0) { - printf("\033[0;31m[D2VINS::Marginalizer::marginalize] keep_block_size=%d remove_state_dim%d\033[0m\n", keep_block_size, remove_state_dim); - return nullptr; + auto eff_residual_size = filterResiduals(); + + sortParams(); // sort the parameters + if (keep_block_size == 0 || remove_state_dim == 0) { + printf( + "\033[0;31m[D2VINS::Marginalizer::marginalize] keep_block_size=%d " + "remove_state_dim%d\033[0m\n", + keep_block_size, remove_state_dim); + return nullptr; + } + int keep_state_dim = total_eff_state_dim - remove_state_dim; + Utility::TicToc tt; + SparseMat J(eff_residual_size, total_eff_state_dim); + auto b = evaluate(J, eff_residual_size, total_eff_state_dim); + double t_eval = tt.toc(); + SparseMat H = SparseMatrix(J.transpose()) * J; + VectorXd g = + J.transpose() * b; // Ignore -b here and also in prior_factor.cpp + // toJacRes to reduce compuation + if (params->enable_perf_output) { + printf("[D2VINS::marginalize] evaluation %.1fms JtJ cost %.1fms\n", t_eval, + tt.toc() - t_eval); + } + std::vector keep_params_list( + params_list.begin(), params_list.begin() + keep_block_size); + if (params->margin_enable_fej && last_prior != nullptr) { + last_prior->replacetoPrevLinearizedPoints(keep_params_list); + } + // Compute the schur complement, by sparse LLT. + PriorFactor* prior = nullptr; + if (params->margin_sparse_solver) { + tt.tic(); + auto Ab = Utility::schurComplement(H, g, keep_state_dim); + if (params->enable_perf_output) { + printf("[D2VINS::marginalize] schurComplement cost %.1fms\n", tt.toc()); } - int keep_state_dim = total_eff_state_dim - remove_state_dim; + prior = new PriorFactor(keep_params_list, Ab.first, Ab.second); + // showDeltaXofschurComplement(keep_params_list, Ab.first, Ab.second); + } else { Utility::TicToc tt; - SparseMat J(eff_residual_size, total_eff_state_dim); - auto b = evaluate(J, eff_residual_size, total_eff_state_dim); - double t_eval = tt.toc(); - SparseMat H = SparseMatrix(J.transpose())*J; - VectorXd g = J.transpose()*b; //Ignore -b here and also in prior_factor.cpp toJacRes to reduce compuation + auto Ab = Utility::schurComplement(H.toDense(), g, keep_state_dim); + double t_schur = tt.toc(); + prior = new PriorFactor(keep_params_list, Ab.first, Ab.second); if (params->enable_perf_output) { - printf("[D2VINS::marginalize] evaluation %.1fms JtJ cost %.1fms\n", t_eval, tt.toc() - t_eval); - } - std::vector keep_params_list(params_list.begin(), params_list.begin() + keep_block_size); - if (params->margin_enable_fej && last_prior!=nullptr) { - last_prior->replacetoPrevLinearizedPoints(keep_params_list); - } - //Compute the schur complement, by sparse LLT. - PriorFactor * prior = nullptr; - if (params->margin_sparse_solver) { - tt.tic(); - auto Ab = Utility::schurComplement(H, g, keep_state_dim); - if (params->enable_perf_output) { - printf("[D2VINS::marginalize] schurComplement cost %.1fms\n", tt.toc()); - } - prior = new PriorFactor(keep_params_list, Ab.first, Ab.second); - // showDeltaXofschurComplement(keep_params_list, Ab.first, Ab.second); - } else { - Utility::TicToc tt; - auto Ab = Utility::schurComplement(H.toDense(), g, keep_state_dim); - double t_schur = tt.toc(); - prior = new PriorFactor(keep_params_list, Ab.first, Ab.second); - if (params->enable_perf_output) { - printf("[D2VINS::marginalize] schurComplement cost %.1fms newPrior %.1fms\n", t_schur, tt.toc() - t_schur); - } + printf( + "[D2VINS::marginalize] schurComplement cost %.1fms newPrior %.1fms\n", + t_schur, tt.toc() - t_schur); } + } - if (params->enable_perf_output || params->verbose) { - printf("[D2VINS::marginalize] time cost %.1fms frame_id %ld total_eff_state_dim: %d keep_size %d remove size %d eff_residual_size: %d keep_block_size %d \n", - tic.toc(), *remove_frame_ids.begin(), total_eff_state_dim, keep_state_dim, remove_state_dim, eff_residual_size, keep_block_size); - } + if (params->enable_perf_output || params->verbose) { + printf( + "[D2VINS::marginalize] time cost %.1fms frame_id %ld " + "total_eff_state_dim: %d keep_size %d remove size %d " + "eff_residual_size: %d keep_block_size %d \n", + tic.toc(), *remove_frame_ids.begin(), total_eff_state_dim, + keep_state_dim, remove_state_dim, eff_residual_size, keep_block_size); + } - if (params->debug_write_margin_matrix) { - Utility::writeMatrixtoFile(params->output_folder + "/H.txt", MatrixXd(H)); - Utility::writeMatrixtoFile(params->output_folder + "/g.txt", MatrixXd(g)); - } - - if (prior->hasNan()) { - printf("\033[0;31m[D2VINS::Marginalizer::marginalize] prior has nan\033[0m\n"); - return nullptr; - } - return prior; + if (params->debug_write_margin_matrix) { + Utility::writeMatrixtoFile(params->output_folder + "/H.txt", MatrixXd(H)); + Utility::writeMatrixtoFile(params->output_folder + "/g.txt", MatrixXd(g)); + } + + if (prior->hasNan()) { + printf( + "\033[0;31m[D2VINS::Marginalizer::marginalize] prior has nan\033[0m\n"); + return nullptr; + } + return prior; } void Marginalizer::sortParams() { - params_list.clear(); - for (auto it: _params) { - params_list.push_back(it.second); - } + params_list.clear(); + for (auto it : _params) { + params_list.push_back(it.second); + } - std::sort(params_list.begin(), params_list.end(), [](const ParamInfo & a, const ParamInfo & b) { - if (a.is_remove != b.is_remove) { - return a.is_remove < b.is_remove; - } else { - return a.type < b.type; - } - }); - - total_eff_state_dim = 0; //here on tangent space - remove_state_dim = 0; - keep_block_size = 0; - for (unsigned i = 0; i < params_list.size(); i++) { - auto & _param = _params.at(params_list[i].pointer); - _param.index = total_eff_state_dim; - total_eff_state_dim += _param.eff_size; - // if (!_param.is_remove) { - // printf("Param %p type %d id %d size %d index %d cul_param_size %d is remove %d\n", - // params_list[i].pointer, _param.type, _param.id, _param.size, _param.index, total_eff_state_dim, _param.is_remove); - // } - if (_param.is_remove) { - remove_state_dim += _param.eff_size; - } else { - keep_block_size ++; - } - params_list[i] = _param; + std::sort(params_list.begin(), params_list.end(), + [](const ParamInfo& a, const ParamInfo& b) { + if (a.is_remove != b.is_remove) { + return a.is_remove < b.is_remove; + } else { + return a.type < b.type; + } + }); + + total_eff_state_dim = 0; // here on tangent space + remove_state_dim = 0; + keep_block_size = 0; + for (unsigned i = 0; i < params_list.size(); i++) { + auto& _param = _params.at(params_list[i].pointer); + _param.index = total_eff_state_dim; + total_eff_state_dim += _param.eff_size; + // if (!_param.is_remove) { + // printf("Param %p type %d id %d size %d index %d cul_param_size %d is + // remove %d\n", + // params_list[i].pointer, _param.type, _param.id, _param.size, + // _param.index, total_eff_state_dim, _param.is_remove); + // } + if (_param.is_remove) { + remove_state_dim += _param.eff_size; + } else { + keep_block_size++; } + params_list[i] = _param; + } } - -} \ No newline at end of file +} // namespace D2VINS \ No newline at end of file diff --git a/d2vins/src/estimator/solver/ConsensusSync.cpp b/d2vins/src/estimator/solver/ConsensusSync.cpp index 9f82c439..e75e402e 100644 --- a/d2vins/src/estimator/solver/ConsensusSync.cpp +++ b/d2vins/src/estimator/solver/ConsensusSync.cpp @@ -1,41 +1,42 @@ #include "ConsensusSync.hpp" + #include namespace D2VINS { -DistributedVinsData::DistributedVinsData(const DistributedVinsData_t & msg): - stamp(toROSTime(msg.timestamp).toSec()), drone_id(msg.drone_id), solver_token(msg.solver_token), - iteration_count(msg.iteration_count), - reference_frame_id(msg.reference_frame_id) -{ - for (int i = 0; i < msg.frame_ids.size(); i++) { - frame_ids.emplace_back(msg.frame_ids[i]); - frame_poses.emplace_back(Swarm::Pose(msg.frame_poses[i])); - } - for (int i = 0; i < msg.extrinsic.size(); i++) { - extrinsic.emplace_back(Swarm::Pose(msg.extrinsic[i])); - cam_ids.emplace_back(msg.cam_ids[i]); - } +DistributedVinsData::DistributedVinsData(const DistributedVinsData_t& msg) + : stamp(toROSTime(msg.timestamp).toSec()), + drone_id(msg.drone_id), + solver_token(msg.solver_token), + iteration_count(msg.iteration_count), + reference_frame_id(msg.reference_frame_id) { + for (int i = 0; i < msg.frame_ids.size(); i++) { + frame_ids.emplace_back(msg.frame_ids[i]); + frame_poses.emplace_back(Swarm::Pose(msg.frame_poses[i])); + } + for (int i = 0; i < msg.extrinsic.size(); i++) { + extrinsic.emplace_back(Swarm::Pose(msg.extrinsic[i])); + cam_ids.emplace_back(msg.cam_ids[i]); + } } DistributedVinsData_t DistributedVinsData::toLCM() const { - DistributedVinsData_t msg; - msg.timestamp = toLCMTime(ros::Time(stamp)); - msg.drone_id = drone_id; - for (int i = 0; i < frame_ids.size(); i++) { - msg.frame_ids.emplace_back(frame_ids[i]); - msg.frame_poses.emplace_back(frame_poses[i].toLCM()); - } - for (int i = 0; i < extrinsic.size(); i++) { - msg.extrinsic.emplace_back(extrinsic[i].toLCM()); - msg.cam_ids.emplace_back(cam_ids[i]); - } - msg.camera_num = extrinsic.size(); - msg.sld_win_len = frame_ids.size(); - msg.solver_token = solver_token; - msg.iteration_count = iteration_count; - msg.reference_frame_id = reference_frame_id; - return msg; + DistributedVinsData_t msg; + msg.timestamp = toLCMTime(ros::Time(stamp)); + msg.drone_id = drone_id; + for (int i = 0; i < frame_ids.size(); i++) { + msg.frame_ids.emplace_back(frame_ids[i]); + msg.frame_poses.emplace_back(frame_poses[i].toLCM()); + } + for (int i = 0; i < extrinsic.size(); i++) { + msg.extrinsic.emplace_back(extrinsic[i].toLCM()); + msg.cam_ids.emplace_back(cam_ids[i]); + } + msg.camera_num = extrinsic.size(); + msg.sld_win_len = frame_ids.size(); + msg.solver_token = solver_token; + msg.iteration_count = iteration_count; + msg.reference_frame_id = reference_frame_id; + return msg; } - -} \ No newline at end of file +} // namespace D2VINS \ No newline at end of file diff --git a/d2vins/src/estimator/solver/VINSConsenusSolver.cpp b/d2vins/src/estimator/solver/VINSConsenusSolver.cpp index 5aad5937..717cfff9 100644 --- a/d2vins/src/estimator/solver/VINSConsenusSolver.cpp +++ b/d2vins/src/estimator/solver/VINSConsenusSolver.cpp @@ -1,109 +1,122 @@ #include "VINSConsenusSolver.hpp" -#include "../d2estimator.hpp" + #include "../../d2vins_params.hpp" +#include "../d2estimator.hpp" namespace D2VINS { void D2VINSConsensusSolver::setStateProperties() { - estimator->setStateProperties(); + estimator->setStateProperties(); } void D2VINSConsensusSolver::broadcastData() { - //sync pointers to remote_params. - for (auto it: consenus_params) { - auto pointer = it.first; - auto consenus_param = it.second; - if (!it.second.local_only) { - remote_params[pointer] = std::map(); - remote_params[pointer][self_id] = VectorXd(consenus_param.global_size); - memcpy(remote_params.at(pointer).at(self_id).data(), pointer, consenus_param.global_size*sizeof(state_type)); - // printf("set to pose id %d\n", params[pointer].id); - // std::cout << "remote_params[pointer][self_id]: " << remote_params[pointer][self_id].transpose() << std::endl; - } + // sync pointers to remote_params. + for (auto it : consenus_params) { + auto pointer = it.first; + auto consenus_param = it.second; + if (!it.second.local_only) { + remote_params[pointer] = std::map(); + remote_params[pointer][self_id] = VectorXd(consenus_param.global_size); + memcpy(remote_params.at(pointer).at(self_id).data(), pointer, + consenus_param.global_size * sizeof(state_type)); + // printf("set to pose id %d\n", params[pointer].id); + // std::cout << "remote_params[pointer][self_id]: " << + // remote_params[pointer][self_id].transpose() << std::endl; } + } - DistributedVinsData dist_data; - for (auto it: all_estimating_params) { - auto pointer = it.first; - auto param = it.second; - if (param.type == POSE) { - dist_data.frame_ids.emplace_back(param.id); - dist_data.frame_poses.emplace_back(Swarm::Pose(pointer)); - } else if (param.type == EXTRINSIC) { - dist_data.cam_ids.emplace_back(param.id); - dist_data.extrinsic.emplace_back(Swarm::Pose(pointer)); - } + DistributedVinsData dist_data; + for (auto it : all_estimating_params) { + auto pointer = it.first; + auto param = it.second; + if (param.type == POSE) { + dist_data.frame_ids.emplace_back(param.id); + dist_data.frame_poses.emplace_back(Swarm::Pose(pointer)); + } else if (param.type == EXTRINSIC) { + dist_data.cam_ids.emplace_back(param.id); + dist_data.extrinsic.emplace_back(Swarm::Pose(pointer)); } - dist_data.stamp = ros::Time::now().toSec(); - dist_data.drone_id = self_id; - dist_data.solver_token = solver_token; - dist_data.iteration_count = iteration_count; - estimator->sendDistributedVinsData(dist_data); + } + dist_data.stamp = ros::Time::now().toSec(); + dist_data.drone_id = self_id; + dist_data.solver_token = solver_token; + dist_data.iteration_count = iteration_count; + estimator->sendDistributedVinsData(dist_data); } - void D2VINSConsensusSolver::receiveAll() { - std::vector sync_datas = receiver->retrive_all(); - for (auto data: sync_datas) { - updateWithDistributedVinsData(data); - } - if (params->verbose) { - printf("[ConsensusSolver::receiveAll@%d] token %d iteration %d receive finsish %ld/%ld\n", - self_id, solver_token, iteration_count, sync_datas.size(), state->availableDrones().size()); - } + std::vector sync_datas = receiver->retrive_all(); + for (auto data : sync_datas) { + updateWithDistributedVinsData(data); + } + if (params->verbose) { + printf( + "[ConsensusSolver::receiveAll@%d] token %d iteration %d receive " + "finsish %ld/%ld\n", + self_id, solver_token, iteration_count, sync_datas.size(), + state->availableDrones().size()); + } } - -void D2VINSConsensusSolver::updateWithDistributedVinsData(const DistributedVinsData & dist_data) { - auto _state = static_cast(state); - for (int i = 0; i < dist_data.frame_ids.size(); i++) { - auto frame_id = dist_data.frame_ids[i]; - if (_state->hasFrame(frame_id)) { - auto pointer = _state->getPoseState(frame_id); - remote_params[pointer][dist_data.drone_id] = VectorXd(POSE_SIZE); - dist_data.frame_poses[i].to_vector(remote_params[pointer][dist_data.drone_id].data()); - Swarm::Pose local(pointer); - } +void D2VINSConsensusSolver::updateWithDistributedVinsData( + const DistributedVinsData& dist_data) { + auto _state = static_cast(state); + for (int i = 0; i < dist_data.frame_ids.size(); i++) { + auto frame_id = dist_data.frame_ids[i]; + if (_state->hasFrame(frame_id)) { + auto pointer = _state->getPoseState(frame_id); + remote_params[pointer][dist_data.drone_id] = VectorXd(POSE_SIZE); + dist_data.frame_poses[i].to_vector( + remote_params[pointer][dist_data.drone_id].data()); + Swarm::Pose local(pointer); } + } - for (int i = 0; i < dist_data.cam_ids.size(); i++) { - auto cam_id = dist_data.cam_ids[i]; - if (_state->hasCamera(cam_id)) { - auto pointer = _state->getExtrinsicState(cam_id); - remote_params[pointer][dist_data.drone_id] = VectorXd(POSE_SIZE); - dist_data.extrinsic[i].to_vector(remote_params[pointer][dist_data.drone_id].data()); - } + for (int i = 0; i < dist_data.cam_ids.size(); i++) { + auto cam_id = dist_data.cam_ids[i]; + if (_state->hasCamera(cam_id)) { + auto pointer = _state->getExtrinsicState(cam_id); + remote_params[pointer][dist_data.drone_id] = VectorXd(POSE_SIZE); + dist_data.extrinsic[i].to_vector( + remote_params[pointer][dist_data.drone_id].data()); } + } - if (config.verbose) { - printf("[ConsensusSolver::updateWithDistributedVinsData@%d] of drone %ld: solver token %ld iteration %ld\n", - self_id, dist_data.drone_id, dist_data.solver_token, dist_data.iteration_count); - } + if (config.verbose) { + printf( + "[ConsensusSolver::updateWithDistributedVinsData@%d] of drone %ld: " + "solver token %ld iteration %ld\n", + self_id, dist_data.drone_id, dist_data.solver_token, + dist_data.iteration_count); + } } - void D2VINSConsensusSolver::waitForSync() { - //Wait for all remote drone to publish result. - Utility::TicToc tic; - if (params->verbose) { - printf("[ConsensusSolver::waitForSync@%d] token %d iteration %d\n", self_id, solver_token, iteration_count - 1); - } - std::vector sync_datas; - while (tic.toc() < config.timout_wait_sync) { - //Wait for remote data - auto ret = receiver->retrive(solver_token, iteration_count); - sync_datas.insert(sync_datas.end(), ret.begin(), ret.end()); - usleep(100); - if (sync_datas.size() == state->availableDrones().size() - 1) { - break; - } - } - for (auto data: sync_datas) { - updateWithDistributedVinsData(data); - } - if (params->verbose) { - printf("[ConsensusSolver::waitForSync@%d] receive finsish %ld/%ld time %.1f/%.1fms\n", - self_id, sync_datas.size() + 1, state->availableDrones().size(), tic.toc(), config.timout_wait_sync); + // Wait for all remote drone to publish result. + Utility::TicToc tic; + if (params->verbose) { + printf("[ConsensusSolver::waitForSync@%d] token %d iteration %d\n", self_id, + solver_token, iteration_count - 1); + } + std::vector sync_datas; + while (tic.toc() < config.timout_wait_sync) { + // Wait for remote data + auto ret = receiver->retrive(solver_token, iteration_count); + sync_datas.insert(sync_datas.end(), ret.begin(), ret.end()); + usleep(100); + if (sync_datas.size() == state->availableDrones().size() - 1) { + break; } + } + for (auto data : sync_datas) { + updateWithDistributedVinsData(data); + } + if (params->verbose) { + printf( + "[ConsensusSolver::waitForSync@%d] receive finsish %ld/%ld time " + "%.1f/%.1fms\n", + self_id, sync_datas.size() + 1, state->availableDrones().size(), + tic.toc(), config.timout_wait_sync); + } } -} \ No newline at end of file +} // namespace D2VINS \ No newline at end of file diff --git a/d2vins/src/factors/prior_factor.cpp b/d2vins/src/factors/prior_factor.cpp index c298b984..d1dda853 100644 --- a/d2vins/src/factors/prior_factor.cpp +++ b/d2vins/src/factors/prior_factor.cpp @@ -1,189 +1,210 @@ #include "prior_factor.h" -#include "../estimator/marginalization/marginalization.hpp" + #include +#include "../estimator/marginalization/marginalization.hpp" + namespace D2VINS { bool PriorFactor::hasNan() const { - if (std::isnan(linearized_jac.maxCoeff()) || std::isnan(linearized_res.minCoeff())) { - printf("\033[0;31m [D2VINS::PriorFactor] linearized_jac has NaN\033[0m\n"); - return true; - } - if (std::isnan(linearized_res.maxCoeff()) || std::isnan(linearized_res.minCoeff())) { - printf("\033[0;31m [D2VINS::PriorFactor] linearized_res has NaN\033[0m\n"); - return true; - } - return false; + if (std::isnan(linearized_jac.maxCoeff()) || + std::isnan(linearized_res.minCoeff())) { + printf("\033[0;31m [D2VINS::PriorFactor] linearized_jac has NaN\033[0m\n"); + return true; + } + if (std::isnan(linearized_res.maxCoeff()) || + std::isnan(linearized_res.minCoeff())) { + printf("\033[0;31m [D2VINS::PriorFactor] linearized_res has NaN\033[0m\n"); + return true; + } + return false; } void PriorFactor::removeFrame(int frame_id) { - int move_idx = 0; - for (auto it = keep_params_list.begin(); it != keep_params_list.end();) { - auto & param = *it; - param.index -= move_idx; - if (param.id == frame_id && (param.type == ParamsType::POSE || param.type == ParamsType::SPEED_BIAS)) { - Utility::removeRows(linearized_jac, param.index, param.eff_size); - Utility::removeCols(linearized_jac, param.index, param.eff_size); - Utility::removeRows(linearized_res, param.index, param.eff_size); - keep_eff_param_dim-=param.eff_size; - keep_param_blk_num--; - move_idx += param.eff_size; - // printf("\033[0;31m [D2VINS::PriorFactor] remove frame %d type %d remain size %d \033[0m\n", frame_id, param.type, keep_eff_param_dim); - it = keep_params_list.erase(it); - } else { - it++; - } + int move_idx = 0; + for (auto it = keep_params_list.begin(); it != keep_params_list.end();) { + auto ¶m = *it; + param.index -= move_idx; + if (param.id == frame_id && (param.type == ParamsType::POSE || + param.type == ParamsType::SPEED_BIAS)) { + Utility::removeRows(linearized_jac, param.index, param.eff_size); + Utility::removeCols(linearized_jac, param.index, param.eff_size); + Utility::removeRows(linearized_res, param.index, param.eff_size); + keep_eff_param_dim -= param.eff_size; + keep_param_blk_num--; + move_idx += param.eff_size; + // printf("\033[0;31m [D2VINS::PriorFactor] remove frame %d type %d remain + // size %d \033[0m\n", frame_id, param.type, keep_eff_param_dim); + it = keep_params_list.erase(it); + } else { + it++; } - initDims(keep_params_list); + } + initDims(keep_params_list); } -bool PriorFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const -{ - Eigen::VectorXd dx(keep_eff_param_dim); +bool PriorFactor::Evaluate(double const *const *parameters, double *residuals, + double **jacobians) const { + Eigen::VectorXd dx(keep_eff_param_dim); + for (int i = 0; i < keep_param_blk_num; i++) { + auto &info = keep_params_list[i]; + int size = + info.size; // Use norminal size instead of tangent space size here. + int idx = info.index; + Eigen::Map x(parameters[i], size); + Eigen::Map x0(info.data_copied.data(), size); + // std::cout << "idx" << idx << "type" << info.type <<"size" << size << + // "keep_eff_param_dim" <(idx + 0) = x.head<3>() - x0.head<3>(); + Eigen::Quaterniond qerr = + Eigen::Quaterniond(x0(6), x0(3), x0(4), x0(5)).inverse() * + Eigen::Quaterniond(x(6), x(3), x(4), x(5)); + dx.segment<3>(idx + 3) = 2.0 * Utility::positify(qerr).vec(); + if (!(qerr.w() >= 0)) { + dx.segment<3>(idx + 3) = 2.0 * -Utility::positify(qerr).vec(); + } + } + } + Eigen::Map res(residuals, keep_eff_param_dim); + res = linearized_res + linearized_jac * dx; + + if (jacobians) { for (int i = 0; i < keep_param_blk_num; i++) { - auto & info = keep_params_list[i]; - int size = info.size; //Use norminal size instead of tangent space size here. + if (jacobians[i]) { + auto &info = keep_params_list[i]; + int size = + info.size; // Use norminal size instead of tangent space size here. int idx = info.index; - Eigen::Map x(parameters[i], size); - Eigen::Map x0(info.data_copied.data(), size); - // std::cout << "idx" << idx << "type" << info.type <<"size" << size << "keep_eff_param_dim" <(idx + 0) = x.head<3>() - x0.head<3>(); - Eigen::Quaterniond qerr = Eigen::Quaterniond(x0(6), x0(3), x0(4), x0(5)).inverse() * Eigen::Quaterniond(x(6), x(3), x(4), x(5)); - dx.segment<3>(idx + 3) = 2.0 * Utility::positify(qerr).vec(); - if (!(qerr.w() >= 0)) - { - dx.segment<3>(idx + 3) = 2.0 * -Utility::positify(qerr).vec(); - } - } + Eigen::Map> + jacobian(jacobians[i], keep_eff_param_dim, size); + jacobian.setZero(); + jacobian.leftCols(info.eff_size) = + linearized_jac.middleCols(idx, info.eff_size); + } } - Eigen::Map res(residuals, keep_eff_param_dim); - res = linearized_res + linearized_jac * dx; - - if (jacobians) { - for (int i = 0; i < keep_param_blk_num; i++) { - if (jacobians[i]) { - auto & info = keep_params_list[i]; - int size = info.size; //Use norminal size instead of tangent space size here. - int idx = info.index; - Eigen::Map> jacobian(jacobians[i], keep_eff_param_dim, size); - jacobian.setZero(); - jacobian.leftCols(info.eff_size) = linearized_jac.middleCols(idx, info.eff_size); - } - } - } - return true; + } + return true; } -std::vector PriorFactor::getKeepParamsPointers() const { - std::vector pointers; - // printf("prior blocks %d\n", keep_param_blk_num); - for (auto & info : keep_params_list) { - pointers.push_back(info.pointer); - } - return pointers; +std::vector PriorFactor::getKeepParamsPointers() const { + std::vector pointers; + // printf("prior blocks %d\n", keep_param_blk_num); + for (auto &info : keep_params_list) { + pointers.push_back(info.pointer); + } + return pointers; } -void PriorFactor::moveByPose(const Swarm::Pose & delta_pose) { - for (auto & info : keep_params_list) { - if (info.type == ParamsType::POSE) { - //Move the poses in x0 - Swarm::Pose pose0(info.data_copied.data()); - pose0 = delta_pose * pose0; - pose0.to_vector(info.data_copied.data()); - } - if (info.type == ParamsType::SPEED_BIAS) { - //Move the velocity - Eigen::Map speed(info.data_copied.data()); - speed = delta_pose.att() * speed; - } +void PriorFactor::moveByPose(const Swarm::Pose &delta_pose) { + for (auto &info : keep_params_list) { + if (info.type == ParamsType::POSE) { + // Move the poses in x0 + Swarm::Pose pose0(info.data_copied.data()); + pose0 = delta_pose * pose0; + pose0.to_vector(info.data_copied.data()); } + if (info.type == ParamsType::SPEED_BIAS) { + // Move the velocity + Eigen::Map speed(info.data_copied.data()); + speed = delta_pose.att() * speed; + } + } } std::vector PriorFactor::getKeepParams() const { - return keep_params_list; + return keep_params_list; } int PriorFactor::getEffParamsDim() const { - int size = 0; - for (auto & info : keep_params_list) { - size += info.eff_size; - } - return size; + int size = 0; + for (auto &info : keep_params_list) { + size += info.eff_size; + } + return size; } -void PriorFactor::initDims(const std::vector & _keep_params_list) { - keep_params_list = _keep_params_list; - keep_param_blk_num = keep_params_list.size(); - keep_eff_param_dim = getEffParamsDim(); - mutable_parameter_block_sizes()->clear(); - for (auto it : keep_params_list) { - mutable_parameter_block_sizes()->push_back(it.size); - keep_params_map[it.pointer] = it; - } - set_num_residuals(keep_eff_param_dim); +void PriorFactor::initDims(const std::vector &_keep_params_list) { + keep_params_list = _keep_params_list; + keep_param_blk_num = keep_params_list.size(); + keep_eff_param_dim = getEffParamsDim(); + mutable_parameter_block_sizes()->clear(); + for (auto it : keep_params_list) { + mutable_parameter_block_sizes()->push_back(it.size); + keep_params_map[it.pointer] = it; + } + set_num_residuals(keep_eff_param_dim); } -std::pair toJacRes(const MatrixXd & A_, const VectorXd & b) { - MatrixXd A = (A_ + A_.transpose())/2; - const double eps = 1e-8; - Eigen::SelfAdjointEigenSolver saes2(A); - Eigen::VectorXd S = Eigen::VectorXd((saes2.eigenvalues().array() > eps).select(saes2.eigenvalues().array(), 0)); - Eigen::VectorXd S_inv = Eigen::VectorXd((saes2.eigenvalues().array() > eps).select(saes2.eigenvalues().array().inverse(), 0)); - - Eigen::VectorXd S_sqrt = S.cwiseSqrt(); - Eigen::VectorXd S_inv_sqrt = S_inv.cwiseSqrt(); - - VectorXd e0 = S_inv_sqrt.asDiagonal() * saes2.eigenvectors().transpose() * b; - MatrixXd J_ = S_sqrt.asDiagonal() * saes2.eigenvectors().transpose(); - // Use pre-conditioned from OKVINS https://github.com/ethz-asl/okvis/blob/master/okvis_ceres/src/MarginalizationError.cpp - // VectorXd p = (A.diagonal().array() > eps).select(A.diagonal().cwiseSqrt(),1.0e-3); - // VectorXd p_inv = p.cwiseInverse(); - // SelfAdjointEigenSolver saes(p_inv.asDiagonal() * A * p_inv.asDiagonal() ); - // VectorXd S_ = (saes.eigenvalues().array() > eps).select( - // saes.eigenvalues().array(), 0); - // VectorXd S_pinv_ = (saes.eigenvalues().array() > eps).select( - // saes.eigenvalues().array().inverse(), 0); - // VectorXd S_sqrt_ = S_.cwiseSqrt(); - // VectorXd S_pinv_sqrt_ = S_pinv_.cwiseSqrt(); - - // // assign Jacobian - // MatrixXd J_ = (p.asDiagonal() * saes.eigenvectors() * (S_sqrt_.asDiagonal())).transpose(); - - // // constant error (residual) _e0 := (-pinv(J^T) * _b): - // Eigen::MatrixXd J_pinv_T = (S_pinv_sqrt_.asDiagonal()) - // * saes.eigenvectors().transpose() *p_inv.asDiagonal() ; - // VectorXd e0 = (-J_pinv_T * b); - if (params->debug_write_margin_matrix) { - Utility::writeMatrixtoFile("/home/xuhao/output/A.txt", A); - Utility::writeMatrixtoFile("/home/xuhao/output/b.txt", b); - Utility::writeMatrixtoFile("/home/xuhao/output/J.txt", J_); - Utility::writeMatrixtoFile("/home/xuhao/output/e0.txt", e0); - } - - return std::make_pair(J_, e0); +std::pair toJacRes(const MatrixXd &A_, const VectorXd &b) { + MatrixXd A = (A_ + A_.transpose()) / 2; + const double eps = 1e-8; + Eigen::SelfAdjointEigenSolver saes2(A); + Eigen::VectorXd S = + Eigen::VectorXd((saes2.eigenvalues().array() > eps) + .select(saes2.eigenvalues().array(), 0)); + Eigen::VectorXd S_inv = + Eigen::VectorXd((saes2.eigenvalues().array() > eps) + .select(saes2.eigenvalues().array().inverse(), 0)); + + Eigen::VectorXd S_sqrt = S.cwiseSqrt(); + Eigen::VectorXd S_inv_sqrt = S_inv.cwiseSqrt(); + + VectorXd e0 = S_inv_sqrt.asDiagonal() * saes2.eigenvectors().transpose() * b; + MatrixXd J_ = S_sqrt.asDiagonal() * saes2.eigenvectors().transpose(); + // Use pre-conditioned from OKVINS + // https://github.com/ethz-asl/okvis/blob/master/okvis_ceres/src/MarginalizationError.cpp + // VectorXd p = (A.diagonal().array() > + // eps).select(A.diagonal().cwiseSqrt(),1.0e-3); VectorXd p_inv = + // p.cwiseInverse(); SelfAdjointEigenSolver + // saes(p_inv.asDiagonal() * A * p_inv.asDiagonal() ); VectorXd S_ = + // (saes.eigenvalues().array() > eps).select( + // saes.eigenvalues().array(), 0); + // VectorXd S_pinv_ = (saes.eigenvalues().array() > eps).select( + // saes.eigenvalues().array().inverse(), 0); + // VectorXd S_sqrt_ = S_.cwiseSqrt(); + // VectorXd S_pinv_sqrt_ = S_pinv_.cwiseSqrt(); + + // // assign Jacobian + // MatrixXd J_ = (p.asDiagonal() * saes.eigenvectors() * + // (S_sqrt_.asDiagonal())).transpose(); + + // // constant error (residual) _e0 := (-pinv(J^T) * _b): + // Eigen::MatrixXd J_pinv_T = (S_pinv_sqrt_.asDiagonal()) + // * saes.eigenvectors().transpose() *p_inv.asDiagonal() ; + // VectorXd e0 = (-J_pinv_T * b); + if (params->debug_write_margin_matrix) { + Utility::writeMatrixtoFile("/home/xuhao/output/A.txt", A); + Utility::writeMatrixtoFile("/home/xuhao/output/b.txt", b); + Utility::writeMatrixtoFile("/home/xuhao/output/J.txt", J_); + Utility::writeMatrixtoFile("/home/xuhao/output/e0.txt", e0); + } + + return std::make_pair(J_, e0); } -std::pair toJacRes(const SparseMat & A, const VectorXd & b) { - return toJacRes(A.toDense(), b); +std::pair toJacRes(const SparseMat &A, const VectorXd &b) { + return toJacRes(A.toDense(), b); } -void PriorFactor::replacetoPrevLinearizedPoints(std::vector & params) { - std::vector new_params; - int count = 0; - for (ParamInfo & info : params) { - if (keep_params_map.count(info.pointer) > 0) { - //Copy the linearized point - info.data_copied = keep_params_map.at(info.pointer).data_copied; - count += 1; - // printf("Id %d type %d, cur_state:\n", info.id, info.type); - // std::cout << Map(info.pointer, info.size).transpose() << std::endl; - // std::cout << "linearized point:\n" << info.data_copied.transpose() << std::endl; - } +void PriorFactor::replacetoPrevLinearizedPoints( + std::vector ¶ms) { + std::vector new_params; + int count = 0; + for (ParamInfo &info : params) { + if (keep_params_map.count(info.pointer) > 0) { + // Copy the linearized point + info.data_copied = keep_params_map.at(info.pointer).data_copied; + count += 1; + // printf("Id %d type %d, cur_state:\n", info.id, info.type); + // std::cout << Map(info.pointer, info.size).transpose() << + // std::endl; std::cout << "linearized point:\n" << + // info.data_copied.transpose() << std::endl; } - // printf("Marginalization FEJ state num %d\n", count); + } + // printf("Marginalization FEJ state num %d\n", count); } - -} \ No newline at end of file +} // namespace D2VINS \ No newline at end of file diff --git a/d2vins/src/factors/projectionOneFrameTwoCamFactor.cpp b/d2vins/src/factors/projectionOneFrameTwoCamFactor.cpp index fd45fcff..369cb5c1 100644 --- a/d2vins/src/factors/projectionOneFrameTwoCamFactor.cpp +++ b/d2vins/src/factors/projectionOneFrameTwoCamFactor.cpp @@ -1,8 +1,9 @@ /******************************************************* - * Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology - * + * Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science + *and Technology + * * This file is part of VINS. - * + * * Licensed under the GNU General Public License v3.0; * you may not use this file except in compliance with the License. * @@ -10,7 +11,9 @@ *******************************************************/ #include "projectionOneFrameTwoCamFactor.h" + #include + #include "../d2vins_params.hpp" using namespace D2Common; @@ -18,174 +21,237 @@ namespace D2VINS { Eigen::Matrix2d ProjectionOneFrameTwoCamFactor::sqrt_info; double ProjectionOneFrameTwoCamFactor::sum_t; -ProjectionOneFrameTwoCamFactor::ProjectionOneFrameTwoCamFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j, - const Eigen::Vector3d &_velocity_i, const Eigen::Vector3d &_velocity_j, - const double _td_i, const double _td_j) : - pts_i(_pts_i), pts_j(_pts_j), - td_i(_td_i), td_j(_td_j), - velocity_i(_velocity_i), velocity_j(_velocity_j) -{ +ProjectionOneFrameTwoCamFactor::ProjectionOneFrameTwoCamFactor( + const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j, + const Eigen::Vector3d &_velocity_i, const Eigen::Vector3d &_velocity_j, + const double _td_i, const double _td_j) + : pts_i(_pts_i), + pts_j(_pts_j), + td_i(_td_i), + td_j(_td_j), + velocity_i(_velocity_i), + velocity_j(_velocity_j) { #ifdef UNIT_SPHERE_ERROR - Eigen::Vector3d b1, b2; - Eigen::Vector3d a = pts_j.normalized(); - Eigen::Vector3d tmp(0, 0, 1); - if(a == tmp) - tmp << 1, 0, 0; - b1 = (tmp - a * (a.transpose() * tmp)).normalized(); - b2 = a.cross(b1); - tangent_base.block<1, 3>(0, 0) = b1.transpose(); - tangent_base.block<1, 3>(1, 0) = b2.transpose(); + Eigen::Vector3d b1, b2; + Eigen::Vector3d a = pts_j.normalized(); + Eigen::Vector3d tmp(0, 0, 1); + if (a == tmp) tmp << 1, 0, 0; + b1 = (tmp - a * (a.transpose() * tmp)).normalized(); + b2 = a.cross(b1); + tangent_base.block<1, 3>(0, 0) = b1.transpose(); + tangent_base.block<1, 3>(1, 0) = b2.transpose(); #endif }; -bool ProjectionOneFrameTwoCamFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const -{ - // - Eigen::Vector3d tic(parameters[0][0], parameters[0][1], parameters[0][2]); - Eigen::Quaterniond qic(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]); +bool ProjectionOneFrameTwoCamFactor::Evaluate(double const *const *parameters, + double *residuals, + double **jacobians) const { + // + Eigen::Vector3d tic(parameters[0][0], parameters[0][1], parameters[0][2]); + Eigen::Quaterniond qic(parameters[0][6], parameters[0][3], parameters[0][4], + parameters[0][5]); - Eigen::Vector3d tic2(parameters[1][0], parameters[1][1], parameters[1][2]); - Eigen::Quaterniond qic2(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]); + Eigen::Vector3d tic2(parameters[1][0], parameters[1][1], parameters[1][2]); + Eigen::Quaterniond qic2(parameters[1][6], parameters[1][3], parameters[1][4], + parameters[1][5]); - double inv_dep_i = parameters[2][0]; + double inv_dep_i = parameters[2][0]; - double td = parameters[3][0]; + double td = parameters[3][0]; - Eigen::Vector3d pts_i_td, pts_j_td; - pts_i_td = pts_i - (td - td_i) * velocity_i; - pts_j_td = pts_j - (td - td_j) * velocity_j; + Eigen::Vector3d pts_i_td, pts_j_td; + pts_i_td = pts_i - (td - td_i) * velocity_i; + pts_j_td = pts_j - (td - td_j) * velocity_j; - Eigen::Vector3d pts_camera_i = pts_i_td / inv_dep_i; - Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic; - Eigen::Vector3d pts_imu_j = pts_imu_i; - Eigen::Vector3d pts_camera_j = qic2.inverse() * (pts_imu_j - tic2); - Eigen::Map residual(residuals); + Eigen::Vector3d pts_camera_i = pts_i_td / inv_dep_i; + Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic; + Eigen::Vector3d pts_imu_j = pts_imu_i; + Eigen::Vector3d pts_camera_j = qic2.inverse() * (pts_imu_j - tic2); + Eigen::Map residual(residuals); - // std::cout << "Source ERROR" << (pts_camera_j.normalized() - pts_j_td.normalized()).transpose() << std::endl; + // std::cout << "Source ERROR" << (pts_camera_j.normalized() - + // pts_j_td.normalized()).transpose() << std::endl; -#ifdef UNIT_SPHERE_ERROR - residual = tangent_base * (pts_camera_j.normalized() - pts_j_td.normalized()); +#ifdef UNIT_SPHERE_ERROR + residual = tangent_base * (pts_camera_j.normalized() - pts_j_td.normalized()); #else - double dep_j = pts_camera_j.z(); - residual = (pts_camera_j / dep_j).head<2>() - pts_j_td.head<2>(); + double dep_j = pts_camera_j.z(); + residual = (pts_camera_j / dep_j).head<2>() - pts_j_td.head<2>(); #endif - residual = sqrt_info * residual; + residual = sqrt_info * residual; - // std::cout << "TAN" << tangent_base.transpose() << std::endl; - // std::cout << "Res" << residual << std::endl; + // std::cout << "TAN" << tangent_base.transpose() << std::endl; + // std::cout << "Res" << residual << std::endl; - if (jacobians) - { - Eigen::Matrix3d ric = qic.toRotationMatrix(); - Eigen::Matrix3d ric2 = qic2.toRotationMatrix(); - Eigen::Matrix reduce(2, 3); - const auto ric2_t = ric2.transpose(); - const auto ric2_t_ric = ric2_t * ric; + if (jacobians) { + Eigen::Matrix3d ric = qic.toRotationMatrix(); + Eigen::Matrix3d ric2 = qic2.toRotationMatrix(); + Eigen::Matrix reduce(2, 3); + const auto ric2_t = ric2.transpose(); + const auto ric2_t_ric = ric2_t * ric; #ifdef UNIT_SPHERE_ERROR - double norm = pts_camera_j.norm(); - Eigen::Matrix3d norm_jaco; - double x1, x2, x3; - x1 = pts_camera_j(0); - x2 = pts_camera_j(1); - x3 = pts_camera_j(2); - double norm_3 = pow(norm, 3); - norm_jaco << 1.0 / norm - x1 * x1 / norm_3, - x1 * x2 / norm_3, - x1 * x3 / norm_3, - - x1 * x2 / norm_3, 1.0 / norm - x2 * x2 / norm_3, - x2 * x3 / norm_3, - - x1 * x3 / norm_3, - x2 * x3 / norm_3, 1.0 / norm - x3 * x3 / norm_3; - reduce = tangent_base * norm_jaco; - Matrix3d reduce_j_td; - x1 = pts_j_td(0); - x2 = pts_j_td(1); - x3 = pts_j_td(2); - norm_3 = pow(pts_j_td.norm(), 3); - reduce_j_td << 1.0 / norm - x1 * x1 / norm_3, - x1 * x2 / norm_3, - x1 * x3 / norm_3, - - x1 * x2 / norm_3, 1.0 / norm - x2 * x2 / norm_3, - x2 * x3 / norm_3, - - x1 * x3 / norm_3, - x2 * x3 / norm_3, 1.0 / norm - x3 * x3 / norm_3; + double norm = pts_camera_j.norm(); + Eigen::Matrix3d norm_jaco; + double x1, x2, x3; + x1 = pts_camera_j(0); + x2 = pts_camera_j(1); + x3 = pts_camera_j(2); + double norm_3 = pow(norm, 3); + norm_jaco << 1.0 / norm - x1 * x1 / norm_3, -x1 * x2 / norm_3, + -x1 * x3 / norm_3, -x1 * x2 / norm_3, 1.0 / norm - x2 * x2 / norm_3, + -x2 * x3 / norm_3, -x1 * x3 / norm_3, -x2 * x3 / norm_3, + 1.0 / norm - x3 * x3 / norm_3; + reduce = tangent_base * norm_jaco; + Matrix3d reduce_j_td; + x1 = pts_j_td(0); + x2 = pts_j_td(1); + x3 = pts_j_td(2); + norm_3 = pow(pts_j_td.norm(), 3); + reduce_j_td << 1.0 / norm - x1 * x1 / norm_3, -x1 * x2 / norm_3, + -x1 * x3 / norm_3, -x1 * x2 / norm_3, 1.0 / norm - x2 * x2 / norm_3, + -x2 * x3 / norm_3, -x1 * x3 / norm_3, -x2 * x3 / norm_3, + 1.0 / norm - x3 * x3 / norm_3; #else - reduce << 1. / dep_j, 0, -pts_camera_j(0) / (dep_j * dep_j), - 0, 1. / dep_j, -pts_camera_j(1) / (dep_j * dep_j); + reduce << 1. / dep_j, 0, -pts_camera_j(0) / (dep_j * dep_j), 0, 1. / dep_j, + -pts_camera_j(1) / (dep_j * dep_j); #endif - reduce = sqrt_info * reduce; - - if (jacobians[0]) - { - Eigen::Map> jacobian_ex_pose(jacobians[0]); - Eigen::Matrix jaco_ex; - jaco_ex.leftCols<3>() = ric2_t; - jaco_ex.rightCols<3>() = ric2_t_ric * -Utility::skewSymmetric(pts_camera_i); - jacobian_ex_pose.leftCols<6>() = reduce * jaco_ex; - jacobian_ex_pose.rightCols<1>().setZero(); - } - if (jacobians[1]) - { - Eigen::Map> jacobian_ex_pose1(jacobians[1]); - Eigen::Matrix jaco_ex; - jaco_ex.leftCols<3>() = - ric2.transpose(); - jaco_ex.rightCols<3>() = Utility::skewSymmetric(pts_camera_j); - jacobian_ex_pose1.leftCols<6>() = reduce * jaco_ex; - jacobian_ex_pose1.rightCols<1>().setZero(); - } - if (jacobians[2]) - { - Eigen::Map jacobian_feature(jacobians[2]); - jacobian_feature = reduce * ric2_t_ric * pts_i_td * -1.0 / (inv_dep_i * inv_dep_i); - } - if (jacobians[3]) - { + reduce = sqrt_info * reduce; + + if (jacobians[0]) { + Eigen::Map> jacobian_ex_pose( + jacobians[0]); + Eigen::Matrix jaco_ex; + jaco_ex.leftCols<3>() = ric2_t; + jaco_ex.rightCols<3>() = + ric2_t_ric * -Utility::skewSymmetric(pts_camera_i); + jacobian_ex_pose.leftCols<6>() = reduce * jaco_ex; + jacobian_ex_pose.rightCols<1>().setZero(); + } + if (jacobians[1]) { + Eigen::Map> + jacobian_ex_pose1(jacobians[1]); + Eigen::Matrix jaco_ex; + jaco_ex.leftCols<3>() = -ric2.transpose(); + jaco_ex.rightCols<3>() = Utility::skewSymmetric(pts_camera_j); + jacobian_ex_pose1.leftCols<6>() = reduce * jaco_ex; + jacobian_ex_pose1.rightCols<1>().setZero(); + } + if (jacobians[2]) { + Eigen::Map jacobian_feature(jacobians[2]); + jacobian_feature = + reduce * ric2_t_ric * pts_i_td * -1.0 / (inv_dep_i * inv_dep_i); + } + if (jacobians[3]) { #ifdef UNIT_SPHERE_ERROR - Eigen::Map jacobian_td(jacobians[3]); - jacobian_td = reduce * (ric2_t_ric * velocity_i / inv_dep_i * -1.0) + - sqrt_info * tangent_base * reduce_j_td * velocity_j; + Eigen::Map jacobian_td(jacobians[3]); + jacobian_td = reduce * (ric2_t_ric * velocity_i / inv_dep_i * -1.0) + + sqrt_info * tangent_base * reduce_j_td * velocity_j; #else - Eigen::Map jacobian_td(jacobians[3]); - jacobian_td = reduce * ric2_t_ric * velocity_i / inv_dep_i * -1.0 + - sqrt_info * velocity_j.head(2); + Eigen::Map jacobian_td(jacobians[3]); + jacobian_td = reduce * ric2_t_ric * velocity_i / inv_dep_i * -1.0 + + sqrt_info * velocity_j.head(2); #endif - } } - return true; + } + return true; } -void ProjectionOneFrameTwoCamFactor::check(double **parameters) -{ - double *res = new double[15]; - double **jaco = new double *[4]; - jaco[0] = new double[2 * 7]; - jaco[1] = new double[2 * 7]; - jaco[2] = new double[2 * 1]; - jaco[3] = new double[2 * 1]; - Evaluate(parameters, res, jaco); - puts("check begins"); - - puts("my"); - - std::cout << Eigen::Map>(res).transpose() << std::endl - << std::endl; - std::cout << "Jacobian" << std::endl; - std::cout << Eigen::Map>(jaco[0]) << std::endl - << std::endl; - std::cout << Eigen::Map>(jaco[1]) << std::endl - << std::endl; - std::cout << "Jacobian2" << std::endl; - std::cout << Eigen::Map(jaco[2]) << std::endl - << std::endl; - std::cout << "Jacobian3" << std::endl; - std::cout << Eigen::Map(jaco[3]) << std::endl - << std::endl; +void ProjectionOneFrameTwoCamFactor::check(double **parameters) { + double *res = new double[15]; + double **jaco = new double *[4]; + jaco[0] = new double[2 * 7]; + jaco[1] = new double[2 * 7]; + jaco[2] = new double[2 * 1]; + jaco[3] = new double[2 * 1]; + Evaluate(parameters, res, jaco); + puts("check begins"); + + puts("my"); + + std::cout << Eigen::Map>(res).transpose() + << std::endl + << std::endl; + std::cout << "Jacobian" << std::endl; + std::cout << Eigen::Map>(jaco[0]) + << std::endl + << std::endl; + std::cout << Eigen::Map>(jaco[1]) + << std::endl + << std::endl; + std::cout << "Jacobian2" << std::endl; + std::cout << Eigen::Map(jaco[2]) << std::endl << std::endl; + std::cout << "Jacobian3" << std::endl; + std::cout << Eigen::Map(jaco[3]) << std::endl << std::endl; + + Eigen::Vector3d tic(parameters[0][0], parameters[0][1], parameters[0][2]); + Eigen::Quaterniond qic(parameters[0][6], parameters[0][3], parameters[0][4], + parameters[0][5]); + + Eigen::Vector3d tic2(parameters[1][0], parameters[1][1], parameters[1][2]); + Eigen::Quaterniond qic2(parameters[1][6], parameters[1][3], parameters[1][4], + parameters[1][5]); + + double inv_dep_i = parameters[2][0]; + + double td = parameters[3][0]; + + Eigen::Vector3d pts_i_td, pts_j_td; + pts_i_td = pts_i - (td - td_i) * velocity_i; + pts_j_td = pts_j - (td - td_j) * velocity_j; + + Eigen::Vector3d pts_camera_i = pts_i_td / inv_dep_i; + Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic; + Eigen::Vector3d pts_imu_j = pts_imu_i; + Eigen::Vector3d pts_camera_j = qic2.inverse() * (pts_imu_j - tic2); + + Eigen::Vector2d residual; +#ifdef UNIT_SPHERE_ERROR + residual = tangent_base * (pts_camera_j.normalized() - pts_j_td.normalized()); +#else + double dep_j = pts_camera_j.z(); + residual = (pts_camera_j / dep_j).head<2>() - pts_j_td.head<2>(); +#endif + residual = sqrt_info * residual; + + puts("num"); + std::cout << residual.transpose() << std::endl; + const double eps = 1e-6; + Eigen::Matrix num_jacobian; + for (int k = 0; k < 14; k++) { Eigen::Vector3d tic(parameters[0][0], parameters[0][1], parameters[0][2]); - Eigen::Quaterniond qic(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]); + Eigen::Quaterniond qic(parameters[0][6], parameters[0][3], parameters[0][4], + parameters[0][5]); Eigen::Vector3d tic2(parameters[1][0], parameters[1][1], parameters[1][2]); - Eigen::Quaterniond qic2(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]); + Eigen::Quaterniond qic2(parameters[1][6], parameters[1][3], + parameters[1][4], parameters[1][5]); double inv_dep_i = parameters[2][0]; double td = parameters[3][0]; + int a = k / 3, b = k % 3; + Eigen::Vector3d delta = Eigen::Vector3d(b == 0, b == 1, b == 2) * eps; + + if (a == 0) + tic += delta; + else if (a == 1) + qic = qic * Utility::deltaQ(delta); + else if (a == 2) + tic2 += delta; + else if (a == 3) + qic2 = qic2 * Utility::deltaQ(delta); + else if (a == 4) { + if (b == 0) + inv_dep_i += delta.x(); + else + td += delta.y(); + } + Eigen::Vector3d pts_i_td, pts_j_td; pts_i_td = pts_i - (td - td_i) * velocity_i; pts_j_td = pts_j - (td - td_j) * velocity_j; @@ -195,77 +261,23 @@ void ProjectionOneFrameTwoCamFactor::check(double **parameters) Eigen::Vector3d pts_imu_j = pts_imu_i; Eigen::Vector3d pts_camera_j = qic2.inverse() * (pts_imu_j - tic2); - - Eigen::Vector2d residual; -#ifdef UNIT_SPHERE_ERROR - residual = tangent_base * (pts_camera_j.normalized() - pts_j_td.normalized()); + Eigen::Vector2d tmp_residual; +#ifdef UNIT_SPHERE_ERROR + tmp_residual = + tangent_base * (pts_camera_j.normalized() - pts_j_td.normalized()); #else double dep_j = pts_camera_j.z(); - residual = (pts_camera_j / dep_j).head<2>() - pts_j_td.head<2>(); + tmp_residual = (pts_camera_j / dep_j).head<2>() - pts_j_td.head<2>(); #endif - residual = sqrt_info * residual; - - puts("num"); - std::cout << residual.transpose() << std::endl; - - const double eps = 1e-6; - Eigen::Matrix num_jacobian; - for (int k = 0; k < 14; k++) - { - Eigen::Vector3d tic(parameters[0][0], parameters[0][1], parameters[0][2]); - Eigen::Quaterniond qic(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]); - - Eigen::Vector3d tic2(parameters[1][0], parameters[1][1], parameters[1][2]); - Eigen::Quaterniond qic2(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]); - - double inv_dep_i = parameters[2][0]; - - double td = parameters[3][0]; - - int a = k / 3, b = k % 3; - Eigen::Vector3d delta = Eigen::Vector3d(b == 0, b == 1, b == 2) * eps; - - if (a == 0) - tic += delta; - else if (a == 1) - qic = qic * Utility::deltaQ(delta); - else if (a == 2) - tic2 += delta; - else if (a == 3) - qic2 = qic2 * Utility::deltaQ(delta); - else if (a == 4) - { - if (b == 0) - inv_dep_i += delta.x(); - else - td += delta.y(); - } - - Eigen::Vector3d pts_i_td, pts_j_td; - pts_i_td = pts_i - (td - td_i) * velocity_i; - pts_j_td = pts_j - (td - td_j) * velocity_j; - - Eigen::Vector3d pts_camera_i = pts_i_td / inv_dep_i; - Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic; - Eigen::Vector3d pts_imu_j = pts_imu_i; - Eigen::Vector3d pts_camera_j = qic2.inverse() * (pts_imu_j - tic2); - - Eigen::Vector2d tmp_residual; -#ifdef UNIT_SPHERE_ERROR - tmp_residual = tangent_base * (pts_camera_j.normalized() - pts_j_td.normalized()); -#else - double dep_j = pts_camera_j.z(); - tmp_residual = (pts_camera_j / dep_j).head<2>() - pts_j_td.head<2>(); -#endif - tmp_residual = sqrt_info * tmp_residual; - num_jacobian.col(k) = (tmp_residual - residual) / eps; - } - std::cout << num_jacobian.block<2, 6>(0, 0) << std::endl; - std::cout << num_jacobian.block<2, 6>(0, 6) << std::endl; - std::cout << "012" << std::endl; - std::cout << num_jacobian.block<2, 1>(0, 12) << std::endl; - std::cout << "013" << std::endl; - std::cout << num_jacobian.block<2, 1>(0, 13) << std::endl; - std::cout << "Check End" << std::endl; + tmp_residual = sqrt_info * tmp_residual; + num_jacobian.col(k) = (tmp_residual - residual) / eps; + } + std::cout << num_jacobian.block<2, 6>(0, 0) << std::endl; + std::cout << num_jacobian.block<2, 6>(0, 6) << std::endl; + std::cout << "012" << std::endl; + std::cout << num_jacobian.block<2, 1>(0, 12) << std::endl; + std::cout << "013" << std::endl; + std::cout << num_jacobian.block<2, 1>(0, 13) << std::endl; + std::cout << "Check End" << std::endl; } -} \ No newline at end of file +} // namespace D2VINS \ No newline at end of file diff --git a/d2vins/src/factors/projectionTwoFrameOneCamDepthFactor.cpp b/d2vins/src/factors/projectionTwoFrameOneCamDepthFactor.cpp index 8786857f..83bd7560 100644 --- a/d2vins/src/factors/projectionTwoFrameOneCamDepthFactor.cpp +++ b/d2vins/src/factors/projectionTwoFrameOneCamDepthFactor.cpp @@ -1,8 +1,9 @@ /******************************************************* - * Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology - * + * Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science + *and Technology + * * This file is part of VINS. - * + * * Licensed under the GNU General Public License v3.0; * you may not use this file except in compliance with the License. * @@ -10,7 +11,9 @@ *******************************************************/ #include "projectionTwoFrameOneCamDepthFactor.h" + #include + #include "../d2vins_params.hpp" using namespace D2Common; @@ -18,155 +21,167 @@ namespace D2VINS { Eigen::Matrix3d ProjectionTwoFrameOneCamDepthFactor::sqrt_info; double ProjectionTwoFrameOneCamDepthFactor::sum_t; -ProjectionTwoFrameOneCamDepthFactor::ProjectionTwoFrameOneCamDepthFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j, - const Eigen::Vector3d &_velocity_i, const Eigen::Vector3d &_velocity_j, - const double _td_i, const double _td_j, const double _depth_j) : - pts_i(_pts_i), pts_j(_pts_j), - td_i(_td_i), td_j(_td_j), inv_depth_j(1/_depth_j), - velocity_i(_velocity_i), velocity_j(_velocity_j) -{ +ProjectionTwoFrameOneCamDepthFactor::ProjectionTwoFrameOneCamDepthFactor( + const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j, + const Eigen::Vector3d &_velocity_i, const Eigen::Vector3d &_velocity_j, + const double _td_i, const double _td_j, const double _depth_j) + : pts_i(_pts_i), + pts_j(_pts_j), + td_i(_td_i), + td_j(_td_j), + inv_depth_j(1 / _depth_j), + velocity_i(_velocity_i), + velocity_j(_velocity_j) { #ifdef UNIT_SPHERE_ERROR - Eigen::Vector3d b1, b2; - Eigen::Vector3d a = pts_j.normalized(); - Eigen::Vector3d tmp(0, 0, 1); - if(a == tmp) - tmp << 1, 0, 0; - b1 = (tmp - a * (a.transpose() * tmp)).normalized(); - b2 = a.cross(b1); - tangent_base.block<1, 3>(0, 0) = b1.transpose(); - tangent_base.block<1, 3>(1, 0) = b2.transpose(); + Eigen::Vector3d b1, b2; + Eigen::Vector3d a = pts_j.normalized(); + Eigen::Vector3d tmp(0, 0, 1); + if (a == tmp) tmp << 1, 0, 0; + b1 = (tmp - a * (a.transpose() * tmp)).normalized(); + b2 = a.cross(b1); + tangent_base.block<1, 3>(0, 0) = b1.transpose(); + tangent_base.block<1, 3>(1, 0) = b2.transpose(); #endif }; -bool ProjectionTwoFrameOneCamDepthFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const -{ - Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]); - Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]); +bool ProjectionTwoFrameOneCamDepthFactor::Evaluate( + double const *const *parameters, double *residuals, + double **jacobians) const { + Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]); + Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], + parameters[0][5]); - Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]); - Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]); + Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]); + Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4], + parameters[1][5]); - Eigen::Vector3d tic(parameters[2][0], parameters[2][1], parameters[2][2]); - Eigen::Quaterniond qic(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]); + Eigen::Vector3d tic(parameters[2][0], parameters[2][1], parameters[2][2]); + Eigen::Quaterniond qic(parameters[2][6], parameters[2][3], parameters[2][4], + parameters[2][5]); - double inv_dep_i = parameters[3][0]; + double inv_dep_i = parameters[3][0]; - double td = parameters[4][0]; + double td = parameters[4][0]; - Eigen::Vector3d pts_i_td, pts_j_td; - pts_i_td = pts_i - (td - td_i) * velocity_i; - pts_j_td = pts_j - (td - td_j) * velocity_j; - Eigen::Vector3d pts_camera_i = pts_i_td / inv_dep_i; - Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic; - Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi; - Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj); - Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic); - Eigen::Map residual(residuals); + Eigen::Vector3d pts_i_td, pts_j_td; + pts_i_td = pts_i - (td - td_i) * velocity_i; + pts_j_td = pts_j - (td - td_j) * velocity_j; + Eigen::Vector3d pts_camera_i = pts_i_td / inv_dep_i; + Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic; + Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi; + Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj); + Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic); + Eigen::Map residual(residuals); - double est_inv_dep_j = 1/pts_camera_j.norm(); -#ifdef UNIT_SPHERE_ERROR - residual.head<2>() = tangent_base * (pts_camera_j.normalized() - pts_j_td.normalized()); + double est_inv_dep_j = 1 / pts_camera_j.norm(); +#ifdef UNIT_SPHERE_ERROR + residual.head<2>() = + tangent_base * (pts_camera_j.normalized() - pts_j_td.normalized()); #else - residual.head<2>() = (pts_camera_j * est_inv_dep_j).head<2>() - pts_j_td.head<2>(); + residual.head<2>() = + (pts_camera_j * est_inv_dep_j).head<2>() - pts_j_td.head<2>(); #endif - residual.z() = est_inv_dep_j - inv_depth_j; - residual = sqrt_info * residual; - - if (jacobians) - { - Eigen::Matrix3d Ri = Qi.toRotationMatrix(); - Eigen::Matrix3d Rj = Qj.toRotationMatrix(); - Eigen::Matrix3d ric = qic.toRotationMatrix(); - Eigen::Matrix3d reduce = Eigen::Matrix3d::Identity(); - const auto ric_t = ric.transpose(); - const auto Rj_t = Rj.transpose(); - const auto J_w = ric_t * Rj_t; - const auto J_imu_i = J_w * Ri; - const auto J_cam_i = J_imu_i * ric; - + residual.z() = est_inv_dep_j - inv_depth_j; + residual = sqrt_info * residual; + + if (jacobians) { + Eigen::Matrix3d Ri = Qi.toRotationMatrix(); + Eigen::Matrix3d Rj = Qj.toRotationMatrix(); + Eigen::Matrix3d ric = qic.toRotationMatrix(); + Eigen::Matrix3d reduce = Eigen::Matrix3d::Identity(); + const auto ric_t = ric.transpose(); + const auto Rj_t = Rj.transpose(); + const auto J_w = ric_t * Rj_t; + const auto J_imu_i = J_w * Ri; + const auto J_cam_i = J_imu_i * ric; + #ifdef UNIT_SPHERE_ERROR - double norm = pts_camera_j.norm(); - Eigen::Matrix3d norm_jaco; - double x1, x2, x3; - x1 = pts_camera_j(0); - x2 = pts_camera_j(1); - x3 = pts_camera_j(2); - double norm_3 = pow(norm, 3); - norm_jaco << 1.0 / norm - x1 * x1 / norm_3, - x1 * x2 / norm_3, - x1 * x3 / norm_3, - - x1 * x2 / norm_3, 1.0 / norm - x2 * x2 / norm_3, - x2 * x3 / norm_3, - - x1 * x3 / norm_3, - x2 * x3 / norm_3, 1.0 / norm - x3 * x3 / norm_3; - reduce.topRows(2) = tangent_base * norm_jaco; - reduce.bottomRows(1) = Vector3d(-x1/norm_3, -x2/norm_3, -x3/norm_3).transpose(); - - Matrix3d reduce_j_td; - x1 = pts_j_td(0); - x2 = pts_j_td(1); - x3 = pts_j_td(2); - norm_3 = pow(pts_j_td.norm(), 3); - reduce_j_td << 1.0 / norm - x1 * x1 / norm_3, - x1 * x2 / norm_3, - x1 * x3 / norm_3, - - x1 * x2 / norm_3, 1.0 / norm - x2 * x2 / norm_3, - x2 * x3 / norm_3, - - x1 * x3 / norm_3, - x2 * x3 / norm_3, 1.0 / norm - x3 * x3 / norm_3; + double norm = pts_camera_j.norm(); + Eigen::Matrix3d norm_jaco; + double x1, x2, x3; + x1 = pts_camera_j(0); + x2 = pts_camera_j(1); + x3 = pts_camera_j(2); + double norm_3 = pow(norm, 3); + norm_jaco << 1.0 / norm - x1 * x1 / norm_3, -x1 * x2 / norm_3, + -x1 * x3 / norm_3, -x1 * x2 / norm_3, 1.0 / norm - x2 * x2 / norm_3, + -x2 * x3 / norm_3, -x1 * x3 / norm_3, -x2 * x3 / norm_3, + 1.0 / norm - x3 * x3 / norm_3; + reduce.topRows(2) = tangent_base * norm_jaco; + reduce.bottomRows(1) = + Vector3d(-x1 / norm_3, -x2 / norm_3, -x3 / norm_3).transpose(); + + Matrix3d reduce_j_td; + x1 = pts_j_td(0); + x2 = pts_j_td(1); + x3 = pts_j_td(2); + norm_3 = pow(pts_j_td.norm(), 3); + reduce_j_td << 1.0 / norm - x1 * x1 / norm_3, -x1 * x2 / norm_3, + -x1 * x3 / norm_3, -x1 * x2 / norm_3, 1.0 / norm - x2 * x2 / norm_3, + -x2 * x3 / norm_3, -x1 * x3 / norm_3, -x2 * x3 / norm_3, + 1.0 / norm - x3 * x3 / norm_3; #else - auto est_inv_dep_j_sqr = est_inv_dep_j * est_inv_dep_j; - reduce << 1. * est_inv_dep_j, 0, -pts_camera_j(0) * est_inv_dep_j_sqr, - 0, 1. * est_inv_dep_j, -pts_camera_j(1) * est_inv_dep_j_sqr, - 0, 0, -est_inv_dep_j_sqr; + auto est_inv_dep_j_sqr = est_inv_dep_j * est_inv_dep_j; + reduce << 1. * est_inv_dep_j, 0, -pts_camera_j(0) * est_inv_dep_j_sqr, 0, + 1. * est_inv_dep_j, -pts_camera_j(1) * est_inv_dep_j_sqr, 0, 0, + -est_inv_dep_j_sqr; #endif - reduce = sqrt_info * reduce; - - if (jacobians[0]) - { - Eigen::Map> jacobian_pose_i(jacobians[0]); - - Eigen::Matrix jaco_i; - jaco_i.leftCols<3>() = J_w; - jaco_i.rightCols<3>() = J_imu_i * -Utility::skewSymmetric(pts_imu_i); - - jacobian_pose_i.leftCols<6>() = reduce * jaco_i; - jacobian_pose_i.rightCols<1>().setZero(); - } - - if (jacobians[1]) - { - Eigen::Map> jacobian_pose_j(jacobians[1]); - - Eigen::Matrix jaco_j; - jaco_j.leftCols<3>() = -J_w; - jaco_j.rightCols<3>() = ric_t * Utility::skewSymmetric(pts_imu_j); - - jacobian_pose_j.leftCols<6>() = reduce * jaco_j; - jacobian_pose_j.rightCols<1>().setZero(); - } - if (jacobians[2]) - { - Eigen::Map> jacobian_ex_pose(jacobians[2]); - Eigen::Matrix jaco_ex; - jaco_ex.leftCols<3>() = J_imu_i - ric_t; - jaco_ex.rightCols<3>() = -J_cam_i * Utility::skewSymmetric(pts_camera_i) + Utility::skewSymmetric(J_cam_i * pts_camera_i) + - Utility::skewSymmetric(J_w * (Ri * tic + Pi - Pj) - Rj_t*tic); - jacobian_ex_pose.leftCols<6>() = reduce * jaco_ex; - jacobian_ex_pose.rightCols<1>().setZero(); - } - if (jacobians[3]) - { - Eigen::Map jacobian_feature(jacobians[3]); - jacobian_feature = reduce * J_cam_i * pts_i_td * -1.0 / (inv_dep_i * inv_dep_i); - } - if (jacobians[4]) - { - Eigen::Map jacobian_td(jacobians[4]); + reduce = sqrt_info * reduce; + + if (jacobians[0]) { + Eigen::Map> jacobian_pose_i( + jacobians[0]); + + Eigen::Matrix jaco_i; + jaco_i.leftCols<3>() = J_w; + jaco_i.rightCols<3>() = J_imu_i * -Utility::skewSymmetric(pts_imu_i); + + jacobian_pose_i.leftCols<6>() = reduce * jaco_i; + jacobian_pose_i.rightCols<1>().setZero(); + } + + if (jacobians[1]) { + Eigen::Map> jacobian_pose_j( + jacobians[1]); + + Eigen::Matrix jaco_j; + jaco_j.leftCols<3>() = -J_w; + jaco_j.rightCols<3>() = ric_t * Utility::skewSymmetric(pts_imu_j); + + jacobian_pose_j.leftCols<6>() = reduce * jaco_j; + jacobian_pose_j.rightCols<1>().setZero(); + } + if (jacobians[2]) { + Eigen::Map> jacobian_ex_pose( + jacobians[2]); + Eigen::Matrix jaco_ex; + jaco_ex.leftCols<3>() = J_imu_i - ric_t; + jaco_ex.rightCols<3>() = + -J_cam_i * Utility::skewSymmetric(pts_camera_i) + + Utility::skewSymmetric(J_cam_i * pts_camera_i) + + Utility::skewSymmetric(J_w * (Ri * tic + Pi - Pj) - Rj_t * tic); + jacobian_ex_pose.leftCols<6>() = reduce * jaco_ex; + jacobian_ex_pose.rightCols<1>().setZero(); + } + if (jacobians[3]) { + Eigen::Map jacobian_feature(jacobians[3]); + jacobian_feature = + reduce * J_cam_i * pts_i_td * -1.0 / (inv_dep_i * inv_dep_i); + } + if (jacobians[4]) { + Eigen::Map jacobian_td(jacobians[4]); #ifdef UNIT_SPHERE_ERROR - Eigen::Vector3d jac_td_j(0., 0., 0.); - jac_td_j.head<2>() = sqrt_info.block<2, 2>(0, 0) * tangent_base * reduce_j_td* velocity_j; - jacobian_td = reduce * J_cam_i * velocity_i / inv_dep_i * -1.0 - + jac_td_j; + Eigen::Vector3d jac_td_j(0., 0., 0.); + jac_td_j.head<2>() = + sqrt_info.block<2, 2>(0, 0) * tangent_base * reduce_j_td * velocity_j; + jacobian_td = reduce * J_cam_i * velocity_i / inv_dep_i * -1.0 + jac_td_j; #else - jacobian_td = reduce * J_cam_i * velocity_i / inv_dep_i * -1.0 + - sqrt_info.leftCols(2) * velocity_j.head(2); + jacobian_td = reduce * J_cam_i * velocity_i / inv_dep_i * -1.0 + + sqrt_info.leftCols(2) * velocity_j.head(2); #endif - } } - return true; -} + } + return true; } +} // namespace D2VINS diff --git a/d2vins/src/factors/projectionTwoFrameOneCamFactor.cpp b/d2vins/src/factors/projectionTwoFrameOneCamFactor.cpp index 54f370d5..c8397d85 100644 --- a/d2vins/src/factors/projectionTwoFrameOneCamFactor.cpp +++ b/d2vins/src/factors/projectionTwoFrameOneCamFactor.cpp @@ -1,8 +1,9 @@ /******************************************************* - * Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology - * + * Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science + *and Technology + * * This file is part of VINS. - * + * * Licensed under the GNU General Public License v3.0; * you may not use this file except in compliance with the License. * @@ -10,7 +11,9 @@ *******************************************************/ #include "projectionTwoFrameOneCamFactor.h" + #include + #include "../d2vins_params.hpp" using namespace D2Common; @@ -18,271 +21,286 @@ namespace D2VINS { Eigen::Matrix2d ProjectionTwoFrameOneCamFactor::sqrt_info; double ProjectionTwoFrameOneCamFactor::sum_t; -ProjectionTwoFrameOneCamFactor::ProjectionTwoFrameOneCamFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j, - const Eigen::Vector3d &_velocity_i, const Eigen::Vector3d &_velocity_j, - const double _td_i, const double _td_j) : - pts_i(_pts_i), pts_j(_pts_j), - td_i(_td_i), td_j(_td_j), - velocity_i(_velocity_i), velocity_j(_velocity_j) -{ +ProjectionTwoFrameOneCamFactor::ProjectionTwoFrameOneCamFactor( + const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j, + const Eigen::Vector3d &_velocity_i, const Eigen::Vector3d &_velocity_j, + const double _td_i, const double _td_j) + : pts_i(_pts_i), + pts_j(_pts_j), + td_i(_td_i), + td_j(_td_j), + velocity_i(_velocity_i), + velocity_j(_velocity_j) { #ifdef UNIT_SPHERE_ERROR - Eigen::Vector3d b1, b2; - Eigen::Vector3d a = pts_j.normalized(); - Eigen::Vector3d tmp(0, 0, 1); - if(a == tmp) - tmp << 1, 0, 0; - b1 = (tmp - a * (a.transpose() * tmp)).normalized(); - b2 = a.cross(b1); - tangent_base.block<1, 3>(0, 0) = b1.transpose(); - tangent_base.block<1, 3>(1, 0) = b2.transpose(); - // printf("vel i %.4f %.4f %.4f j %.4f %.4f %.4f\n", velocity_i(0), velocity_i(1), velocity_i(2), velocity_j(0), velocity_j(1), velocity_j(2)); + Eigen::Vector3d b1, b2; + Eigen::Vector3d a = pts_j.normalized(); + Eigen::Vector3d tmp(0, 0, 1); + if (a == tmp) tmp << 1, 0, 0; + b1 = (tmp - a * (a.transpose() * tmp)).normalized(); + b2 = a.cross(b1); + tangent_base.block<1, 3>(0, 0) = b1.transpose(); + tangent_base.block<1, 3>(1, 0) = b2.transpose(); + // printf("vel i %.4f %.4f %.4f j %.4f %.4f %.4f\n", velocity_i(0), + // velocity_i(1), velocity_i(2), velocity_j(0), velocity_j(1), velocity_j(2)); #endif }; -bool ProjectionTwoFrameOneCamFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const -{ - Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]); - Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]); +bool ProjectionTwoFrameOneCamFactor::Evaluate(double const *const *parameters, + double *residuals, + double **jacobians) const { + Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]); + Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], + parameters[0][5]); - Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]); - Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]); + Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]); + Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4], + parameters[1][5]); - Eigen::Vector3d tic(parameters[2][0], parameters[2][1], parameters[2][2]); - Eigen::Quaterniond qic(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]); + Eigen::Vector3d tic(parameters[2][0], parameters[2][1], parameters[2][2]); + Eigen::Quaterniond qic(parameters[2][6], parameters[2][3], parameters[2][4], + parameters[2][5]); - double inv_dep_i = parameters[3][0]; + double inv_dep_i = parameters[3][0]; - double td = parameters[4][0]; + double td = parameters[4][0]; - Eigen::Vector3d pts_i_td, pts_j_td; - pts_i_td = pts_i - (td - td_i) * velocity_i; - pts_j_td = pts_j - (td - td_j) * velocity_j; - Eigen::Vector3d pts_camera_i = pts_i_td / inv_dep_i; - Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic; - Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi; - Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj); - Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic); - Eigen::Map residual(residuals); + Eigen::Vector3d pts_i_td, pts_j_td; + pts_i_td = pts_i - (td - td_i) * velocity_i; + pts_j_td = pts_j - (td - td_j) * velocity_j; + Eigen::Vector3d pts_camera_i = pts_i_td / inv_dep_i; + Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic; + Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi; + Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj); + Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic); + Eigen::Map residual(residuals); -#ifdef UNIT_SPHERE_ERROR - residual = tangent_base * (pts_camera_j.normalized() - pts_j_td.normalized()); +#ifdef UNIT_SPHERE_ERROR + residual = tangent_base * (pts_camera_j.normalized() - pts_j_td.normalized()); #else - double dep_j = pts_camera_j.z(); - residual = (pts_camera_j / dep_j).head<2>() - pts_j_td.head<2>(); + double dep_j = pts_camera_j.z(); + residual = (pts_camera_j / dep_j).head<2>() - pts_j_td.head<2>(); #endif - residual = sqrt_info * residual; + residual = sqrt_info * residual; - if (jacobians) - { - Eigen::Matrix3d Ri = Qi.toRotationMatrix(); - Eigen::Matrix3d Rj = Qj.toRotationMatrix(); - Eigen::Matrix3d ric = qic.toRotationMatrix(); - Eigen::Matrix reduce(2, 3); - const auto ric_t = ric.transpose(); - const auto Rj_t = Rj.transpose(); - const auto J_w = ric_t * Rj_t; - const auto J_imu_i = J_w * Ri; - const auto J_cam_i = J_imu_i * ric; + if (jacobians) { + Eigen::Matrix3d Ri = Qi.toRotationMatrix(); + Eigen::Matrix3d Rj = Qj.toRotationMatrix(); + Eigen::Matrix3d ric = qic.toRotationMatrix(); + Eigen::Matrix reduce(2, 3); + const auto ric_t = ric.transpose(); + const auto Rj_t = Rj.transpose(); + const auto J_w = ric_t * Rj_t; + const auto J_imu_i = J_w * Ri; + const auto J_cam_i = J_imu_i * ric; #ifdef UNIT_SPHERE_ERROR - double norm = pts_camera_j.norm(); - Eigen::Matrix3d norm_jaco; - double x1, x2, x3; - x1 = pts_camera_j(0); - x2 = pts_camera_j(1); - x3 = pts_camera_j(2); - double norm_3 = pow(norm, 3); - norm_jaco << 1.0 / norm - x1 * x1 / norm_3, - x1 * x2 / norm_3, - x1 * x3 / norm_3, - - x1 * x2 / norm_3, 1.0 / norm - x2 * x2 / norm_3, - x2 * x3 / norm_3, - - x1 * x3 / norm_3, - x2 * x3 / norm_3, 1.0 / norm - x3 * x3 / norm_3; - reduce = tangent_base * norm_jaco; - Matrix3d reduce_j_td; - x1 = pts_j_td(0); - x2 = pts_j_td(1); - x3 = pts_j_td(2); - norm_3 = pow(pts_j_td.norm(), 3); - reduce_j_td << 1.0 / norm - x1 * x1 / norm_3, - x1 * x2 / norm_3, - x1 * x3 / norm_3, - - x1 * x2 / norm_3, 1.0 / norm - x2 * x2 / norm_3, - x2 * x3 / norm_3, - - x1 * x3 / norm_3, - x2 * x3 / norm_3, 1.0 / norm - x3 * x3 / norm_3; + double norm = pts_camera_j.norm(); + Eigen::Matrix3d norm_jaco; + double x1, x2, x3; + x1 = pts_camera_j(0); + x2 = pts_camera_j(1); + x3 = pts_camera_j(2); + double norm_3 = pow(norm, 3); + norm_jaco << 1.0 / norm - x1 * x1 / norm_3, -x1 * x2 / norm_3, + -x1 * x3 / norm_3, -x1 * x2 / norm_3, 1.0 / norm - x2 * x2 / norm_3, + -x2 * x3 / norm_3, -x1 * x3 / norm_3, -x2 * x3 / norm_3, + 1.0 / norm - x3 * x3 / norm_3; + reduce = tangent_base * norm_jaco; + Matrix3d reduce_j_td; + x1 = pts_j_td(0); + x2 = pts_j_td(1); + x3 = pts_j_td(2); + norm_3 = pow(pts_j_td.norm(), 3); + reduce_j_td << 1.0 / norm - x1 * x1 / norm_3, -x1 * x2 / norm_3, + -x1 * x3 / norm_3, -x1 * x2 / norm_3, 1.0 / norm - x2 * x2 / norm_3, + -x2 * x3 / norm_3, -x1 * x3 / norm_3, -x2 * x3 / norm_3, + 1.0 / norm - x3 * x3 / norm_3; #else - reduce << 1. / dep_j, 0, -pts_camera_j(0) / (dep_j * dep_j), - 0, 1. / dep_j, -pts_camera_j(1) / (dep_j * dep_j); + reduce << 1. / dep_j, 0, -pts_camera_j(0) / (dep_j * dep_j), 0, 1. / dep_j, + -pts_camera_j(1) / (dep_j * dep_j); #endif - reduce = sqrt_info * reduce; - - if (jacobians[0]) - { - Eigen::Map> jacobian_pose_i(jacobians[0]); - - Eigen::Matrix jaco_i; - jaco_i.leftCols<3>() = J_w; - jaco_i.rightCols<3>() = J_imu_i * -Utility::skewSymmetric(pts_imu_i); - - jacobian_pose_i.leftCols<6>() = reduce * jaco_i; - jacobian_pose_i.rightCols<1>().setZero(); - } - - if (jacobians[1]) - { - Eigen::Map> jacobian_pose_j(jacobians[1]); - - Eigen::Matrix jaco_j; - jaco_j.leftCols<3>() = -J_w; - jaco_j.rightCols<3>() = ric_t * Utility::skewSymmetric(pts_imu_j); - - jacobian_pose_j.leftCols<6>() = reduce * jaco_j; - jacobian_pose_j.rightCols<1>().setZero(); - } - if (jacobians[2]) - { - Eigen::Map> jacobian_ex_pose(jacobians[2]); - Eigen::Matrix jaco_ex; - jaco_ex.leftCols<3>() = J_imu_i - ric_t; - jaco_ex.rightCols<3>() = -J_cam_i * Utility::skewSymmetric(pts_camera_i) + Utility::skewSymmetric(J_cam_i * pts_camera_i) + - Utility::skewSymmetric(J_w * (Ri * tic + Pi - Pj) - Rj_t*tic); - jacobian_ex_pose.leftCols<6>() = reduce * jaco_ex; - jacobian_ex_pose.rightCols<1>().setZero(); - } - if (jacobians[3]) - { - Eigen::Map jacobian_feature(jacobians[3]); - jacobian_feature = reduce * J_cam_i * pts_i_td * -1.0 / (inv_dep_i * inv_dep_i); - } - if (jacobians[4]) - { - Eigen::Map jacobian_td(jacobians[4]); + reduce = sqrt_info * reduce; + + if (jacobians[0]) { + Eigen::Map> jacobian_pose_i( + jacobians[0]); + + Eigen::Matrix jaco_i; + jaco_i.leftCols<3>() = J_w; + jaco_i.rightCols<3>() = J_imu_i * -Utility::skewSymmetric(pts_imu_i); + + jacobian_pose_i.leftCols<6>() = reduce * jaco_i; + jacobian_pose_i.rightCols<1>().setZero(); + } + + if (jacobians[1]) { + Eigen::Map> jacobian_pose_j( + jacobians[1]); + + Eigen::Matrix jaco_j; + jaco_j.leftCols<3>() = -J_w; + jaco_j.rightCols<3>() = ric_t * Utility::skewSymmetric(pts_imu_j); + + jacobian_pose_j.leftCols<6>() = reduce * jaco_j; + jacobian_pose_j.rightCols<1>().setZero(); + } + if (jacobians[2]) { + Eigen::Map> jacobian_ex_pose( + jacobians[2]); + Eigen::Matrix jaco_ex; + jaco_ex.leftCols<3>() = J_imu_i - ric_t; + jaco_ex.rightCols<3>() = + -J_cam_i * Utility::skewSymmetric(pts_camera_i) + + Utility::skewSymmetric(J_cam_i * pts_camera_i) + + Utility::skewSymmetric(J_w * (Ri * tic + Pi - Pj) - Rj_t * tic); + jacobian_ex_pose.leftCols<6>() = reduce * jaco_ex; + jacobian_ex_pose.rightCols<1>().setZero(); + } + if (jacobians[3]) { + Eigen::Map jacobian_feature(jacobians[3]); + jacobian_feature = + reduce * J_cam_i * pts_i_td * -1.0 / (inv_dep_i * inv_dep_i); + } + if (jacobians[4]) { + Eigen::Map jacobian_td(jacobians[4]); #ifdef UNIT_SPHERE_ERROR - jacobian_td = reduce * J_cam_i * velocity_i / inv_dep_i * -1.0 + - sqrt_info * tangent_base * reduce_j_td * velocity_j; + jacobian_td = reduce * J_cam_i * velocity_i / inv_dep_i * -1.0 + + sqrt_info * tangent_base * reduce_j_td * velocity_j; #else - jacobian_td = reduce * J_cam_i * velocity_i / inv_dep_i * -1.0 + - sqrt_info * velocity_j.head(2); + jacobian_td = reduce * J_cam_i * velocity_i / inv_dep_i * -1.0 + + sqrt_info * velocity_j.head(2); #endif - } } - return true; + } + return true; } -void ProjectionTwoFrameOneCamFactor::check(double **parameters) -{ - double *res = new double[2]; - double **jaco = new double *[5]; - jaco[0] = new double[2 * 7]; - jaco[1] = new double[2 * 7]; - jaco[2] = new double[2 * 7]; - jaco[3] = new double[2 * 1]; - jaco[4] = new double[2 * 1]; - Evaluate(parameters, res, jaco); - puts("check begins"); - - puts("my"); - - std::cout << Eigen::Map>(res).transpose() << std::endl - << std::endl; - std::cout << Eigen::Map>(jaco[0]) << std::endl - << std::endl; - std::cout << Eigen::Map>(jaco[1]) << std::endl - << std::endl; - std::cout << Eigen::Map>(jaco[2]) << std::endl - << std::endl; - std::cout << Eigen::Map(jaco[3]) << std::endl - << std::endl; - std::cout << Eigen::Map(jaco[4]) << std::endl - << std::endl; +void ProjectionTwoFrameOneCamFactor::check(double **parameters) { + double *res = new double[2]; + double **jaco = new double *[5]; + jaco[0] = new double[2 * 7]; + jaco[1] = new double[2 * 7]; + jaco[2] = new double[2 * 7]; + jaco[3] = new double[2 * 1]; + jaco[4] = new double[2 * 1]; + Evaluate(parameters, res, jaco); + puts("check begins"); + + puts("my"); + + std::cout << Eigen::Map>(res).transpose() + << std::endl + << std::endl; + std::cout << Eigen::Map>(jaco[0]) + << std::endl + << std::endl; + std::cout << Eigen::Map>(jaco[1]) + << std::endl + << std::endl; + std::cout << Eigen::Map>(jaco[2]) + << std::endl + << std::endl; + std::cout << Eigen::Map(jaco[3]) << std::endl << std::endl; + std::cout << Eigen::Map(jaco[4]) << std::endl << std::endl; + + Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]); + Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], + parameters[0][5]); + + Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]); + Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4], + parameters[1][5]); + + Eigen::Vector3d tic(parameters[2][0], parameters[2][1], parameters[2][2]); + Eigen::Quaterniond qic(parameters[2][6], parameters[2][3], parameters[2][4], + parameters[2][5]); + double inv_dep_i = parameters[3][0]; + double td = parameters[4][0]; + + Eigen::Vector3d pts_i_td, pts_j_td; + pts_i_td = pts_i - (td - td_i) * velocity_i; + pts_j_td = pts_j - (td - td_j) * velocity_j; + Eigen::Vector3d pts_camera_i = pts_i_td / inv_dep_i; + Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic; + Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi; + Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj); + Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic); + Eigen::Vector2d residual; +#ifdef UNIT_SPHERE_ERROR + residual = tangent_base * (pts_camera_j.normalized() - pts_j_td.normalized()); +#else + double dep_j = pts_camera_j.z(); + residual = (pts_camera_j / dep_j).head<2>() - pts_j_td.head<2>(); +#endif + residual = sqrt_info * residual; + + puts("num"); + std::cout << residual.transpose() << std::endl; + + const double eps = 1e-6; + Eigen::Matrix num_jacobian; + for (int k = 0; k < 20; k++) { Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]); - Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]); + Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], + parameters[0][5]); Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]); - Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]); + Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4], + parameters[1][5]); Eigen::Vector3d tic(parameters[2][0], parameters[2][1], parameters[2][2]); - Eigen::Quaterniond qic(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]); + Eigen::Quaterniond qic(parameters[2][6], parameters[2][3], parameters[2][4], + parameters[2][5]); double inv_dep_i = parameters[3][0]; double td = parameters[4][0]; + int a = k / 3, b = k % 3; + Eigen::Vector3d delta = Eigen::Vector3d(b == 0, b == 1, b == 2) * eps; + + if (a == 0) + Pi += delta; + else if (a == 1) + Qi = Qi * Utility::deltaQ(delta); + else if (a == 2) + Pj += delta; + else if (a == 3) + Qj = Qj * Utility::deltaQ(delta); + else if (a == 4) + tic += delta; + else if (a == 5) + qic = qic * Utility::deltaQ(delta); + else if (a == 6 && b == 0) + inv_dep_i += delta.x(); + else if (a == 6 && b == 1) + td += delta.y(); + Eigen::Vector3d pts_i_td, pts_j_td; - pts_i_td = pts_i - (td - td_i ) * velocity_i; - pts_j_td = pts_j - (td - td_j ) * velocity_j; + pts_i_td = pts_i - (td - td_i) * velocity_i; + pts_j_td = pts_j - (td - td_j) * velocity_j; Eigen::Vector3d pts_camera_i = pts_i_td / inv_dep_i; Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic; Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi; Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj); Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic); - Eigen::Vector2d residual; + Eigen::Vector2d tmp_residual; -#ifdef UNIT_SPHERE_ERROR - residual = tangent_base * (pts_camera_j.normalized() - pts_j_td.normalized()); +#ifdef UNIT_SPHERE_ERROR + tmp_residual = + tangent_base * (pts_camera_j.normalized() - pts_j_td.normalized()); #else double dep_j = pts_camera_j.z(); - residual = (pts_camera_j / dep_j).head<2>() - pts_j_td.head<2>(); -#endif - residual = sqrt_info * residual; - - puts("num"); - std::cout << residual.transpose() << std::endl; - - const double eps = 1e-6; - Eigen::Matrix num_jacobian; - for (int k = 0; k < 20; k++) - { - Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]); - Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]); - - Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]); - Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]); - - Eigen::Vector3d tic(parameters[2][0], parameters[2][1], parameters[2][2]); - Eigen::Quaterniond qic(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]); - double inv_dep_i = parameters[3][0]; - double td = parameters[4][0]; - - - int a = k / 3, b = k % 3; - Eigen::Vector3d delta = Eigen::Vector3d(b == 0, b == 1, b == 2) * eps; - - if (a == 0) - Pi += delta; - else if (a == 1) - Qi = Qi * Utility::deltaQ(delta); - else if (a == 2) - Pj += delta; - else if (a == 3) - Qj = Qj * Utility::deltaQ(delta); - else if (a == 4) - tic += delta; - else if (a == 5) - qic = qic * Utility::deltaQ(delta); - else if (a == 6 && b == 0) - inv_dep_i += delta.x(); - else if (a == 6 && b == 1) - td += delta.y(); - - Eigen::Vector3d pts_i_td, pts_j_td; - pts_i_td = pts_i - (td - td_i) * velocity_i; - pts_j_td = pts_j - (td - td_j) * velocity_j; - Eigen::Vector3d pts_camera_i = pts_i_td / inv_dep_i; - Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic; - Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi; - Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj); - Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic); - Eigen::Vector2d tmp_residual; - -#ifdef UNIT_SPHERE_ERROR - tmp_residual = tangent_base * (pts_camera_j.normalized() - pts_j_td.normalized()); -#else - double dep_j = pts_camera_j.z(); - tmp_residual = (pts_camera_j / dep_j).head<2>() - pts_j_td.head<2>(); + tmp_residual = (pts_camera_j / dep_j).head<2>() - pts_j_td.head<2>(); #endif - tmp_residual = sqrt_info * tmp_residual; - - num_jacobian.col(k) = (tmp_residual - residual) / eps; - } - std::cout << num_jacobian.block<2, 6>(0, 0) << std::endl; - std::cout << num_jacobian.block<2, 6>(0, 6) << std::endl; - std::cout << num_jacobian.block<2, 6>(0, 12) << std::endl; - std::cout << num_jacobian.block<2, 1>(0, 18) << std::endl; - std::cout << num_jacobian.block<2, 1>(0, 19) << std::endl; + tmp_residual = sqrt_info * tmp_residual; + + num_jacobian.col(k) = (tmp_residual - residual) / eps; + } + std::cout << num_jacobian.block<2, 6>(0, 0) << std::endl; + std::cout << num_jacobian.block<2, 6>(0, 6) << std::endl; + std::cout << num_jacobian.block<2, 6>(0, 12) << std::endl; + std::cout << num_jacobian.block<2, 1>(0, 18) << std::endl; + std::cout << num_jacobian.block<2, 1>(0, 19) << std::endl; } -} \ No newline at end of file +} // namespace D2VINS \ No newline at end of file diff --git a/d2vins/src/factors/projectionTwoFrameTwoCamFactor.cpp b/d2vins/src/factors/projectionTwoFrameTwoCamFactor.cpp index e840ac7a..dfdff612 100644 --- a/d2vins/src/factors/projectionTwoFrameTwoCamFactor.cpp +++ b/d2vins/src/factors/projectionTwoFrameTwoCamFactor.cpp @@ -1,8 +1,9 @@ /******************************************************* - * Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology - * + * Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science + *and Technology + * * This file is part of VINS. - * + * * Licensed under the GNU General Public License v3.0; * you may not use this file except in compliance with the License. * @@ -10,7 +11,9 @@ *******************************************************/ #include "projectionTwoFrameTwoCamFactor.h" + #include + #include "../d2vins_params.hpp" using namespace D2Common; @@ -18,161 +21,168 @@ namespace D2VINS { Eigen::Matrix2d ProjectionTwoFrameTwoCamFactor::sqrt_info; double ProjectionTwoFrameTwoCamFactor::sum_t; -ProjectionTwoFrameTwoCamFactor::ProjectionTwoFrameTwoCamFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j, - const Eigen::Vector3d &_velocity_i, const Eigen::Vector3d &_velocity_j, - const double _td_i, const double _td_j) : - pts_i(_pts_i), pts_j(_pts_j), - td_i(_td_i), td_j(_td_j), - velocity_i(_velocity_i), velocity_j(_velocity_j) -{ +ProjectionTwoFrameTwoCamFactor::ProjectionTwoFrameTwoCamFactor( + const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j, + const Eigen::Vector3d &_velocity_i, const Eigen::Vector3d &_velocity_j, + const double _td_i, const double _td_j) + : pts_i(_pts_i), + pts_j(_pts_j), + td_i(_td_i), + td_j(_td_j), + velocity_i(_velocity_i), + velocity_j(_velocity_j) { #ifdef UNIT_SPHERE_ERROR - Eigen::Vector3d b1, b2; - Eigen::Vector3d a = pts_j.normalized(); - Eigen::Vector3d tmp(0, 0, 1); - if(a == tmp) - tmp << 1, 0, 0; - b1 = (tmp - a * (a.transpose() * tmp)).normalized(); - b2 = a.cross(b1); - tangent_base.block<1, 3>(0, 0) = b1.transpose(); - tangent_base.block<1, 3>(1, 0) = b2.transpose(); + Eigen::Vector3d b1, b2; + Eigen::Vector3d a = pts_j.normalized(); + Eigen::Vector3d tmp(0, 0, 1); + if (a == tmp) tmp << 1, 0, 0; + b1 = (tmp - a * (a.transpose() * tmp)).normalized(); + b2 = a.cross(b1); + tangent_base.block<1, 3>(0, 0) = b1.transpose(); + tangent_base.block<1, 3>(1, 0) = b2.transpose(); #endif }; -bool ProjectionTwoFrameTwoCamFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const -{ - Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]); - Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]); +bool ProjectionTwoFrameTwoCamFactor::Evaluate(double const *const *parameters, + double *residuals, + double **jacobians) const { + Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]); + Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], + parameters[0][5]); - Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]); - Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]); + Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]); + Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4], + parameters[1][5]); - Eigen::Vector3d tic(parameters[2][0], parameters[2][1], parameters[2][2]); - Eigen::Quaterniond qic(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]); + Eigen::Vector3d tic(parameters[2][0], parameters[2][1], parameters[2][2]); + Eigen::Quaterniond qic(parameters[2][6], parameters[2][3], parameters[2][4], + parameters[2][5]); - Eigen::Vector3d tic2(parameters[3][0], parameters[3][1], parameters[3][2]); - Eigen::Quaterniond qic2(parameters[3][6], parameters[3][3], parameters[3][4], parameters[3][5]); + Eigen::Vector3d tic2(parameters[3][0], parameters[3][1], parameters[3][2]); + Eigen::Quaterniond qic2(parameters[3][6], parameters[3][3], parameters[3][4], + parameters[3][5]); - double inv_dep_i = parameters[4][0]; + double inv_dep_i = parameters[4][0]; - double td = parameters[5][0]; + double td = parameters[5][0]; - Eigen::Vector3d pts_i_td, pts_j_td; - pts_i_td = pts_i - (td - td_i) * velocity_i; - pts_j_td = pts_j - (td - td_j) * velocity_j; + Eigen::Vector3d pts_i_td, pts_j_td; + pts_i_td = pts_i - (td - td_i) * velocity_i; + pts_j_td = pts_j - (td - td_j) * velocity_j; - Eigen::Vector3d pts_camera_i = pts_i_td / inv_dep_i; - Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic; - Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi; - Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj); - Eigen::Vector3d pts_camera_j = qic2.inverse() * (pts_imu_j - tic2); - Eigen::Map residual(residuals); + Eigen::Vector3d pts_camera_i = pts_i_td / inv_dep_i; + Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic; + Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi; + Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj); + Eigen::Vector3d pts_camera_j = qic2.inverse() * (pts_imu_j - tic2); + Eigen::Map residual(residuals); -#ifdef UNIT_SPHERE_ERROR - residual = tangent_base * (pts_camera_j.normalized() - pts_j_td.normalized()); +#ifdef UNIT_SPHERE_ERROR + residual = tangent_base * (pts_camera_j.normalized() - pts_j_td.normalized()); #else - double dep_j = pts_camera_j.z(); - residual = (pts_camera_j / dep_j).head<2>() - pts_j_td.head<2>(); + double dep_j = pts_camera_j.z(); + residual = (pts_camera_j / dep_j).head<2>() - pts_j_td.head<2>(); #endif - residual = sqrt_info * residual; + residual = sqrt_info * residual; - if (jacobians) - { - Eigen::Matrix3d Ri = Qi.toRotationMatrix(); - Eigen::Matrix3d Rj = Qj.toRotationMatrix(); - Eigen::Matrix3d ric = qic.toRotationMatrix(); - Eigen::Matrix3d ric2 = qic2.toRotationMatrix(); - Eigen::Matrix reduce(2, 3); - const auto ric2_t = ric2.transpose(); - const auto J_w = ric2_t * Rj.transpose(); - const auto J_imu_i = J_w * Ri; - const auto J_cam_i = J_imu_i * ric; + if (jacobians) { + Eigen::Matrix3d Ri = Qi.toRotationMatrix(); + Eigen::Matrix3d Rj = Qj.toRotationMatrix(); + Eigen::Matrix3d ric = qic.toRotationMatrix(); + Eigen::Matrix3d ric2 = qic2.toRotationMatrix(); + Eigen::Matrix reduce(2, 3); + const auto ric2_t = ric2.transpose(); + const auto J_w = ric2_t * Rj.transpose(); + const auto J_imu_i = J_w * Ri; + const auto J_cam_i = J_imu_i * ric; #ifdef UNIT_SPHERE_ERROR - double norm = pts_camera_j.norm(); - Eigen::Matrix3d norm_jaco; - double x1, x2, x3; - x1 = pts_camera_j(0); - x2 = pts_camera_j(1); - x3 = pts_camera_j(2); - double norm_3 = pow(norm, 3); - norm_jaco << 1.0 / norm - x1 * x1 / norm_3, - x1 * x2 / norm_3, - x1 * x3 / norm_3, - - x1 * x2 / norm_3, 1.0 / norm - x2 * x2 / norm_3, - x2 * x3 / norm_3, - - x1 * x3 / norm_3, - x2 * x3 / norm_3, 1.0 / norm - x3 * x3 / norm_3; - reduce = tangent_base * norm_jaco; - Matrix3d reduce_j_td; - x1 = pts_j_td(0); - x2 = pts_j_td(1); - x3 = pts_j_td(2); - norm_3 = pow(pts_j_td.norm(), 3); - reduce_j_td << 1.0 / norm - x1 * x1 / norm_3, - x1 * x2 / norm_3, - x1 * x3 / norm_3, - - x1 * x2 / norm_3, 1.0 / norm - x2 * x2 / norm_3, - x2 * x3 / norm_3, - - x1 * x3 / norm_3, - x2 * x3 / norm_3, 1.0 / norm - x3 * x3 / norm_3; + double norm = pts_camera_j.norm(); + Eigen::Matrix3d norm_jaco; + double x1, x2, x3; + x1 = pts_camera_j(0); + x2 = pts_camera_j(1); + x3 = pts_camera_j(2); + double norm_3 = pow(norm, 3); + norm_jaco << 1.0 / norm - x1 * x1 / norm_3, -x1 * x2 / norm_3, + -x1 * x3 / norm_3, -x1 * x2 / norm_3, 1.0 / norm - x2 * x2 / norm_3, + -x2 * x3 / norm_3, -x1 * x3 / norm_3, -x2 * x3 / norm_3, + 1.0 / norm - x3 * x3 / norm_3; + reduce = tangent_base * norm_jaco; + Matrix3d reduce_j_td; + x1 = pts_j_td(0); + x2 = pts_j_td(1); + x3 = pts_j_td(2); + norm_3 = pow(pts_j_td.norm(), 3); + reduce_j_td << 1.0 / norm - x1 * x1 / norm_3, -x1 * x2 / norm_3, + -x1 * x3 / norm_3, -x1 * x2 / norm_3, 1.0 / norm - x2 * x2 / norm_3, + -x2 * x3 / norm_3, -x1 * x3 / norm_3, -x2 * x3 / norm_3, + 1.0 / norm - x3 * x3 / norm_3; #else - reduce << 1. / dep_j, 0, -pts_camera_j(0) / (dep_j * dep_j), - 0, 1. / dep_j, -pts_camera_j(1) / (dep_j * dep_j); + reduce << 1. / dep_j, 0, -pts_camera_j(0) / (dep_j * dep_j), 0, 1. / dep_j, + -pts_camera_j(1) / (dep_j * dep_j); #endif - reduce = sqrt_info * reduce; - - if (jacobians[0]) - { - Eigen::Map> jacobian_pose_i(jacobians[0]); - - Eigen::Matrix jaco_i; - jaco_i.leftCols<3>() = J_w; - jaco_i.rightCols<3>() = J_imu_i * -Utility::skewSymmetric(pts_imu_i); - - jacobian_pose_i.leftCols<6>() = reduce * jaco_i; - jacobian_pose_i.rightCols<1>().setZero(); - } - - if (jacobians[1]) - { - Eigen::Map> jacobian_pose_j(jacobians[1]); - - Eigen::Matrix jaco_j; - jaco_j.leftCols<3>() = -J_w; - jaco_j.rightCols<3>() = ric2_t * Utility::skewSymmetric(pts_imu_j); - - jacobian_pose_j.leftCols<6>() = reduce * jaco_j; - jacobian_pose_j.rightCols<1>().setZero(); - } - if (jacobians[2]) - { - Eigen::Map> jacobian_ex_pose(jacobians[2]); - Eigen::Matrix jaco_ex; - jaco_ex.leftCols<3>() = J_imu_i; - jaco_ex.rightCols<3>() = J_cam_i * -Utility::skewSymmetric(pts_camera_i); - jacobian_ex_pose.leftCols<6>() = reduce * jaco_ex; - jacobian_ex_pose.rightCols<1>().setZero(); - } - if (jacobians[3]) - { - Eigen::Map> jacobian_ex_pose1(jacobians[3]); - Eigen::Matrix jaco_ex; - jaco_ex.leftCols<3>() = - ric2_t; - jaco_ex.rightCols<3>() = Utility::skewSymmetric(pts_camera_j); - jacobian_ex_pose1.leftCols<6>() = reduce * jaco_ex; - jacobian_ex_pose1.rightCols<1>().setZero(); - } - if (jacobians[4]) - { - Eigen::Map jacobian_feature(jacobians[4]); - jacobian_feature = reduce * J_cam_i * pts_i_td * -1.0 / (inv_dep_i * inv_dep_i); - } - if (jacobians[5]) - { + reduce = sqrt_info * reduce; + + if (jacobians[0]) { + Eigen::Map> jacobian_pose_i( + jacobians[0]); + + Eigen::Matrix jaco_i; + jaco_i.leftCols<3>() = J_w; + jaco_i.rightCols<3>() = J_imu_i * -Utility::skewSymmetric(pts_imu_i); + + jacobian_pose_i.leftCols<6>() = reduce * jaco_i; + jacobian_pose_i.rightCols<1>().setZero(); + } + + if (jacobians[1]) { + Eigen::Map> jacobian_pose_j( + jacobians[1]); + + Eigen::Matrix jaco_j; + jaco_j.leftCols<3>() = -J_w; + jaco_j.rightCols<3>() = ric2_t * Utility::skewSymmetric(pts_imu_j); + + jacobian_pose_j.leftCols<6>() = reduce * jaco_j; + jacobian_pose_j.rightCols<1>().setZero(); + } + if (jacobians[2]) { + Eigen::Map> jacobian_ex_pose( + jacobians[2]); + Eigen::Matrix jaco_ex; + jaco_ex.leftCols<3>() = J_imu_i; + jaco_ex.rightCols<3>() = J_cam_i * -Utility::skewSymmetric(pts_camera_i); + jacobian_ex_pose.leftCols<6>() = reduce * jaco_ex; + jacobian_ex_pose.rightCols<1>().setZero(); + } + if (jacobians[3]) { + Eigen::Map> + jacobian_ex_pose1(jacobians[3]); + Eigen::Matrix jaco_ex; + jaco_ex.leftCols<3>() = -ric2_t; + jaco_ex.rightCols<3>() = Utility::skewSymmetric(pts_camera_j); + jacobian_ex_pose1.leftCols<6>() = reduce * jaco_ex; + jacobian_ex_pose1.rightCols<1>().setZero(); + } + if (jacobians[4]) { + Eigen::Map jacobian_feature(jacobians[4]); + jacobian_feature = + reduce * J_cam_i * pts_i_td * -1.0 / (inv_dep_i * inv_dep_i); + } + if (jacobians[5]) { #ifdef UNIT_SPHERE_ERROR - Eigen::Map jacobian_td(jacobians[5]); - jacobian_td = reduce * J_cam_i * velocity_i / inv_dep_i * -1.0 + - sqrt_info * tangent_base * reduce_j_td * velocity_j; + Eigen::Map jacobian_td(jacobians[5]); + jacobian_td = reduce * J_cam_i * velocity_i / inv_dep_i * -1.0 + + sqrt_info * tangent_base * reduce_j_td * velocity_j; #else - Eigen::Map jacobian_td(jacobians[5]); - jacobian_td = reduce * J_cam_i * velocity_i / inv_dep_i * -1.0 + - sqrt_info * velocity_j.head(2); + Eigen::Map jacobian_td(jacobians[5]); + jacobian_td = reduce * J_cam_i * velocity_i / inv_dep_i * -1.0 + + sqrt_info * velocity_j.head(2); #endif - } } - return true; + } + return true; } -} \ No newline at end of file +} // namespace D2VINS \ No newline at end of file diff --git a/d2vins/src/factors/reprojection3d.h b/d2vins/src/factors/reprojection3d.h new file mode 100644 index 00000000..821bd592 --- /dev/null +++ b/d2vins/src/factors/reprojection3d.h @@ -0,0 +1,35 @@ +#pragma once + +#include +#include +#include + +struct ReprojectionError3D +{ + ReprojectionError3D(double observed_u, double observed_v) + :observed_u(observed_u), observed_v(observed_v) + {} + + template + bool operator()(const T* const camera_R, const T* const camera_T, const T* point, T* residuals) const + { + Eigen::Map> p_eigen(camera_T); + Eigen::Map> q_eigen(camera_R); + Eigen::Map> point_eigen(point); + Eigen::Matrix pt_cam = q_eigen.inverse()*(point_eigen - p_eigen); + residuals[0] = pt_cam.x()/pt_cam.z() - T(observed_u); + residuals[1] = pt_cam.y()/pt_cam.z() - T(observed_v); + return true; + } + + static ceres::CostFunction* Create(const double observed_x, + const double observed_y) + { + return (new ceres::AutoDiffCostFunction< + ReprojectionError3D, 2, 4, 3, 3>( + new ReprojectionError3D(observed_x,observed_y))); + } + + double observed_u; + double observed_v; +}; \ No newline at end of file diff --git a/d2vins/src/network/d2vins_net.cpp b/d2vins/src/network/d2vins_net.cpp index 90ca909d..f3b29187 100644 --- a/d2vins/src/network/d2vins_net.cpp +++ b/d2vins/src/network/d2vins_net.cpp @@ -1,71 +1,74 @@ #include "d2vins_net.hpp" -#include "../estimator/d2estimator.hpp" + #include +#include "../estimator/d2estimator.hpp" + namespace D2VINS { -D2VINSNet::D2VINSNet(D2Estimator * _estimator, std::string _lcm_uri): - lcm(_lcm_uri), estimator(_estimator), state(_estimator->getState()) { - lcm.subscribe("DISTRIB_VINS_DATA", &D2VINSNet::onDistributedVinsData, this); - lcm.subscribe("SYNC_SIGNAL", &D2VINSNet::receiveSyncSignal, this); +D2VINSNet::D2VINSNet(D2Estimator* _estimator, std::string _lcm_uri) + : lcm(_lcm_uri), estimator(_estimator), state(_estimator->getState()) { + lcm.subscribe("DISTRIB_VINS_DATA", &D2VINSNet::onDistributedVinsData, this); + lcm.subscribe("SYNC_SIGNAL", &D2VINSNet::receiveSyncSignal, this); } void D2VINSNet::pubSlidingWindow() { - if (state.size() == 0) { - return; - } - SlidingWindow_t sld_win; - sld_win.timestamp = toLCMTime(ros::Time(state.lastFrame().stamp)); - sld_win.sld_win_len = state.size(); - sld_win.drone_id = params->self_id; - for (int i = 0; i < state.size(); i ++) { - auto & frame = state.getFrame(i); - sld_win.frame_ids.push_back(frame.frame_id); - } - lcm.publish("SYNC_SLDWIN", &sld_win); + if (state.size() == 0) { + return; + } + SlidingWindow_t sld_win; + sld_win.timestamp = toLCMTime(ros::Time(state.lastFrame().stamp)); + sld_win.sld_win_len = state.size(); + sld_win.drone_id = params->self_id; + for (int i = 0; i < state.size(); i++) { + auto& frame = state.getFrame(i); + sld_win.frame_ids.push_back(frame.frame_id); + } + lcm.publish("SYNC_SLDWIN", &sld_win); } void D2VINSNet::sendSyncSignal(int signal, int64_t token) { - DistributedSync_t sync_signal; - sync_signal.drone_id = params->self_id; - sync_signal.sync_signal = signal; - sync_signal.timestamp = toLCMTime(ros::Time::now()); - sync_signal.solver_token = token; - lcm.publish("SYNC_SIGNAL", &sync_signal); + DistributedSync_t sync_signal; + sync_signal.drone_id = params->self_id; + sync_signal.sync_signal = signal; + sync_signal.timestamp = toLCMTime(ros::Time::now()); + sync_signal.solver_token = token; + lcm.publish("SYNC_SIGNAL", &sync_signal); } void D2VINSNet::receiveSyncSignal(const lcm::ReceiveBuffer* rbuf, - const std::string& chan, - const DistributedSync_t* msg) { - if (msg->drone_id == params->self_id) { - return; - } - DistributedSync_callback(msg->drone_id, msg->sync_signal, msg->solver_token); + const std::string& chan, + const DistributedSync_t* msg) { + if (msg->drone_id == params->self_id) { + return; + } + DistributedSync_callback(msg->drone_id, msg->sync_signal, msg->solver_token); } void D2VINSNet::onSldWinReceived(const lcm::ReceiveBuffer* rbuf, - const std::string& chan, - const SlidingWindow_t* msg) { - if (msg->drone_id == params->self_id) { - return; - } - state.updateSldwin(msg->drone_id, msg->frame_ids); + const std::string& chan, + const SlidingWindow_t* msg) { + if (msg->drone_id == params->self_id) { + return; + } + state.updateSldwin(msg->drone_id, msg->frame_ids); } void D2VINSNet::onDistributedVinsData(const lcm::ReceiveBuffer* rbuf, - const std::string& chan, - const DistributedVinsData_t * msg) { - if (msg->drone_id == params->self_id) { - return; - } - DistributedVinsData_callback(DistributedVinsData(*msg)); + const std::string& chan, + const DistributedVinsData_t* msg) { + if (msg->drone_id == params->self_id) { + return; + } + DistributedVinsData_callback(DistributedVinsData(*msg)); } -void D2VINSNet::sendDistributedVinsData(const DistributedVinsData & data) { - DistributedVinsData_t msg = data.toLCM(); - if (params->print_network_status) { - printf("[D2VINS] Broadcast VINS Data size %ld with %ld poses %ld extrinsic.\n", - msg.getEncodedSize(), data.frame_poses.size(), data.extrinsic.size()); - } - lcm.publish("DISTRIB_VINS_DATA", &msg); +void D2VINSNet::sendDistributedVinsData(const DistributedVinsData& data) { + DistributedVinsData_t msg = data.toLCM(); + if (params->print_network_status) { + printf( + "[D2VINS] Broadcast VINS Data size %ld with %ld poses %ld extrinsic.\n", + msg.getEncodedSize(), data.frame_poses.size(), data.extrinsic.size()); + } + lcm.publish("DISTRIB_VINS_DATA", &msg); } -} \ No newline at end of file +} // namespace D2VINS \ No newline at end of file diff --git a/d2vins/src/utils/solve_5pts.cpp b/d2vins/src/utils/solve_5pts.cpp new file mode 100644 index 00000000..52a40493 --- /dev/null +++ b/d2vins/src/utils/solve_5pts.cpp @@ -0,0 +1,52 @@ +#include "solve_5pts.h" + +#include "spdlog/spdlog.h" + +namespace utils { + +bool MotionEstimator::solveRelativeRT( + const std::vector> &corres, + Matrix3d &Rotation, Vector3d &Translation) { + if (corres.size() >= 15) { + std::vector ll, rr; + for (int i = 0; i < int(corres.size()); i++) { + if (fabs(corres[i].first(2)) > 0.001 && + fabs(corres[i].second(2)) > 0.001) { + cv::Point2f first = + cv::Point2f(corres[i].first(0) / corres[i].first(2), + corres[i].first(1) / corres[i].first(2)); + cv::Point2f second = + cv::Point2f(corres[i].second(0) / corres[i].second(2), + corres[i].second(1) / corres[i].second(2)); + ll.push_back(first); + rr.push_back(second); + } + } + cv::Mat cameraMatrix = + (cv::Mat_(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1); + cv::Mat mask; + cv::Mat E = cv::findEssentialMat(ll, rr, cameraMatrix, cv::RANSAC, 0.99, + 0.3 / 460, 1000, mask); + // cv::Mat E = cv::findFundamentalMat(ll, rr, cv::FM_RANSAC, 0.3 / 460, + // 0.99, mask); + cv::Mat rot, trans; + int inlier_cnt = cv::recoverPose(E, ll, rr, cameraMatrix, rot, trans, mask); + SPDLOG_INFO("solveRelativeRT inlier_cnt: {}/{}", inlier_cnt, corres.size()); + Eigen::Matrix3d R; + Eigen::Vector3d T; + for (int i = 0; i < 3; i++) { + T(i) = trans.at(i, 0); + for (int j = 0; j < 3; j++) R(i, j) = rot.at(i, j); + } + + Rotation = R.transpose(); + Translation = -R.transpose() * T; + if (inlier_cnt > 12) + return true; + else + return false; + } + return false; +} + +} // namespace utils diff --git a/d2vins/src/utils/solve_5pts.h b/d2vins/src/utils/solve_5pts.h new file mode 100644 index 00000000..bd376ef2 --- /dev/null +++ b/d2vins/src/utils/solve_5pts.h @@ -0,0 +1,24 @@ +#pragma once + +#include + +#include +#include + +using namespace Eigen; + +namespace utils { +class MotionEstimator { + public: + bool + solveRelativeRT(const std::vector> &corres, + Matrix3d &R, Vector3d &T); + + private: + double testTriangulation(const std::vector &l, + const std::vector &r, cv::Mat_ R, + cv::Mat_ t); + void decomposeE(cv::Mat E, cv::Mat_ &R1, cv::Mat_ &R2, + cv::Mat_ &t1, cv::Mat_ &t2); +}; +} // namespace utils diff --git a/d2vins/src/visualization/CameraPoseVisualization.cpp b/d2vins/src/visualization/CameraPoseVisualization.cpp index 408e451c..42697959 100644 --- a/d2vins/src/visualization/CameraPoseVisualization.cpp +++ b/d2vins/src/visualization/CameraPoseVisualization.cpp @@ -1,8 +1,9 @@ /******************************************************* - * Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology - * + * Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science + *and Technology + * * This file is part of VINS. - * + * * Licensed under the GNU General Public License v3.0; * you may not use this file except in compliance with the License. *******************************************************/ @@ -10,304 +11,313 @@ #include "CameraPoseVisualization.h" namespace D2VINS { -const Eigen::Vector3d CameraPoseVisualization::imlt = Eigen::Vector3d(-1.0, -0.5, 1.0); -const Eigen::Vector3d CameraPoseVisualization::imrt = Eigen::Vector3d( 1.0, -0.5, 1.0); -const Eigen::Vector3d CameraPoseVisualization::imlb = Eigen::Vector3d(-1.0, 0.5, 1.0); -const Eigen::Vector3d CameraPoseVisualization::imrb = Eigen::Vector3d( 1.0, 0.5, 1.0); -const Eigen::Vector3d CameraPoseVisualization::lt0 = Eigen::Vector3d(-0.7, -0.5, 1.0); -const Eigen::Vector3d CameraPoseVisualization::lt1 = Eigen::Vector3d(-0.7, -0.2, 1.0); -const Eigen::Vector3d CameraPoseVisualization::lt2 = Eigen::Vector3d(-1.0, -0.2, 1.0); -const Eigen::Vector3d CameraPoseVisualization::oc = Eigen::Vector3d(0.0, 0.0, 0.0); +const Eigen::Vector3d CameraPoseVisualization::imlt = + Eigen::Vector3d(-1.0, -0.5, 1.0); +const Eigen::Vector3d CameraPoseVisualization::imrt = + Eigen::Vector3d(1.0, -0.5, 1.0); +const Eigen::Vector3d CameraPoseVisualization::imlb = + Eigen::Vector3d(-1.0, 0.5, 1.0); +const Eigen::Vector3d CameraPoseVisualization::imrb = + Eigen::Vector3d(1.0, 0.5, 1.0); +const Eigen::Vector3d CameraPoseVisualization::lt0 = + Eigen::Vector3d(-0.7, -0.5, 1.0); +const Eigen::Vector3d CameraPoseVisualization::lt1 = + Eigen::Vector3d(-0.7, -0.2, 1.0); +const Eigen::Vector3d CameraPoseVisualization::lt2 = + Eigen::Vector3d(-1.0, -0.2, 1.0); +const Eigen::Vector3d CameraPoseVisualization::oc = + Eigen::Vector3d(0.0, 0.0, 0.0); void Eigen2Point(const Eigen::Vector3d& v, geometry_msgs::Point& p) { - p.x = v.x(); - p.y = v.y(); - p.z = v.z(); + p.x = v.x(); + p.y = v.y(); + p.z = v.z(); } CameraPoseVisualization::CameraPoseVisualization() : m_marker_ns("CameraPoseVisualization"), m_scale(0.2), m_line_width(0.01) { - m_image_boundary_color.r = 1; - m_image_boundary_color.g = 0; - m_image_boundary_color.b = 0; - m_image_boundary_color.a = 1; - m_optical_center_connector_color.r = 1; - m_optical_center_connector_color.g = 0; - m_optical_center_connector_color.b = 0; - m_optical_center_connector_color.a = 1; + m_image_boundary_color.r = 1; + m_image_boundary_color.g = 0; + m_image_boundary_color.b = 0; + m_image_boundary_color.a = 1; + m_optical_center_connector_color.r = 1; + m_optical_center_connector_color.g = 0; + m_optical_center_connector_color.b = 0; + m_optical_center_connector_color.a = 1; } CameraPoseVisualization::CameraPoseVisualization(Eigen::Vector3d rgb, float a) : m_marker_ns("CameraPoseVisualization"), m_scale(0.2), m_line_width(0.01) { - m_image_boundary_color.r = rgb.x(); - m_image_boundary_color.g = rgb.y(); - m_image_boundary_color.b = rgb.z(); - m_image_boundary_color.a = a; - m_optical_center_connector_color.r = rgb.x(); - m_optical_center_connector_color.g = rgb.y(); - m_optical_center_connector_color.b = rgb.z(); - m_optical_center_connector_color.a = a; + m_image_boundary_color.r = rgb.x(); + m_image_boundary_color.g = rgb.y(); + m_image_boundary_color.b = rgb.z(); + m_image_boundary_color.a = a; + m_optical_center_connector_color.r = rgb.x(); + m_optical_center_connector_color.g = rgb.y(); + m_optical_center_connector_color.b = rgb.z(); + m_optical_center_connector_color.a = a; } -void CameraPoseVisualization::setImageBoundaryColor(float r, float g, float b, float a) { - m_image_boundary_color.r = r; - m_image_boundary_color.g = g; - m_image_boundary_color.b = b; - m_image_boundary_color.a = a; +void CameraPoseVisualization::setImageBoundaryColor(float r, float g, float b, + float a) { + m_image_boundary_color.r = r; + m_image_boundary_color.g = g; + m_image_boundary_color.b = b; + m_image_boundary_color.a = a; } -void CameraPoseVisualization::setOpticalCenterConnectorColor(float r, float g, float b, float a) { - m_optical_center_connector_color.r = r; - m_optical_center_connector_color.g = g; - m_optical_center_connector_color.b = b; - m_optical_center_connector_color.a = a; +void CameraPoseVisualization::setOpticalCenterConnectorColor(float r, float g, + float b, float a) { + m_optical_center_connector_color.r = r; + m_optical_center_connector_color.g = g; + m_optical_center_connector_color.b = b; + m_optical_center_connector_color.a = a; } -void CameraPoseVisualization::setScale(double s) { - m_scale = s; -} +void CameraPoseVisualization::setScale(double s) { m_scale = s; } void CameraPoseVisualization::setLineWidth(double width) { - m_line_width = width; + m_line_width = width; } -void CameraPoseVisualization::addEdge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1){ - visualization_msgs::Marker marker; +void CameraPoseVisualization::addEdge(const Eigen::Vector3d& p0, + const Eigen::Vector3d& p1) { + visualization_msgs::Marker marker; - marker.ns = m_marker_ns; - marker.id = m_markers.size() + 1; - marker.type = visualization_msgs::Marker::LINE_LIST; - marker.action = visualization_msgs::Marker::ADD; - marker.scale.x = 0.005; + marker.ns = m_marker_ns; + marker.id = m_markers.size() + 1; + marker.type = visualization_msgs::Marker::LINE_LIST; + marker.action = visualization_msgs::Marker::ADD; + marker.scale.x = 0.005; - marker.color.g = 1.0f; - marker.color.a = 1.0; + marker.color.g = 1.0f; + marker.color.a = 1.0; - geometry_msgs::Point point0, point1; + geometry_msgs::Point point0, point1; - Eigen2Point(p0, point0); - Eigen2Point(p1, point1); + Eigen2Point(p0, point0); + Eigen2Point(p1, point1); - marker.points.push_back(point0); - marker.points.push_back(point1); + marker.points.push_back(point0); + marker.points.push_back(point1); - m_markers.push_back(marker); + m_markers.push_back(marker); } -void CameraPoseVisualization::addLoopedge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1){ - visualization_msgs::Marker marker; +void CameraPoseVisualization::addLoopedge(const Eigen::Vector3d& p0, + const Eigen::Vector3d& p1) { + visualization_msgs::Marker marker; - marker.ns = m_marker_ns; - marker.id = m_markers.size() + 1; - marker.type = visualization_msgs::Marker::LINE_LIST; - marker.action = visualization_msgs::Marker::ADD; - marker.scale.x = 0.04; - //marker.scale.x = 0.3; + marker.ns = m_marker_ns; + marker.id = m_markers.size() + 1; + marker.type = visualization_msgs::Marker::LINE_LIST; + marker.action = visualization_msgs::Marker::ADD; + marker.scale.x = 0.04; + // marker.scale.x = 0.3; - marker.color.r = 1.0f; - marker.color.b = 1.0f; - marker.color.a = 1.0; + marker.color.r = 1.0f; + marker.color.b = 1.0f; + marker.color.a = 1.0; - geometry_msgs::Point point0, point1; + geometry_msgs::Point point0, point1; - Eigen2Point(p0, point0); - Eigen2Point(p1, point1); + Eigen2Point(p0, point0); + Eigen2Point(p1, point1); - marker.points.push_back(point0); - marker.points.push_back(point1); + marker.points.push_back(point0); + marker.points.push_back(point1); - m_markers.push_back(marker); + m_markers.push_back(marker); } - -void CameraPoseVisualization::addPose(const Eigen::Vector3d& p, const Eigen::Quaterniond& q) { - visualization_msgs::Marker marker; - - marker.ns = m_marker_ns; - marker.id = m_markers.size() + 1; - marker.type = visualization_msgs::Marker::LINE_STRIP; - marker.action = visualization_msgs::Marker::ADD; - marker.scale.x = m_line_width; - - marker.pose.position.x = 0.0; - marker.pose.position.y = 0.0; - marker.pose.position.z = 0.0; - marker.pose.orientation.w = 1.0; - marker.pose.orientation.x = 0.0; - marker.pose.orientation.y = 0.0; - marker.pose.orientation.z = 0.0; - - - geometry_msgs::Point pt_lt, pt_lb, pt_rt, pt_rb, pt_oc, pt_lt0, pt_lt1, pt_lt2; - - Eigen2Point(q * (m_scale *imlt) + p, pt_lt); - Eigen2Point(q * (m_scale *imlb) + p, pt_lb); - Eigen2Point(q * (m_scale *imrt) + p, pt_rt); - Eigen2Point(q * (m_scale *imrb) + p, pt_rb); - Eigen2Point(q * (m_scale *lt0 ) + p, pt_lt0); - Eigen2Point(q * (m_scale *lt1 ) + p, pt_lt1); - Eigen2Point(q * (m_scale *lt2 ) + p, pt_lt2); - Eigen2Point(q * (m_scale *oc ) + p, pt_oc); - - // image boundaries - marker.points.push_back(pt_lt); - marker.points.push_back(pt_lb); - marker.colors.push_back(m_image_boundary_color); - marker.colors.push_back(m_image_boundary_color); - - marker.points.push_back(pt_lb); - marker.points.push_back(pt_rb); - marker.colors.push_back(m_image_boundary_color); - marker.colors.push_back(m_image_boundary_color); - - marker.points.push_back(pt_rb); - marker.points.push_back(pt_rt); - marker.colors.push_back(m_image_boundary_color); - marker.colors.push_back(m_image_boundary_color); - - marker.points.push_back(pt_rt); - marker.points.push_back(pt_lt); - marker.colors.push_back(m_image_boundary_color); - marker.colors.push_back(m_image_boundary_color); - - // top-left indicator - marker.points.push_back(pt_lt0); - marker.points.push_back(pt_lt1); - marker.colors.push_back(m_image_boundary_color); - marker.colors.push_back(m_image_boundary_color); - - marker.points.push_back(pt_lt1); - marker.points.push_back(pt_lt2); - marker.colors.push_back(m_image_boundary_color); - marker.colors.push_back(m_image_boundary_color); - - // optical center connector - marker.points.push_back(pt_lt); - marker.points.push_back(pt_oc); - marker.colors.push_back(m_optical_center_connector_color); - marker.colors.push_back(m_optical_center_connector_color); - - - marker.points.push_back(pt_lb); - marker.points.push_back(pt_oc); - marker.colors.push_back(m_optical_center_connector_color); - marker.colors.push_back(m_optical_center_connector_color); - - marker.points.push_back(pt_rt); - marker.points.push_back(pt_oc); - marker.colors.push_back(m_optical_center_connector_color); - marker.colors.push_back(m_optical_center_connector_color); - - marker.points.push_back(pt_rb); - marker.points.push_back(pt_oc); - marker.colors.push_back(m_optical_center_connector_color); - marker.colors.push_back(m_optical_center_connector_color); - - m_markers.push_back(marker); +void CameraPoseVisualization::addPose(const Eigen::Vector3d& p, + const Eigen::Quaterniond& q) { + visualization_msgs::Marker marker; + + marker.ns = m_marker_ns; + marker.id = m_markers.size() + 1; + marker.type = visualization_msgs::Marker::LINE_STRIP; + marker.action = visualization_msgs::Marker::ADD; + marker.scale.x = m_line_width; + + marker.pose.position.x = 0.0; + marker.pose.position.y = 0.0; + marker.pose.position.z = 0.0; + marker.pose.orientation.w = 1.0; + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + + geometry_msgs::Point pt_lt, pt_lb, pt_rt, pt_rb, pt_oc, pt_lt0, pt_lt1, + pt_lt2; + + Eigen2Point(q * (m_scale * imlt) + p, pt_lt); + Eigen2Point(q * (m_scale * imlb) + p, pt_lb); + Eigen2Point(q * (m_scale * imrt) + p, pt_rt); + Eigen2Point(q * (m_scale * imrb) + p, pt_rb); + Eigen2Point(q * (m_scale * lt0) + p, pt_lt0); + Eigen2Point(q * (m_scale * lt1) + p, pt_lt1); + Eigen2Point(q * (m_scale * lt2) + p, pt_lt2); + Eigen2Point(q * (m_scale * oc) + p, pt_oc); + + // image boundaries + marker.points.push_back(pt_lt); + marker.points.push_back(pt_lb); + marker.colors.push_back(m_image_boundary_color); + marker.colors.push_back(m_image_boundary_color); + + marker.points.push_back(pt_lb); + marker.points.push_back(pt_rb); + marker.colors.push_back(m_image_boundary_color); + marker.colors.push_back(m_image_boundary_color); + + marker.points.push_back(pt_rb); + marker.points.push_back(pt_rt); + marker.colors.push_back(m_image_boundary_color); + marker.colors.push_back(m_image_boundary_color); + + marker.points.push_back(pt_rt); + marker.points.push_back(pt_lt); + marker.colors.push_back(m_image_boundary_color); + marker.colors.push_back(m_image_boundary_color); + + // top-left indicator + marker.points.push_back(pt_lt0); + marker.points.push_back(pt_lt1); + marker.colors.push_back(m_image_boundary_color); + marker.colors.push_back(m_image_boundary_color); + + marker.points.push_back(pt_lt1); + marker.points.push_back(pt_lt2); + marker.colors.push_back(m_image_boundary_color); + marker.colors.push_back(m_image_boundary_color); + + // optical center connector + marker.points.push_back(pt_lt); + marker.points.push_back(pt_oc); + marker.colors.push_back(m_optical_center_connector_color); + marker.colors.push_back(m_optical_center_connector_color); + + marker.points.push_back(pt_lb); + marker.points.push_back(pt_oc); + marker.colors.push_back(m_optical_center_connector_color); + marker.colors.push_back(m_optical_center_connector_color); + + marker.points.push_back(pt_rt); + marker.points.push_back(pt_oc); + marker.colors.push_back(m_optical_center_connector_color); + marker.colors.push_back(m_optical_center_connector_color); + + marker.points.push_back(pt_rb); + marker.points.push_back(pt_oc); + marker.colors.push_back(m_optical_center_connector_color); + marker.colors.push_back(m_optical_center_connector_color); + + m_markers.push_back(marker); } -void CameraPoseVisualization::addPose(const Eigen::Vector3d& p, const Eigen::Quaterniond& q, const Eigen::Vector3d& color, double alpha) { - visualization_msgs::Marker marker; - std_msgs::ColorRGBA m_color; - m_color.r = color.x(); - m_color.g = color.y(); - m_color.b = color.z(); - m_color.a = alpha; - - marker.ns = m_marker_ns; - marker.id = m_markers.size() + 1; - marker.type = visualization_msgs::Marker::LINE_STRIP; - marker.action = visualization_msgs::Marker::ADD; - marker.scale.x = m_line_width; - - marker.pose.position.x = 0.0; - marker.pose.position.y = 0.0; - marker.pose.position.z = 0.0; - marker.pose.orientation.w = 1.0; - marker.pose.orientation.x = 0.0; - marker.pose.orientation.y = 0.0; - marker.pose.orientation.z = 0.0; - - - geometry_msgs::Point pt_lt, pt_lb, pt_rt, pt_rb, pt_oc, pt_lt0, pt_lt1, pt_lt2; - - Eigen2Point(q * (m_scale *imlt) + p, pt_lt); - Eigen2Point(q * (m_scale *imlb) + p, pt_lb); - Eigen2Point(q * (m_scale *imrt) + p, pt_rt); - Eigen2Point(q * (m_scale *imrb) + p, pt_rb); - Eigen2Point(q * (m_scale *lt0 ) + p, pt_lt0); - Eigen2Point(q * (m_scale *lt1 ) + p, pt_lt1); - Eigen2Point(q * (m_scale *lt2 ) + p, pt_lt2); - Eigen2Point(q * (m_scale *oc ) + p, pt_oc); - - // image boundaries - marker.points.push_back(pt_lt); - marker.points.push_back(pt_lb); - marker.colors.push_back(m_color); - marker.colors.push_back(m_color); - - marker.points.push_back(pt_lb); - marker.points.push_back(pt_rb); - marker.colors.push_back(m_color); - marker.colors.push_back(m_color); - - marker.points.push_back(pt_rb); - marker.points.push_back(pt_rt); - marker.colors.push_back(m_color); - marker.colors.push_back(m_color); - - marker.points.push_back(pt_rt); - marker.points.push_back(pt_lt); - marker.colors.push_back(m_color); - marker.colors.push_back(m_color); - - // top-left indicator - marker.points.push_back(pt_lt0); - marker.points.push_back(pt_lt1); - marker.colors.push_back(m_color); - marker.colors.push_back(m_color); - - marker.points.push_back(pt_lt1); - marker.points.push_back(pt_lt2); - marker.colors.push_back(m_color); - marker.colors.push_back(m_color); - - // optical center connector - marker.points.push_back(pt_lt); - marker.points.push_back(pt_oc); - marker.colors.push_back(m_color); - marker.colors.push_back(m_color); - - - marker.points.push_back(pt_lb); - marker.points.push_back(pt_oc); - marker.colors.push_back(m_color); - marker.colors.push_back(m_color); - - marker.points.push_back(pt_rt); - marker.points.push_back(pt_oc); - marker.colors.push_back(m_color); - marker.colors.push_back(m_color); - - marker.points.push_back(pt_rb); - marker.points.push_back(pt_oc); - marker.colors.push_back(m_color); - marker.colors.push_back(m_color); - - m_markers.push_back(marker); +void CameraPoseVisualization::addPose(const Eigen::Vector3d& p, + const Eigen::Quaterniond& q, + const Eigen::Vector3d& color, + double alpha) { + visualization_msgs::Marker marker; + std_msgs::ColorRGBA m_color; + m_color.r = color.x(); + m_color.g = color.y(); + m_color.b = color.z(); + m_color.a = alpha; + + marker.ns = m_marker_ns; + marker.id = m_markers.size() + 1; + marker.type = visualization_msgs::Marker::LINE_STRIP; + marker.action = visualization_msgs::Marker::ADD; + marker.scale.x = m_line_width; + + marker.pose.position.x = 0.0; + marker.pose.position.y = 0.0; + marker.pose.position.z = 0.0; + marker.pose.orientation.w = 1.0; + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + + geometry_msgs::Point pt_lt, pt_lb, pt_rt, pt_rb, pt_oc, pt_lt0, pt_lt1, + pt_lt2; + + Eigen2Point(q * (m_scale * imlt) + p, pt_lt); + Eigen2Point(q * (m_scale * imlb) + p, pt_lb); + Eigen2Point(q * (m_scale * imrt) + p, pt_rt); + Eigen2Point(q * (m_scale * imrb) + p, pt_rb); + Eigen2Point(q * (m_scale * lt0) + p, pt_lt0); + Eigen2Point(q * (m_scale * lt1) + p, pt_lt1); + Eigen2Point(q * (m_scale * lt2) + p, pt_lt2); + Eigen2Point(q * (m_scale * oc) + p, pt_oc); + + // image boundaries + marker.points.push_back(pt_lt); + marker.points.push_back(pt_lb); + marker.colors.push_back(m_color); + marker.colors.push_back(m_color); + + marker.points.push_back(pt_lb); + marker.points.push_back(pt_rb); + marker.colors.push_back(m_color); + marker.colors.push_back(m_color); + + marker.points.push_back(pt_rb); + marker.points.push_back(pt_rt); + marker.colors.push_back(m_color); + marker.colors.push_back(m_color); + + marker.points.push_back(pt_rt); + marker.points.push_back(pt_lt); + marker.colors.push_back(m_color); + marker.colors.push_back(m_color); + + // top-left indicator + marker.points.push_back(pt_lt0); + marker.points.push_back(pt_lt1); + marker.colors.push_back(m_color); + marker.colors.push_back(m_color); + + marker.points.push_back(pt_lt1); + marker.points.push_back(pt_lt2); + marker.colors.push_back(m_color); + marker.colors.push_back(m_color); + + // optical center connector + marker.points.push_back(pt_lt); + marker.points.push_back(pt_oc); + marker.colors.push_back(m_color); + marker.colors.push_back(m_color); + + marker.points.push_back(pt_lb); + marker.points.push_back(pt_oc); + marker.colors.push_back(m_color); + marker.colors.push_back(m_color); + + marker.points.push_back(pt_rt); + marker.points.push_back(pt_oc); + marker.colors.push_back(m_color); + marker.colors.push_back(m_color); + + marker.points.push_back(pt_rb); + marker.points.push_back(pt_oc); + marker.colors.push_back(m_color); + marker.colors.push_back(m_color); + + m_markers.push_back(marker); } +void CameraPoseVisualization::reset() { m_markers.clear(); } -void CameraPoseVisualization::reset() { - m_markers.clear(); -} +void CameraPoseVisualization::publishBy(ros::Publisher& pub, + const std_msgs::Header& header) { + visualization_msgs::MarkerArray markerArray_msg; -void CameraPoseVisualization::publishBy( ros::Publisher &pub, const std_msgs::Header &header ) { - visualization_msgs::MarkerArray markerArray_msg; - - for(auto& marker : m_markers) { - marker.header = header; - markerArray_msg.markers.push_back(marker); - } + for (auto& marker : m_markers) { + marker.header = header; + markerArray_msg.markers.push_back(marker); + } - pub.publish(markerArray_msg); + pub.publish(markerArray_msg); } -} \ No newline at end of file +} // namespace D2VINS \ No newline at end of file diff --git a/d2vins/src/visualization/visualization.cpp b/d2vins/src/visualization/visualization.cpp index d8046001..4fe18c02 100644 --- a/d2vins/src/visualization/visualization.cpp +++ b/d2vins/src/visualization/visualization.cpp @@ -1,205 +1,236 @@ #include "visualization.hpp" + #include #include +#include + +#include + #include "../estimator/d2estimator.hpp" #include "CameraPoseVisualization.h" -#include -#include +#include "spdlog/spdlog.h" namespace D2VINS { -sensor_msgs::PointCloud toPointCloud(const std::vector landmarks, bool use_raw_color = false); +sensor_msgs::PointCloud toPointCloud( + const std::vector landmarks, + bool use_raw_color = false); std::vector D2Visualization::drone_colors{ - Vector3d(1, 1, 0), //drone 0 yellow - Vector3d(1, 0, 0), //drone 1 red - Vector3d(0, 1, 0), //drone 2 green - Vector3d(0, 0, 1), //drone 3 blue - Vector3d(0, 1, 1), //drone 4 cyan - Vector3d(1, 0, 1), //drone 5 magenta - Vector3d(1, 1, 1), //drone 6 white - Vector3d(0, 0, 0), //drone 7 black - Vector3d(0.5, 0.5, 0.5), //drone 8 gray - Vector3d(0.5, 0, 0), //drone 9 orange - Vector3d(0, 0.5, 0), //drone 10 green - Vector3d(0.1, 0.1, 0.5), //drone 11 blue - Vector3d(0.5, 0, 0.5), //drone 12 purple - Vector3d(0.5, 0.5, 0), //drone 13 orange - Vector3d(0, 0.5, 0.5), //drone 14 cyan - Vector3d(0.5, 0.5, 0.5) //drone 15 white + Vector3d(1, 1, 0), // drone 0 yellow + Vector3d(1, 0, 0), // drone 1 red + Vector3d(0, 1, 0), // drone 2 green + Vector3d(0, 0, 1), // drone 3 blue + Vector3d(0, 1, 1), // drone 4 cyan + Vector3d(1, 0, 1), // drone 5 magenta + Vector3d(1, 1, 1), // drone 6 white + Vector3d(0, 0, 0), // drone 7 black + Vector3d(0.5, 0.5, 0.5), // drone 8 gray + Vector3d(0.5, 0, 0), // drone 9 orange + Vector3d(0, 0.5, 0), // drone 10 green + Vector3d(0.1, 0.1, 0.5), // drone 11 blue + Vector3d(0.5, 0, 0.5), // drone 12 purple + Vector3d(0.5, 0.5, 0), // drone 13 orange + Vector3d(0, 0.5, 0.5), // drone 14 cyan + Vector3d(0.5, 0.5, 0.5) // drone 15 white }; -D2Visualization::D2Visualization() -{} +D2Visualization::D2Visualization() {} -void D2Visualization::init(ros::NodeHandle & nh, D2Estimator * estimator) { - pcl_pub = nh.advertise("point_cloud", 1000); - margined_pcl = nh.advertise("margined_cloud", 1000); - odom_pub = nh.advertise("odometry", 1000); - imu_prop_pub = nh.advertise("imu_propagation", 1000); - path_pub = nh.advertise("path", 1000); - sld_win_pub = nh.advertise("slding_window", 1000); - cam_pub = nh.advertise("camera_visual", 1000); - frame_pub_local = nh.advertise("frame_local", 1000); - frame_pub_remote = nh.advertise("frame_remote", 1000); - for (int i = 0; i < estimator->getState().localCameraExtrinsics().size(); i++) { - char topic_name[64] = {0}; - sprintf(topic_name, "camera_pose_%d", i); - camera_pose_pubs.emplace_back(nh.advertise(topic_name, 1000)); - } - _estimator = estimator; - _nh = &nh; +void D2Visualization::init(ros::NodeHandle& nh, D2Estimator* estimator) { + pcl_pub = nh.advertise("point_cloud", 1000); + margined_pcl = nh.advertise("margined_cloud", 1000); + odom_pub = nh.advertise("odometry", 1000); + imu_prop_pub = nh.advertise("imu_propagation", 1000); + path_pub = nh.advertise("path", 1000); + sld_win_pub = + nh.advertise("slding_window", 1000); + cam_pub = + nh.advertise("camera_visual", 1000); + frame_pub_local = nh.advertise("frame_local", 1000); + frame_pub_remote = nh.advertise("frame_remote", 1000); + for (int i = 0; i < estimator->getState().localCameraExtrinsics().size(); + i++) { + char topic_name[64] = {0}; + sprintf(topic_name, "camera_pose_%d", i); + camera_pose_pubs.emplace_back( + nh.advertise(topic_name, 1000)); + } + _estimator = estimator; + _nh = &nh; } -void D2Visualization::pubIMUProp(const Swarm::Odometry & odom) { - imu_prop_pub.publish(odom.toRos()); +void D2Visualization::pubIMUProp(const Swarm::Odometry& odom) { + imu_prop_pub.publish(odom.toRos()); } -void D2Visualization::pubOdometry(int drone_id, const Swarm::Odometry & odom) { - auto odom_ros = odom.toRos(); - if (paths.find(drone_id) != paths.end() && (odom_ros.header.stamp - paths[drone_id].header.stamp).toSec() < 1e-3) { - return; - } - auto & path = paths[drone_id]; - if (odom_pubs.find(drone_id) == odom_pubs.end()) { - odom_pubs[drone_id] = _nh->advertise("odometry_" + std::to_string(drone_id), 1000); - path_pubs[drone_id] = _nh->advertise("path_" + std::to_string(drone_id), 1000); - csv_output_files[drone_id] = std::ofstream(params->output_folder + "/d2vins_" + std::to_string(drone_id) + ".csv", std::ios::out); - } - geometry_msgs::PoseStamped pose_stamped; - pose_stamped.header = odom_ros.header; - pose_stamped.header.frame_id = "world"; - pose_stamped.pose = odom_ros.pose.pose; - path.header = odom_ros.header; - path.header.frame_id = "world"; - path.poses.push_back(pose_stamped); - - if (drone_id == params->self_id) { - CameraPoseVisualization camera_visual; - YAML::Node output_params; - path_pub.publish(path); - odom_pub.publish(odom_ros); - tf::Transform transform = odom.toTF(); - br.sendTransform(tf::StampedTransform(transform, odom_ros.header.stamp, "world", "imu")); - auto & state = _estimator->getState(); - auto exts = state.localCameraExtrinsics(); - for (int i = 0; i < exts.size(); i ++) { - auto camera_pose = exts[i]; - auto pose = odom.pose()*camera_pose; - geometry_msgs::PoseStamped camera_pose_ros; - camera_pose_ros.header = odom_ros.header; - camera_pose_ros.header.frame_id = "world"; - camera_pose_ros.pose = pose.toROS(); - camera_pose_pubs[i].publish(camera_pose_ros); - camera_visual.addPose(pose.pos(), pose.att(), Vector3d(0.1, 0.1, 0.5), display_alpha); - Eigen::Matrix4d eigen_T = camera_pose.toMatrix(); - cv::Mat cv_T; - cv::eigen2cv(eigen_T, cv_T); - std::ofstream ofs(params->output_folder + "/extrinsic" + std::to_string(i) + ".csv", std::ios::out); - ofs << "body_T_cam" << std::to_string(i) << std::endl << cv_T ; - ofs.close(); - std::vector> output_data; - for (auto k = 0; k < 4; k++) { - std::vector row; - for (auto j = 0; j < 4; j++) { - row.push_back(cv_T.at(k, j)); - } - output_data.push_back(row); - } - output_params["body_T_cam" + std::to_string(i)] = output_data; +void D2Visualization::pubOdometry(int drone_id, const Swarm::Odometry& odom) { + if (!_estimator->isInitialized()) { + return; + } + auto odom_ros = odom.toRos(); + if (paths.find(drone_id) != paths.end() && + (odom_ros.header.stamp - paths[drone_id].header.stamp).toSec() < 1e-3) { + return; + } + auto& path = paths[drone_id]; + if (odom_pubs.find(drone_id) == odom_pubs.end()) { + odom_pubs[drone_id] = _nh->advertise( + "odometry_" + std::to_string(drone_id), 1000); + path_pubs[drone_id] = _nh->advertise( + "path_" + std::to_string(drone_id), 1000); + csv_output_files[drone_id] = std::ofstream( + params->output_folder + "/d2vins_" + std::to_string(drone_id) + ".csv", + std::ios::out); + } + geometry_msgs::PoseStamped pose_stamped; + pose_stamped.header = odom_ros.header; + pose_stamped.header.frame_id = "world"; + pose_stamped.pose = odom_ros.pose.pose; + path.header = odom_ros.header; + path.header.frame_id = "world"; + path.poses.push_back(pose_stamped); + + if (drone_id == params->self_id) { + CameraPoseVisualization camera_visual; + YAML::Node output_params; + path_pub.publish(path); + odom_pub.publish(odom_ros); + tf::Transform transform = odom.toTF(); + br.sendTransform( + tf::StampedTransform(transform, odom_ros.header.stamp, "world", "imu")); + auto& state = _estimator->getState(); + auto exts = state.localCameraExtrinsics(); + for (int i = 0; i < exts.size(); i++) { + auto camera_pose = exts[i]; + auto pose = odom.pose() * camera_pose; + geometry_msgs::PoseStamped camera_pose_ros; + camera_pose_ros.header = odom_ros.header; + camera_pose_ros.header.frame_id = "world"; + camera_pose_ros.pose = pose.toROS(); + camera_pose_pubs[i].publish(camera_pose_ros); + camera_visual.addPose(pose.pos(), pose.att(), Vector3d(0.1, 0.1, 0.5), + display_alpha); + Eigen::Matrix4d eigen_T = camera_pose.toMatrix(); + cv::Mat cv_T; + cv::eigen2cv(eigen_T, cv_T); + std::ofstream ofs( + params->output_folder + "/extrinsic" + std::to_string(i) + ".csv", + std::ios::out); + ofs << "body_T_cam" << std::to_string(i) << std::endl << cv_T; + ofs.close(); + std::vector> output_data; + for (auto k = 0; k < 4; k++) { + std::vector row; + for (auto j = 0; j < 4; j++) { + row.push_back(cv_T.at(k, j)); } - output_params["drone_id"] = drone_id; - output_params["td"] = state.getTd(drone_id); - std::ofstream ofs(params->output_folder + "/extrinsic.yaml", std::ios::out); - ofs << output_params << std::endl; - camera_visual.publishBy(cam_pub, odom_ros.header); + output_data.push_back(row); + } + output_params["body_T_cam" + std::to_string(i)] = output_data; } - //Write output_params to yaml - csv_output_files[drone_id] << std::setprecision(std::numeric_limits::digits10 + 1) << odom.stamp << " " << odom.pose().toStr(true) << std::endl; - odom_pubs[drone_id].publish(odom_ros); - path_pubs[drone_id].publish(path); + output_params["drone_id"] = drone_id; + output_params["td"] = state.getTd(drone_id); + std::ofstream ofs(params->output_folder + "/extrinsic.yaml", std::ios::out); + ofs << output_params << std::endl; + camera_visual.publishBy(cam_pub, odom_ros.header); + } + // Write output_params to yaml + csv_output_files[drone_id] + << std::setprecision(std::numeric_limits::digits10 + 1) + << odom.stamp << " " << odom.pose().toStr(true) << std::endl; + odom_pubs[drone_id].publish(odom_ros); + path_pubs[drone_id].publish(path); } void D2Visualization::pubFrame(D2Common::VINSFrame* frame) { - if (frame == nullptr) { - return; - } - //Publish the VINSFrame - if (frame->drone_id == params->self_id) { - auto exts = _estimator->getState().localCameraExtrinsics(); - swarm_msgs::VIOFrame msg = frame->toROS(exts); - frame_pub_local.publish(msg); - } else { - swarm_msgs::VIOFrame msg = frame->toROS(); - frame_pub_remote.publish(msg); - } - pubOdometry(frame->drone_id, frame->odom); + if (frame == nullptr) { + return; + } + // Publish the VINSFrame + if (frame->drone_id == params->self_id) { + auto exts = _estimator->getState().localCameraExtrinsics(); + swarm_msgs::VIOFrame msg = frame->toROS(exts); + frame_pub_local.publish(msg); + } else { + swarm_msgs::VIOFrame msg = frame->toROS(); + frame_pub_remote.publish(msg); + } + pubOdometry(frame->drone_id, frame->odom); } void D2Visualization::postSolve() { - D2Common::Utility::TicToc tic; - auto & state = _estimator->getState(); - state.lock_state(); - auto pcl = state.getInitializedLandmarks(); - pcl_pub.publish(toPointCloud(pcl)); - margined_pcl.publish(toPointCloud(_estimator->getMarginedLandmarks(), true)); + D2Common::Utility::TicToc tic; + if (!_estimator->isInitialized()) { + return; + } + auto& state = _estimator->getState(); + state.lock_state(); + auto pcl = state.getInitializedLandmarks(); + pcl_pub.publish(toPointCloud(pcl)); + margined_pcl.publish(toPointCloud(_estimator->getMarginedLandmarks(), true)); - for (auto drone_id: state.availableDrones()) { - if (state.size(drone_id) == 0) { - continue; - } - auto odom = _estimator->getOdometry(drone_id); - pubOdometry(drone_id, odom); + for (auto drone_id : state.availableDrones()) { + if (state.size(drone_id) == 0) { + continue; } + auto odom = _estimator->getOdometry(drone_id); + pubOdometry(drone_id, odom); + } - CameraPoseVisualization sld_win_visual; - for (auto drone_id: state.availableDrones()) { - for (int i = 0; i < state.size(drone_id); i ++) { - auto & frame = state.getFrame(drone_id, i); - CamIdType camera_id = *state.getAvailableCameraIds().begin(); - auto frame_pose = frame.odom.pose(); - auto cam_pose = frame.odom.pose()*state.getExtrinsic(camera_id); - sld_win_visual.addPose(cam_pose.pos(), cam_pose.att(), drone_colors[drone_id], display_alpha); - } + CameraPoseVisualization sld_win_visual; + for (auto drone_id : state.availableDrones()) { + for (int i = 0; i < state.size(drone_id); i++) { + auto& frame = state.getFrame(drone_id, i); + CamIdType camera_id = *state.getAvailableCameraIds().begin(); + auto frame_pose = frame.odom.pose(); + auto cam_pose = frame.odom.pose() * state.getExtrinsic(camera_id); + sld_win_visual.addPose(cam_pose.pos(), cam_pose.att(), + drone_colors[drone_id], display_alpha); } - std_msgs::Header header; - header.frame_id = "world"; - header.stamp = ros::Time::now(); - sld_win_visual.publishBy(sld_win_pub, header); - state.unlock_state(); - // printf("[D2VIZ::postSolve] time cost %.1fms\n", tic.toc()); + } + std_msgs::Header header; + header.frame_id = "world"; + header.stamp = ros::Time::now(); + sld_win_visual.publishBy(sld_win_pub, header); + state.unlock_state(); + // printf("[D2VIZ::postSolve] time cost %.1fms\n", tic.toc()); } -sensor_msgs::PointCloud toPointCloud(const std::vector landmarks, bool use_raw_color) { - sensor_msgs::PointCloud pcl; - pcl.header.frame_id = "world"; - pcl.points.resize(landmarks.size()); - pcl.channels.resize(3); - pcl.channels[0].name = "rgb"; - pcl.channels[0].values.resize(landmarks.size()); - for (int i = 0; i < landmarks.size(); i++) { - pcl.points[i].x = landmarks[i].position.x(); - pcl.points[i].y = landmarks[i].position.y(); - pcl.points[i].z = landmarks[i].position.z(); - Vector3i color(255, 255, 0.); - if (use_raw_color) { - color = Vector3i(landmarks[i].color[2], landmarks[i].color[1], landmarks[i].color[0]); - } else { - // if (landmarks[i].flag == D2Common::LandmarkFlag::ESTIMATED) { - // //set color to green - // color = Vector3i(0, 255, 0.); - // } else if (landmarks[i].flag == D2Common::LandmarkFlag::OUTLIER) { - // //set color to gray - // color = Vector3i(200, 200, 200.); - // } - //Set color with drone id - color = (D2Visualization::drone_colors[landmarks[i].track[0].drone_id]*255).template cast(); - } - uint32_t hex_r = (0xff & color(0)) << 16; - uint32_t hex_g = (0xff & color(1)) << 8; - uint32_t hex_b = (0xff & color(2)); - uint32_t hex_rgb = hex_r | hex_g | hex_b; - memcpy(pcl.channels[0].values.data() + i, &hex_rgb, sizeof(float)); +sensor_msgs::PointCloud toPointCloud( + const std::vector landmarks, bool use_raw_color) { + sensor_msgs::PointCloud pcl; + pcl.header.frame_id = "world"; + pcl.points.resize(landmarks.size()); + pcl.channels.resize(3); + pcl.channels[0].name = "rgb"; + pcl.channels[0].values.resize(landmarks.size()); + for (int i = 0; i < landmarks.size(); i++) { + pcl.points[i].x = landmarks[i].position.x(); + pcl.points[i].y = landmarks[i].position.y(); + pcl.points[i].z = landmarks[i].position.z(); + Vector3i color(255, 255, 0.); + if (use_raw_color) { + color = Vector3i(landmarks[i].color[2], landmarks[i].color[1], + landmarks[i].color[0]); + } else { + // if (landmarks[i].flag == D2Common::LandmarkFlag::ESTIMATED) { + // //set color to green + // color = Vector3i(0, 255, 0.); + // } else if (landmarks[i].flag == D2Common::LandmarkFlag::OUTLIER) { + // //set color to gray + // color = Vector3i(200, 200, 200.); + // } + // Set color with drone id + color = + (D2Visualization::drone_colors[landmarks[i].track[0].drone_id] * 255) + .template cast(); } - return pcl; + uint32_t hex_r = (0xff & color(0)) << 16; + uint32_t hex_g = (0xff & color(1)) << 8; + uint32_t hex_b = (0xff & color(2)); + uint32_t hex_rgb = hex_r | hex_g | hex_b; + memcpy(pcl.channels[0].values.data() + i, &hex_rgb, sizeof(float)); + } + return pcl; } -} \ No newline at end of file +} // namespace D2VINS \ No newline at end of file diff --git a/data_analysis/MIT_single.ipynb b/data_analysis/MIT_single.ipynb new file mode 100644 index 00000000..c11c7387 --- /dev/null +++ b/data_analysis/MIT_single.ipynb @@ -0,0 +1,242 @@ +{ + "cells": [ + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "%matplotlib inline\n", + "%load_ext autoreload\n", + "%autoreload 2\n", + "from local_plot import *\n", + "from utils import *" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "path_gt = \"/home/xuhao/data/d2slam/mit_campus_hybrid/acl_jackal_gt_odom.csv\"\n", + "# path_odom = \"/home/xuhao/data/d2slam/mit_campus_hybrid/12_08_acl_jackal-vins-mono.csv\"\n", + "# path_pgo = \"/home/xuhao/data/d2slam/mit_campus_hybrid/12_08_acl_jackal-vins-mono.csv\"\n", + "\n", + "path_odom = \"/home/xuhao/data/d2slam/mit_campus_hybrid/outputs/d2slam/swarm1/d2vins_1.csv\"\n", + "path_pgo = \"/home/xuhao/data/d2slam/mit_campus_hybrid/outputs/d2slam/swarm1/pgo_1.csv\"\n", + "\n", + "end_time = 420\n", + "traj_gt, t0 = read_path_from_csv(path_gt, delimiter=\",\", time_multiplier=1e-9, dte=end_time)\n", + "traj_odom, _ = read_path_from_csv(path_odom, t0=t0-30, dte=end_time)\n", + "traj_pgo, _ = read_path_from_csv(path_pgo, t0=t0-30, dte=end_time)\n", + "align_path_by_minimize(traj_odom, traj_gt, True)\n", + "align_path_by_minimize(traj_pgo, traj_gt, True)\n", + "plot_fused([1], {1: traj_odom}, {1: traj_gt}, {1:traj_pgo}, figsize=(10,10))\n", + "plot_fused_err([1], {1: traj_odom}, {1: traj_gt}, {1: traj_odom}, {1:traj_pgo}, show=False)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "path_gt = \"/home/xuhao/data/d2slam/mit_campus_hybrid/acl_jackal2_gt_odom.csv\"\n", + "# path_odom = \"/home/xuhao/data/d2slam/mit_campus_hybrid/12_08_acl_jackal2-vins-mono.csv\"\n", + "# path_pgo = \"/home/xuhao/data/d2slam/mit_campus_hybrid/12_08_acl_jackal2-vins-mono.csv\"\n", + "\n", + "path_odom = \"/home/xuhao/data/d2slam/mit_campus_hybrid/outputs/d2vins_1.csv\"\n", + "path_pgo = \"/home/xuhao/data/d2slam/mit_campus_hybrid/outputs/pgo_1.csv\"\n", + "\n", + "end_time = 1500\n", + "t0 = None\n", + "# traj_gt, t0 = read_path_from_csv(path_gt, delimiter=\",\", time_multiplier=1e-9, dte=end_time)\n", + "# t0 = t0 + 300\n", + "traj_odom, _ = read_path_from_csv(path_odom, t0=t0, dte=end_time)\n", + "traj_pgo, _ = read_path_from_csv(path_pgo, t0=t0, dte=end_time)\n", + "\n", + "plot_fused([2], {2: traj_odom}, None, {2:traj_pgo}, figsize=(10,10))\n", + "# align_path_by_minimize(traj_odom, traj_gt, True)\n", + "# align_path_by_minimize(traj_pgo, traj_gt, True)\n", + "# plot_fused([1], {1: traj_odom}, {1: traj_gt}, {1:traj_pgo}, figsize=(10,10))\n", + "# plot_fused_err([1], {1: traj_odom}, {1: traj_gt}, {1: traj_odom}, {1:traj_pgo}, show=False)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "path_gt = \"/home/xuhao/data/d2slam/mit_campus_outdoor/acl_jackal2_gt_odom.csv\"\n", + "# path_odom = \"/home/xuhao/data/d2slam/mit_campus_hybrid/12_08_acl_jackal2-vins-mono.csv\"\n", + "# path_pgo = \"/home/xuhao/data/d2slam/mit_campus_hybrid/12_08_acl_jackal2-vins-mono.csv\"\n", + "\n", + "path_odom = \"/home/xuhao/data/d2slam/mit_campus_hybrid/outputs/d2vins_1.csv\"\n", + "path_pgo = \"/home/xuhao/data/d2slam/mit_campus_hybrid/outputs/pgo_1.csv\"\n", + "\n", + "end_time = 1500\n", + "t0 = None\n", + "traj_gt, t0 = read_path_from_csv(path_gt, delimiter=\",\", time_multiplier=1e-9, dte=end_time)\n", + "traj_odom, _ = read_path_from_csv(path_odom, t0=t0, dte=end_time)\n", + "traj_pgo, _ = read_path_from_csv(path_pgo, t0=t0, dte=end_time)\n", + "\n", + "# plot_fused([2], {2: traj_odom}, None, {2:traj_pgo}, figsize=(10,10))\n", + "align_path_by_minimize(traj_odom, traj_gt, True)\n", + "align_path_by_minimize(traj_pgo, traj_gt, True)\n", + "plot_fused([1], {1: traj_odom}, {1: traj_gt}, {1:traj_pgo}, figsize=(10,10), plot_3d=False)\n", + "plot_fused_err([1], {1: traj_odom}, {1: traj_gt}, {1: traj_odom}, {1:traj_pgo}, show=False)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "path_gt = \"/home/xuhao/data/d2slam/mit_campus_outdoor/acl_jackal_gt_odom.csv\"\n", + "\n", + "path_odom = \"/home/xuhao/data/d2slam/mit_campus_hybrid/outputs/d2vins_1.csv\"\n", + "path_pgo = \"/home/xuhao/data/d2slam/mit_campus_hybrid/outputs/pgo_1.csv\"\n", + "\n", + "end_time = 1500\n", + "t0 = None\n", + "traj_gt, t0 = read_path_from_csv(path_gt, delimiter=\",\", time_multiplier=1e-9, dte=end_time)\n", + "traj_odom, _ = read_path_from_csv(path_odom, t0=t0, dte=end_time)\n", + "traj_pgo, _ = read_path_from_csv(path_pgo, t0=t0, dte=end_time)\n", + "\n", + "# plot_fused([2], {2: traj_odom}, None, {2:traj_pgo}, figsize=(10,10))\n", + "align_path_by_minimize(traj_odom, traj_gt, True)\n", + "align_path_by_minimize(traj_pgo, traj_gt, True)\n", + "plot_fused([1], {1: traj_odom}, {1: traj_gt}, {1:traj_pgo}, figsize=(10,10), plot_3d=False)\n", + "plot_fused_err([1], {1: traj_odom}, {1: traj_gt}, {1: traj_odom}, {1:traj_pgo}, show=False)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "path_gt = \"/home/xuhao/data/d2slam/mit_campus_outdoor/hathor_gt_odom.csv\"\n", + "\n", + "path_odom = \"/home/xuhao/data/d2slam/mit_campus_hybrid/outputs/d2vins_1.csv\"\n", + "path_pgo = \"/home/xuhao/data/d2slam/mit_campus_hybrid/outputs/pgo_1.csv\"\n", + "\n", + "end_time = 1500\n", + "t0 = None\n", + "traj_gt, t0 = read_path_from_csv(path_gt, delimiter=\",\", time_multiplier=1e-9, dte=end_time)\n", + "traj_odom, _ = read_path_from_csv(path_odom, t0=t0, dte=end_time)\n", + "traj_pgo, _ = read_path_from_csv(path_pgo, t0=t0, dte=end_time)\n", + "\n", + "# plot_fused([2], {2: traj_odom}, None, {2:traj_pgo}, figsize=(10,10))\n", + "align_path_by_minimize(traj_odom, traj_gt, True)\n", + "align_path_by_minimize(traj_pgo, traj_gt, True)\n", + "plot_fused([1], {1: traj_odom}, {1: traj_gt}, {1:traj_pgo}, figsize=(10,10), plot_3d=False)\n", + "plot_fused_err([1], {1: traj_odom}, {1: traj_gt}, {1: traj_odom}, {1:traj_pgo}, show=False)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "path_gt = \"/home/xuhao/data/d2slam/mit_campus_outdoor/sparkal1_gt_odom.csv\"\n", + "\n", + "path_odom = \"/home/xuhao/data/d2slam/mit_campus_hybrid/outputs/d2vins_1.csv\"\n", + "path_pgo = \"/home/xuhao/data/d2slam/mit_campus_hybrid/outputs/pgo_1.csv\"\n", + "\n", + "end_time = 1500\n", + "t0 = None\n", + "traj_gt, t0 = read_path_from_csv(path_gt, delimiter=\",\", time_multiplier=1e-9, dte=end_time)\n", + "traj_odom, _ = read_path_from_csv(path_odom, t0=t0, dte=end_time)\n", + "traj_pgo, _ = read_path_from_csv(path_pgo, t0=t0, dte=end_time)\n", + "\n", + "# plot_fused([2], {2: traj_odom}, None, {2:traj_pgo}, figsize=(10,10))\n", + "align_path_by_minimize(traj_odom, traj_gt, True)\n", + "align_path_by_minimize(traj_pgo, traj_gt, True)\n", + "plot_fused([1], {1: traj_odom}, {1: traj_gt}, {1:traj_pgo}, figsize=(10,10), plot_3d=False)\n", + "plot_fused_err([1], {1: traj_odom}, {1: traj_gt}, {1: traj_odom}, {1:traj_pgo}, show=False)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "path_gt = \"/home/xuhao/data/d2slam/mit_campus_outdoor/sparkal2_gt_odom.csv\"\n", + "\n", + "path_odom = \"/home/xuhao/data/d2slam/mit_campus_hybrid/outputs/d2vins_1.csv\"\n", + "path_pgo = \"/home/xuhao/data/d2slam/mit_campus_hybrid/outputs/pgo_1.csv\"\n", + "\n", + "end_time = 1500\n", + "t0 = None\n", + "traj_gt, t0 = read_path_from_csv(path_gt, delimiter=\",\", time_multiplier=1e-9, dte=end_time)\n", + "traj_odom, _ = read_path_from_csv(path_odom, t0=t0, dte=end_time)\n", + "traj_pgo, _ = read_path_from_csv(path_pgo, t0=t0, dte=end_time)\n", + "\n", + "# plot_fused([2], {2: traj_odom}, None, {2:traj_pgo}, figsize=(10,10))\n", + "align_path_by_minimize(traj_odom, traj_gt, True)\n", + "align_path_by_minimize(traj_pgo, traj_gt, True)\n", + "plot_fused([1], {1: traj_odom}, {1: traj_gt}, {1:traj_pgo}, figsize=(10,10), plot_3d=False)\n", + "plot_fused_err([1], {1: traj_odom}, {1: traj_gt}, {1: traj_odom}, {1:traj_pgo}, show=False)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "path_gt = \"/home/xuhao/data/d2slam/mit_campus_outdoor/thoth_gt_odom.csv\"\n", + "\n", + "path_odom = \"/home/xuhao/data/d2slam/mit_campus_hybrid/outputs/d2vins_1.csv\"\n", + "path_pgo = \"/home/xuhao/data/d2slam/mit_campus_hybrid/outputs/pgo_1.csv\"\n", + "\n", + "end_time = 1500\n", + "t0 = None\n", + "traj_gt, t0 = read_path_from_csv(path_gt, delimiter=\",\", time_multiplier=1e-9, dte=end_time)\n", + "traj_odom, _ = read_path_from_csv(path_odom, t0=t0, dte=end_time)\n", + "traj_pgo, _ = read_path_from_csv(path_pgo, t0=t0, dte=end_time)\n", + "\n", + "# plot_fused([2], {2: traj_odom}, None, {2:traj_pgo}, figsize=(10,10))\n", + "align_path_by_minimize(traj_odom, traj_gt, True)\n", + "align_path_by_minimize(traj_pgo, traj_gt, True)\n", + "plot_fused([1], {1: traj_odom}, {1: traj_gt}, {1:traj_pgo}, figsize=(10,10), plot_3d=False)\n", + "plot_fused_err([1], {1: traj_odom}, {1: traj_gt}, {1: traj_odom}, {1:traj_pgo}, show=False)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "kernelspec": { + "display_name": "base", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.8.5" + }, + "orig_nbformat": 4 + }, + "nbformat": 4, + "nbformat_minor": 2 +} diff --git a/data_analysis/graco_multi.ipynb b/data_analysis/graco_multi.ipynb new file mode 100644 index 00000000..34f93e7d --- /dev/null +++ b/data_analysis/graco_multi.ipynb @@ -0,0 +1,82 @@ +{ + "cells": [ + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "%matplotlib inline\n", + "%load_ext autoreload\n", + "%autoreload 2\n", + "from local_plot import *\n", + "from utils import *\n", + "def read(folder, nodes, dte=10000000):\n", + " paths = {}\n", + " paths_pgo = {}\n", + " t0 = None\n", + " for i in nodes:\n", + " output_folder = folder + str(i) + \"/\"\n", + " _paths, t0 = read_paths(output_folder, nodes, t0=t0, dte=dte)\n", + " _paths_pgo, t0 = read_paths(output_folder, nodes, prefix=\"pgo_\", t0=t0, dte=dte)\n", + " paths[i] = _paths[i]\n", + " paths_pgo[i] = _paths_pgo[i]\n", + " return paths, paths_pgo, t0" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "nodes = [1, 3]\n", + "output_folder = \"/home/xuhao/data/d2slam/NTU_VIRAL/outputs/d2slam/swarm\"\n", + "data_folder=\"/home/xuhao/data/d2slam/NTU_VIRAL/\"\n", + "dte = 1000\n", + "paths, paths_pgo, t0 = read(output_folder, nodes, dte=dte)\n", + "paths_gt, t0 = read_paths(data_folder, nodes, prefix=\"groundtruth_\", t0=t0)\n", + "align_paths(paths, paths_gt, align_by_first=True, align_with_minize=True)\n", + "align_paths(paths_pgo, paths_gt, align_by_first=True, align_with_minize=True)\n", + "\n", + "display(plot_fused_err(nodes, paths, paths_gt, poses_pgo=paths_pgo, dte=dte, show=False))\n", + "# display(relative_pose_err(nodes, paths, paths_gt, dte=dte, common_time_dt=0.1))\n", + "\n", + "plot_fused(nodes, paths, paths_gt, poses_pgo=paths_pgo, figsize=(15,10))" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3 (ipykernel)", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.8.5" + }, + "vscode": { + "interpreter": { + "hash": "08ce52785f0fedc81003ce387e097a83d6cc9494681cd746006386992005bb71" + } + } + }, + "nbformat": 4, + "nbformat_minor": 4 +} diff --git a/data_analysis/graco_single.ipynb b/data_analysis/graco_single.ipynb new file mode 100644 index 00000000..5a03dac2 --- /dev/null +++ b/data_analysis/graco_single.ipynb @@ -0,0 +1,106 @@ +{ + "cells": [ + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "%matplotlib inline\n", + "%load_ext autoreload\n", + "%autoreload 2\n", + "from local_plot import *\n", + "from utils import *" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "path_gt = \"/home/xuhao/data/d2slam/NTU_VIRAL/groundtruth_1.csv\"\n", + "path_odom = \"/home/xuhao/output/d2vins_1.csv\"\n", + "path_pgo = \"/home/xuhao/output/pgo_1.csv\"\n", + "# path_odom = \"/home/xuhao/data/d2slam/tum_datasets/vins-mono_1.csv\"\n", + "\n", + "traj_gt, t0 = read_path_from_csv(path_gt)\n", + "traj_odom, _ = read_path_from_csv(path_odom, t0=t0)\n", + "traj_pgo, _ = read_path_from_csv(path_pgo, t0=t0)\n", + "align_path_by_minimize(traj_odom, traj_gt, True)\n", + "align_path_by_minimize(traj_pgo, traj_gt, True)\n", + "plot_fused([1], {1: traj_odom}, {1: traj_gt}, {1:traj_pgo}, figsize=(10,10))\n", + "plot_fused_err([1], {1: traj_odom}, {1: traj_gt}, {1: traj_odom}, {1:traj_pgo}, show=True)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "path_gt = \"/home/xuhao/data/d2slam/tum_corr/groundtruth_2.csv\"\n", + "path_odom = \"/home/xuhao/output/d2vins_1.csv\"\n", + "path_pgo = \"/home/xuhao/output/pgo_1.csv\"\n", + "# path_odom = \"/home/xuhao/data/d2slam/tum_corr/vins-mono_2.csv\"\n", + "\n", + "traj_gt, t0 = read_path_from_csv(path_gt)\n", + "traj_odom, _ = read_path_from_csv(path_odom, t0=t0)\n", + "traj_pgo, _ = read_path_from_csv(path_pgo, t0=t0)\n", + "align_path_by_minimize(traj_odom, traj_gt, True)\n", + "align_path_by_minimize(traj_pgo, traj_gt, True)\n", + "plot_fused([1], {1: traj_odom}, {1: traj_gt}, {1: traj_pgo}, figsize=(10,10))\n", + "plot_fused_err([1], {1: traj_odom}, {1: traj_gt}, {1: traj_odom}, {1: traj_pgo}, show=True)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "path_gt = \"/home/xuhao/data/d2slam/NTU_VIRAL/eee_01-groundtruth.txt\"\n", + "path_odom = \"/home/xuhao/output/d2vins_1.csv\"\n", + "path_pgo = \"/home/xuhao/output/pgo_1.csv\"\n", + "# path_odom = \"/home/xuhao/data/d2slam/tum_corr/vins-mono_2.csv\"\n", + "\n", + "traj_gt, t0 = read_path_from_csv(path_gt)\n", + "traj_odom, _ = read_path_from_csv(path_odom, t0=t0)\n", + "traj_pgo, _ = read_path_from_csv(path_pgo, t0=t0)\n", + "align_path_by_minimize(traj_odom, traj_gt, True)\n", + "align_path_by_minimize(traj_pgo, traj_gt, True)\n", + "plot_fused([1], {1: traj_odom}, {1: traj_gt}, {1: traj_pgo}, figsize=(10,10))\n", + "plot_fused_err([1], {1: traj_odom}, {1: traj_gt}, {1: traj_odom}, {1: traj_pgo}, show=True)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "kernelspec": { + "display_name": "base", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.8.5" + }, + "orig_nbformat": 4 + }, + "nbformat": 4, + "nbformat_minor": 2 +} diff --git a/data_analysis/handhold_realworld.ipynb b/data_analysis/handhold_realworld.ipynb index 0112f31a..3685820e 100644 --- a/data_analysis/handhold_realworld.ipynb +++ b/data_analysis/handhold_realworld.ipynb @@ -63,7 +63,7 @@ " \n", "def extract_trackLocal_from_log(log_content):\n", " # Sample log: \n", - " # [D2FeatureTracker] frame_id: 2000072, landmark_num: 400, time_cost: 3.0ms\n", + " # frame_id: 2000072, landmark_num: 400, time_cost: 3.0ms\n", " # Match using regex\n", " pattern = re.compile(r\"\\[D2FeatureTracker\\] frame_id: \\d+, landmark_num: \\d+, time_cost: (\\d+\\.\\d+)ms\")\n", " matched = pattern.findall(log_content)\n", diff --git a/data_analysis/local_plot.py b/data_analysis/local_plot.py index de39c0dc..e1d3e44d 100755 --- a/data_analysis/local_plot.py +++ b/data_analysis/local_plot.py @@ -8,9 +8,9 @@ from utils import * from trajectory import * -def read_path_from_csv(path, t0=None, delimiter=None,dte=None, reset_orientation=False): +def read_path_from_csv(path, t0=None, delimiter=None,dte=None, reset_orientation=False, time_multiplier=1.0): arr = np.loadtxt(path, delimiter=delimiter) - t = arr[:, 0] + t = arr[:, 0] * time_multiplier if t0 is None: t0 = t[0] t = t - t0 @@ -59,49 +59,52 @@ def read_multi_folder(folder, nodes, enable_pgo=True, t0=None): return paths, None, t0 return paths, paths_pgo, t0 -def plot_fused(nodes, poses_fused, poses_gt=None, poses_pgo=None , output_path="/home/xuhao/output/", id_map = None, figsize=(6, 6), plot_each=True): - fig = plt.figure("plot3d", figsize=figsize) - ax = fig.add_subplot(111, projection='3d') - # ax = fig.gca(projection='3d') +def plot_fused(nodes, poses_fused, poses_gt=None, poses_pgo=None , output_path="/home/xuhao/output/", id_map = None, figsize=(6, 6), plot_each=True, plot_3d = True): + import matplotlib as mpl + mpl.style.use('seaborn-whitegrid') + if id_map is None: id_map = {} for i in nodes: id_map[i] = i - for i in nodes: - if poses_gt is not None: - ax.plot(poses_gt[i].pos[:,0], poses_gt[i].pos[:,1],poses_gt[i].pos[:,2], label=f"GT {i}") - if poses_pgo is not None: - ax.plot(poses_pgo[i].pos[:,0], poses_pgo[i].pos[:,1],poses_pgo[i].pos[:,2], label=f"PGO {i}") - ax.plot(poses_fused[i].pos[:,0], poses_fused[i].pos[:,1],poses_fused[i].pos[:,2], label=f"$D^2$VINS {id_map[i]}") - - ax.set_xlabel('$X$') - ax.set_ylabel('$Y$') - ax.set_zlabel('$Z$') - - plt.legend() - plt.savefig(output_path+"plot3d.png") - - #Plot Fused Vs GT 3D - fig = plt.figure("FusedVsGT3D", figsize=figsize) - # fig.suptitle("Fused Vs GT 3D") - k = 0 - for i in nodes: - _id = id_map[i] - ax = fig.add_subplot(1, len(nodes), k+1, projection='3d') - ax.set_title(f"Drone {_id}") - if poses_gt is not None: - ax.plot(poses_gt[i].pos[:,0], poses_gt[i].pos[:,1],poses_gt[i].pos[:,2], label=f"Ground Truth") - ax.plot(poses_fused[i].pos[:,0], poses_fused[i].pos[:,1],poses_fused[i].pos[:,2], label=f"$D^2$VINS") - if poses_pgo is not None: - ax.plot(poses_pgo[i].pos[:,0], poses_pgo[i].pos[:,1],poses_pgo[i].pos[:,2], label=f"$D^2$PGO") - if i == nodes[0]: - plt.legend() + if plot_3d: + fig = plt.figure("plot3d", figsize=figsize) + ax = fig.add_subplot(111, projection='3d') + for i in nodes: + if poses_gt is not None: + ax.plot(poses_gt[i].pos[:,0], poses_gt[i].pos[:,1],poses_gt[i].pos[:,2], label=f"GT {i}") + if poses_pgo is not None: + ax.plot(poses_pgo[i].pos[:,0], poses_pgo[i].pos[:,1],poses_pgo[i].pos[:,2], label=f"PGO {i}") + ax.plot(poses_fused[i].pos[:,0], poses_fused[i].pos[:,1],poses_fused[i].pos[:,2], label=f"$D^2$VINS {id_map[i]}") + ax.set_xlabel('$X$') ax.set_ylabel('$Y$') ax.set_zlabel('$Z$') - k += 1 - plt.savefig(output_path+"FusedVsGT3D.pdf") + + plt.legend() + plt.savefig(output_path+"plot3d.png") + + #Plot Fused Vs GT 3D + fig = plt.figure("FusedVsGT3D", figsize=figsize) + # fig.suptitle("Fused Vs GT 3D") + k = 0 + for i in nodes: + _id = id_map[i] + ax = fig.add_subplot(1, len(nodes), k+1, projection='3d') + ax.set_title(f"Drone {_id}") + if poses_gt is not None: + ax.plot(poses_gt[i].pos[:,0], poses_gt[i].pos[:,1],poses_gt[i].pos[:,2], label=f"Ground Truth") + ax.plot(poses_fused[i].pos[:,0], poses_fused[i].pos[:,1],poses_fused[i].pos[:,2], label=f"$D^2$VINS") + if poses_pgo is not None: + ax.plot(poses_pgo[i].pos[:,0], poses_pgo[i].pos[:,1],poses_pgo[i].pos[:,2], label=f"$D^2$PGO") + if i == nodes[0]: + plt.legend() + ax.set_xlabel('$X$') + ax.set_ylabel('$Y$') + ax.set_zlabel('$Z$') + k += 1 + plt.savefig(output_path+"FusedVsGT3D.pdf") fig = plt.figure("Fused Multi 2d", figsize=figsize) plt.gca().set_aspect('equal') @@ -141,7 +144,8 @@ def plot_fused(nodes, poses_fused, poses_gt=None, poses_pgo=None , output_path=" ax1.plot(poses_pgo[i].t, poses_pgo[i].pos[:,0], '.', label=f"$D^2$PGO Traj{i}") ax2.plot(poses_pgo[i].t, poses_pgo[i].pos[:,1], '.', label=f"$D^2$PGO Traj{i}") ax3.plot(poses_pgo[i].t, poses_pgo[i].pos[:,2], '.', label=f"$D^2$PGO Traj{i}") - + + ax1.tick_params( axis='x', which='both', bottom=False, top=False, labelbottom=False) ax1.set_ylabel("x") ax2.tick_params( axis='x', which='both', bottom=False, top=False, labelbottom=False) @@ -149,11 +153,32 @@ def plot_fused(nodes, poses_fused, poses_gt=None, poses_pgo=None , output_path=" ax3.set_ylabel("z") ax3.set_xlabel("t") ax3.legend() - ax1.grid() - ax2.grid() - ax3.grid() plt.savefig(output_path+f"est_by_t{i}_position.png") + fig = plt.figure(f"Drone {i} fused Vs GT Vel", figsize=figsize) + fig.suptitle(f"Drone {i} fused Vs GT 1D") + ax1, ax2, ax3 = fig.subplots(3, 1) + if poses_gt is not None: + # Plot velocity + ax1.plot(poses_gt[i].t, poses_gt[i].vel[:,0], label=f"Ground Truth ${i}$", marker='.', linestyle = 'None') + ax2.plot(poses_gt[i].t, poses_gt[i].vel[:,1], label=f"Ground Truth ${i}$", marker='.', linestyle = 'None') + ax3.plot(poses_gt[i].t, poses_gt[i].vel[:,2], label=f"Ground Truth ${i}$", marker='.', linestyle = 'None') + ax1.plot(poses_fused[i].t, poses_fused[i].vel[:,0], label=f"$D^2$VINS {_id}") + ax2.plot(poses_fused[i].t, poses_fused[i].vel[:,1], label=f"$D^2$VINS {_id}") + ax3.plot(poses_fused[i].t, poses_fused[i].vel[:,2], label=f"$D^2$VINS {_id}") + if poses_pgo is not None: + ax1.plot(poses_pgo[i].t, poses_pgo[i].vel[:,0], '.', label=f"$D^2$PGO Traj{i}") + ax2.plot(poses_pgo[i].t, poses_pgo[i].vel[:,1], '.', label=f"$D^2$PGO Traj{i}") + ax3.plot(poses_pgo[i].t, poses_pgo[i].vel[:,2], '.', label=f"$D^2$PGO Traj{i}") + ax1.tick_params( axis='x', which='both', bottom=False, top=False, labelbottom=False) + ax1.set_ylabel("vx") + ax2.tick_params( axis='x', which='both', bottom=False, top=False, labelbottom=False) + ax2.set_ylabel("vy") + ax3.set_ylabel("vz") + ax3.set_xlabel("t") + ax3.legend() + plt.savefig(output_path+f"est_by_t{i}_velocity.png") + fig = plt.figure(f"Drone {i} fused Vs GT Att", figsize=figsize) ax1, ax2, ax3 = fig.subplots(3, 1) @@ -173,15 +198,12 @@ def plot_fused(nodes, poses_fused, poses_gt=None, poses_pgo=None , output_path=" ax1.set_ylabel("Yaw (deg)") ax1.set_xlabel("t") ax1.legend() - ax1.grid() ax2.set_ylabel("Pitch (deg)") ax2.set_xlabel("t") ax2.legend() - ax2.grid() ax3.set_xlabel("t") ax2.set_ylabel("Roll (deg)") ax3.legend() - ax3.grid() plt.savefig(output_path+f"est_by_t{i}_attitude.png") def plot_relative_pose_err(main_id, target_ids, poses_fused, poses_gt, poses_vo=None,outlier_thres=100, @@ -507,14 +529,14 @@ def plot_fused_err(nodes, poses_fused, poses_gt, poses_vo=None, poses_pgo=None,m rmse_angular_path = nan if poses_vo is not None: - pos_gt_vo= poses_gt[i].resample_pos(poses_vo[i].t) - ypr_gt_vo = poses_gt[i].resample_ypr(poses_vo[i].t) + pos_gt_vo= poses_gt[i].resample_pos(t_) + ypr_gt_vo = poses_gt[i].resample_ypr(t_) mask_vo = np.linalg.norm(pos_gt_vo - pos_vo, axis=1) < outlier_thres pos_vo = pos_vo[mask_vo] pos_gt_vo = pos_gt_vo[mask_vo] ypr_vo = ypr_vo[mask_vo] ypr_gt_vo = ypr_gt_vo[mask_vo] - t_vo = poses_vo[i].t[mask_vo] + t_vo = t_[mask_vo] rmse_vo_x = RMSE(pos_vo[:,0] , pos_gt_vo[:,0]) rmse_vo_y = RMSE(pos_vo[:,1] , pos_gt_vo[:,1]) rmse_vo_z = RMSE(pos_vo[:,2] , pos_gt_vo[:,2]) @@ -623,6 +645,7 @@ def plot_fused_err(nodes, poses_fused, poses_gt, poses_vo=None, poses_pgo=None,m "", f"{ate_pgo_sum/num:.3f}",f"{rmse_pgo_ang_sum/num*180/pi:3.2f}"]) if poses_pgo is None: #Remove the last two columns of output table + print("no pgo") output_table = [row[:-2] for row in output_table] if output_ATE: return ate_fused_sum/num, ate_ang_sum/num diff --git a/data_analysis/trajectory.py b/data_analysis/trajectory.py index 5f73ed71..d4f879f3 100644 --- a/data_analysis/trajectory.py +++ b/data_analysis/trajectory.py @@ -17,6 +17,14 @@ def interp(self): self.ypr_func = interp1d(self.t, self.ypr, axis=0,bounds_error=False,fill_value="extrapolate") self.ypr[:,0] = wrap_pi(self.ypr[:,0]) + # Compute velocity + dp = np.diff(self.pos, axis=0) + dt = np.diff(self.t) + self.vel = np.concatenate([np.zeros((1,3)), dp/dt[:,None]], axis=0) + # Smooth velocity with a moving average filter + self.vel = np.apply_along_axis(lambda x: np.convolve(x, np.ones(5)/5, mode='same'), 0, self.vel) + self.vel_func = interp1d(self.t, self.vel, axis=0,bounds_error=False,fill_value="extrapolate") + def length(self, t=10000000): mask = self.t < t dp = np.diff(self.pos[mask], axis=0) @@ -131,7 +139,10 @@ def odometry_covariance_per_meter_with_rp(pos_vo, yaw_vo, pos_gt, yaw_gt, rp_len # plt.plot(rp_errors) # plt.grid() plt.show() - return sqr_err_pos_per_meter/c, sqr_err_yaw_per_meter/c + if c > 0: + return sqr_err_pos_per_meter/c, sqr_err_yaw_per_meter/c + else: + return np.NaN, np.NaN def odometry_covariance_per_meter(pos_vo, yaw_vo, pos_gt, yaw_gt, rp_lengths=[1.0], gt_outlier_thres=1.0, show=False,step=100): pos_covs = [] @@ -210,7 +221,7 @@ def cost(x): dyaw = x[3] pos_err = np.linalg.norm(pos_gt - yaw_rotate_vec(dyaw, pos) - dpos, axis=1) yaw_err = np.abs(wrap_pi(ypr_gt[:,0] - ypr[:,0] - dyaw)) - return np.sum(pos_err) + np.sum(yaw_err) + return np.sum(pos_err) #+ np.sum(yaw_err) inital_pos = pos_gt[0] - pos[0] inital_yaw = wrap_pi(ypr_gt[0, 0] - ypr[0, 0]) inital_guess = np.concatenate([inital_pos, [inital_yaw]]) @@ -234,7 +245,7 @@ def cost(x): pos_err = np.linalg.norm(pos_gt - yaw_rotate_vec(dyaw, pos) - dpos, axis=1) yaw_err = np.abs(wrap_pi(ypr_gt[:,0] - ypr[:,0] - dyaw)) rp_err = np.abs(wrap_pi(ypr_gt[:,1:] - ypr[:,1:] - d_rp)) - return np.sum(pos_err) + np.sum(yaw_err) + np.sum(rp_err) + return np.sum(pos_err) #+ np.sum(yaw_err) + np.sum(rp_err) inital_pos = pos_gt[0] - pos[0] inital_yaw = wrap_pi(ypr_gt[0, 0] - ypr[0, 0]) inital_guess = np.concatenate([inital_pos, [inital_yaw, 0, 0]]) diff --git a/docker/Dockerfile.jetson b/docker/Dockerfile.jetson index de2d689a..2fdde016 100644 --- a/docker/Dockerfile.jetson +++ b/docker/Dockerfile.jetson @@ -1,4 +1,4 @@ -FROM xuhao1/d2slam:jetson_base_35.1.0 +FROM hkustswarm/d2slam:jetson_orin_base_35.3.1 ARG ROS_VERSION=noetic ENV SWARM_WS=/root/swarm_ws diff --git a/docker/Dockerfile.jetson_base_35.1.0 b/docker/Dockerfile.jetson_base_35.1.0 index 9e51fedc..79ecb025 100644 --- a/docker/Dockerfile.jetson_base_35.1.0 +++ b/docker/Dockerfile.jetson_base_35.1.0 @@ -1,10 +1,9 @@ #Ubuntu 18.04/Jetpack 5.0.2/ROS Melodic FROM nvcr.io/nvidia/l4t-jetpack:r35.1.0 ARG CERES_VERSION=2.1.0 -ARG CMAKE_VERSION=3.23.1 ARG ONNX_VERSION=1.12.1 ENV SWARM_WS=/root/swarm_ws -ARG CMAKE_VERSION=3.23.1 +ARG CMAKE_VERSION=3.24.1 ARG ONNX_VERSION=1.12.1 ARG OPENCV_VERSION=4.6.0 ARG ROS_VERSION=noetic @@ -102,14 +101,14 @@ RUN apt update && \ cudev,cudaoptflow,cudaimgproc,cudalegacy,cudaarithm,cudacodec,cudastereo,\ cudafeatures2d,xfeatures2d,tracking,stereo,\ aruco,videoio,ccalib && \ - make -j ${USE_PROC} && \ - make install + make -j$(nproc) && \ + make install RUN git clone https://github.com/HKUST-Swarm/ceres-solver -b D2SLAM && \ cd ceres-solver && \ mkdir build && cd build && \ cmake -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTING=OFF -DBUILD_EXAMPLES=OFF -DBUILD_BENCHMARKS=OFF -DCUDA=OFF .. && \ - make -j2 install && \ + make -j$(nproc) install && \ rm -rf ../../ceres-solver && \ apt-get clean all @@ -119,19 +118,19 @@ RUN git clone https://github.com/lcm-proj/lcm && \ git checkout tags/v1.4.0 && \ mkdir build && cd build && \ cmake -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTING=OFF -DBUILD_EXAMPLES=OFF -DBUILD_BENCHMARKS=OFF .. && \ - make -j${USE_PROC} install + make -j$(nproc) install #Install Faiss RUN git clone https://github.com/facebookresearch/faiss.git && \ cd faiss && git checkout tags/v${FAISS_VERSION} && \ cmake -B build -DCMAKE_BUILD_TYPE=Release -DFAISS_ENABLE_PYTHON=OFF -DBUILD_TESTING=OFF -DFAISS_ENABLE_GPU=OFF . && \ - make -C build -j faiss && \ + make -C build -j$(nproc) faiss && \ make -C build install && \ rm -rf ../faiss #Install OpenGV RUN git clone https://github.com/HKUST-Swarm/opengv && \ - mkdir opengv/build && cd opengv/build && cmake .. && make -j${USE_PROC} && \ + mkdir opengv/build && cd opengv/build && cmake .. && make -j$(nproc) && \ make install #Install Backward @@ -157,3 +156,13 @@ RUN pip3 install --no-cache https://developer.download.nvidia.com/compute/redi # Install for TaichiSLAM RUN pip install transformations numpy lcm matplotlib scipy && apt install ros-${ROS_VERSION}-ros-numpy # taichi + + + +#Install spdlog +RUN wget https://github.com/gabime/spdlog/archive/refs/tags/v1.12.0.tar.gz && \ + tar -zxvf v1.12.0.tar.gz && \ + cd spdlog-1.12.0 && \ + mkdir build && cd build && \ + cmake .. && make -j$(nproc) && \ + make install \ No newline at end of file diff --git a/docker/Dockerfile.jetson_orin_base_35.3.1 b/docker/Dockerfile.jetson_orin_base_35.3.1 new file mode 100644 index 00000000..06d20d6a --- /dev/null +++ b/docker/Dockerfile.jetson_orin_base_35.3.1 @@ -0,0 +1,168 @@ +#Ubuntu 18.04/Jetpack 5.0.2/ROS Melodic +FROM nvcr.io/nvidia/l4t-jetpack:r35.3.1 +ARG CERES_VERSION=2.1.0 +ARG ONNX_VERSION=1.13.1 +ENV SWARM_WS=/root/swarm_ws +ARG CMAKE_VERSION=3.24.1 +ARG OPENCV_VERSION=4.6.0 +ARG ROS_VERSION=noetic +#Modified if use PC +ARG USE_PROC=8 +ENV SWARM_WS=/root/swarm_ws +ENV DEBIAN_FRONTEND=noninteractive +# Only 7.2 for NX +# 5.3 for Jetson nano and 6.2 for Jetson TX2 +# ARG CUDA_ARCH_BIN="6.2,7.2,8.7" + #for orin NX +ARG CUDA_ARCH_BIN="8.7" +ARG ENABLE_NEON="ON" + +#Some basic dependencies +RUN apt-get -y update && \ + TZ=Asia/Beijing apt-get -y install tzdata && \ + apt-get install -y wget curl lsb-release git \ + libatlas-base-dev \ + libeigen3-dev \ + libgoogle-glog-dev \ + libsuitesparse-dev \ + libglib2.0-dev \ + libyaml-cpp-dev \ + net-tools \ + htop \ + xterm \ + gdb \ + zip \ + unzip \ + libdw-dev \ + vim \ + xterm + +#ROS +RUN sh -c 'echo "deb http://mirrors.ustc.edu.cn/ros/ubuntu/ `lsb_release -cs` main" > /etc/apt/sources.list.d/ros-latest.list' && \ + curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | apt-key add - && \ + apt-get update && \ + apt-get install -y --no-install-recommends \ + ros-${ROS_VERSION}-ros-base \ + ros-${ROS_VERSION}-nav-msgs \ + ros-${ROS_VERSION}-sensor-msgs \ + ros-${ROS_VERSION}-cv-bridge \ + ros-${ROS_VERSION}-rviz \ + ros-${ROS_VERSION}-image-transport-plugins \ + ros-${ROS_VERSION}-pcl-ros \ + build-essential \ + ros-${ROS_VERSION}-catkin \ + python3-catkin-tools python3-rosdep python3-rosinstall python3-rosinstall-generator python3-wstool build-essential && \ + rosdep init && rosdep update + +#Replace CMAKE +RUN rm /usr/bin/cmake && \ + wget https://github.com/Kitware/CMake/releases/download/v${CMAKE_VERSION}/cmake-${CMAKE_VERSION}-Linux-aarch64.sh \ + -q -O /tmp/cmake-install.sh \ + && chmod u+x /tmp/cmake-install.sh \ + && /tmp/cmake-install.sh --skip-license --prefix=/usr/ \ + && rm /tmp/cmake-install.sh \ + && cmake --version + +#Install OpenCV4 with CUDA +RUN apt update && \ + apt install libgtk2.0-dev -y && \ + wget https://github.com/opencv/opencv/archive/${OPENCV_VERSION}.zip -O opencv.zip && \ + unzip opencv.zip && \ + rm opencv.zip && \ + git clone https://github.com/opencv/opencv_contrib.git -b ${OPENCV_VERSION} && \ + cd opencv-${OPENCV_VERSION} && \ + mkdir build && cd build && \ + cmake .. \ + -D CMAKE_BUILD_TYPE=RELEASE \ + -D CMAKE_INSTALL_PREFIX=/usr/local \ + -D WITH_CUDA=ON \ + -D WITH_CUDNN=ON \ + -D WITH_CUBLAS=ON \ + -D CUDA_ARCH_BIN=${CUDA_ARCH_BIN} \ + -D CUDA_ARCH_PTX= \ + -D CUDA_FAST_MATH=ON \ + -D WITH_TBB=ON \ + -D BUILD_opencv_python2=OFF \ + -D BUILD_opencv_python3=ON \ + -D OPENCV_DNN_CUDA=ON \ + -D OPENCV_ENABLE_NONFREE=ON \ + -D OPENCV_EXTRA_MODULES_PATH=../../opencv_contrib/modules \ + -D BUILD_EXAMPLES=OFF \ + -D BUILD_opencv_java=OFF \ + -D BUILD_opencv_python=OFF \ + -D BUILD_TESTS=OFF \ + -D BUILD_PERF_TESTS=OFF \ + -D BUILD_opencv_apps=OFF \ + -D ENABLE_NEON=${ENABLE_NEON} \ + -D EIGEN_INCLUDE_PATH=/usr/include/eigen3 \ + -D WITH_EIGEN=ON \ + -D ENABLE_NEON=${ENABLE_NEON} \ + -D WITH_IPP=OFF \ + -D WITH_OPENCL=OFF \ + -D BUILD_LIST=calib3d,features2d,highgui,dnn,imgproc,imgcodecs,\ +cudev,cudaoptflow,cudaimgproc,cudalegacy,cudaarithm,cudacodec,cudastereo,\ +cudafeatures2d,xfeatures2d,tracking,stereo,\ +aruco,videoio,ccalib && \ + make -j$(nproc) && \ + make install + +RUN git clone https://github.com/HKUST-Swarm/ceres-solver -b D2SLAM && \ + cd ceres-solver && \ + mkdir build && cd build && \ + cmake -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTING=OFF -DBUILD_EXAMPLES=OFF -DBUILD_BENCHMARKS=OFF -DCUDA=OFF .. && \ + make -j$(nproc) install && \ + rm -rf ../../ceres-solver && \ + apt-get clean all + +#Install LCM +RUN git clone https://github.com/lcm-proj/lcm && \ + cd lcm && \ + git checkout tags/v1.4.0 && \ + mkdir build && cd build && \ + cmake -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTING=OFF -DBUILD_EXAMPLES=OFF -DBUILD_BENCHMARKS=OFF .. && \ + make -j$(nproc) install + +#Install Faiss +RUN git clone https://github.com/facebookresearch/faiss.git && \ + cd faiss && \ + cmake -B build -DCMAKE_BUILD_TYPE=Release -DFAISS_ENABLE_PYTHON=OFF -DBUILD_TESTING=OFF -DFAISS_ENABLE_GPU=OFF . && \ + make -C build -j$(nproc) faiss && \ + make -C build install && \ + rm -rf ../faiss + +#Install OpenGV +RUN git clone https://github.com/HKUST-Swarm/opengv && \ + mkdir opengv/build && cd opengv/build && cmake .. && make -j$(nproc) && \ + make install + +#Install Backward +RUN wget https://raw.githubusercontent.com/bombela/backward-cpp/master/backward.hpp -O /usr/local/include/backward.hpp + +#Install ONNXRuntime with CUDA and TensorRT +RUN git clone --recursive https://github.com/Microsoft/onnxruntime && \ + cd onnxruntime && \ + git checkout tags/v${ONNX_VERSION} +RUN cd onnxruntime && \ + ./build.sh --config Release --build_shared_lib --parallel \ + --use_cuda --cudnn_home /usr/ --cuda_home /usr/local/cuda --skip_test \ + --use_tensorrt --tensorrt_home /usr/ &&\ + cd build/Linux/Release && \ + make install && \ + rm -rf ../../../onnxruntime +RUN apt install python3-pip libopenblas-dev vim -y && \ + wget https://nvidia.box.com/shared/static/v59xkrnvederwewo2f1jtv6yurl92xso.whl -O onnxruntime_gpu-1.12.1-cp38-cp38-linux_aarch64.whl && \ + pip3 install onnxruntime_gpu-1.12.1-cp38-cp38-linux_aarch64.whl && \ + rm onnxruntime_gpu-1.12.1-cp38-cp38-linux_aarch64.whl +#Install Pytorch +RUN pip3 install --no-cache https://developer.download.nvidia.com/compute/redist/jp/v50/pytorch/torch-1.12.0a0+84d1cb9.nv22.4-cp38-cp38-linux_aarch64.whl + +# Install for TaichiSLAM +RUN pip install transformations numpy lcm matplotlib scipy && apt install ros-${ROS_VERSION}-ros-numpy # taichi + +#Install spdlog +RUN wget https://github.com/gabime/spdlog/archive/refs/tags/v1.12.0.tar.gz && \ + tar -zxvf v1.12.0.tar.gz && \ + cd spdlog-1.12.0 && \ + mkdir build && cd build && \ + cmake .. && make -j$(nproc) && \ + make install \ No newline at end of file diff --git a/docker/Dockerfile b/docker/Dockerfile.x86 similarity index 88% rename from docker/Dockerfile rename to docker/Dockerfile.x86 index 1f3756d6..0d82f284 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile.x86 @@ -20,40 +20,39 @@ RUN apt-get -y update && \ libglib2.0-dev \ libyaml-cpp-dev \ libdw-dev \ - vim + lsb-release \ + vim \ + curl \ + ca-certificates \ + htop \ + xterm \ + gdb \ + build-essential \ + net-tools \ + htop \ + xterm \ + gdb \ + zip \ + unzip -#Install ROS -# update ros repository -# some code copied from https://github.com/HKUST-Aerial-Robotics/VINS-Fusion/blob/master/docker/Dockerfile -# RUN sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' &&\ +#ROS RUN sh -c 'echo "deb http://mirrors.ustc.edu.cn/ros/ubuntu/ `lsb_release -cs` main" > /etc/apt/sources.list.d/ros-latest.list' && \ curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | apt-key add - && \ apt-get update && \ - if [ "x$(nproc)" = "x1" ] ; then export USE_PROC=1 ; \ - else export USE_PROC=$(($(nproc)/2)) ; fi && \ - apt-get update && \ - DEBIAN_FRONTEND=noninteractive apt-get install -y \ + apt-get install -y --no-install-recommends \ ros-${ROS_VERSION}-ros-base \ ros-${ROS_VERSION}-nav-msgs \ ros-${ROS_VERSION}-sensor-msgs \ ros-${ROS_VERSION}-cv-bridge \ ros-${ROS_VERSION}-rviz \ - ros-${ROS_VERSION}-pcl-ros \ ros-${ROS_VERSION}-image-transport-plugins \ - python3-rosdep \ - python3-rosinstall \ - python3-rosinstall-generator \ - python3-wstool \ + ros-${ROS_VERSION}-pcl-ros \ build-essential \ - python3-rosdep \ + libdw-dev \ ros-${ROS_VERSION}-catkin \ - net-tools \ - python3-catkin-tools \ - htop \ - xterm \ - gdb && \ - rosdep init && \ - rosdep update + python3-catkin-tools python3-rosdep python3-rosinstall python3-rosinstall-generator python3-wstool build-essential && \ + rosdep init && rosdep update + #Replace CMAKE RUN rm /usr/bin/cmake && \ @@ -69,7 +68,7 @@ RUN git clone https://github.com/HKUST-Swarm/ceres-solver -b D2SLAM && \ cd ceres-solver && \ mkdir build && cd build && \ cmake -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTING=OFF -DBUILD_EXAMPLES=OFF -DBUILD_BENCHMARKS=OFF .. && \ - make -j$(USE_PROC) install && \ + make -j$(nproc) install && \ rm -rf ../../ceres-solver && \ apt-get clean all @@ -98,14 +97,14 @@ RUN git clone https://github.com/lcm-proj/lcm && \ git checkout tags/v1.4.0 && \ mkdir build && cd build && \ cmake -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTING=OFF -DBUILD_EXAMPLES=OFF -DBUILD_BENCHMARKS=OFF .. && \ - make -j$(USE_PROC) install && \ + make -j$(nproc) install && \ pip install lcm #Install Faiss RUN git clone https://github.com/facebookresearch/faiss.git && \ cd faiss && git checkout tags/v${FAISS_VERSION} && \ cmake -B build -DCMAKE_BUILD_TYPE=Release -DFAISS_OPT_LEVEL=avx2 -DFAISS_ENABLE_PYTHON=OFF -DBUILD_TESTING=OFF -DFAISS_ENABLE_GPU=OFF . && \ - make -C build -j faiss && \ + make -C build -j$(nproc) faiss && \ make -C build install && \ rm -rf ../faiss @@ -138,12 +137,12 @@ RUN cd opencv-${OPENCV_VERSION} && \ cudev,cudaoptflow,cudaimgproc,cudalegacy,cudaarithm,cudacodec,cudastereo,\ cudafeatures2d,xfeatures2d,tracking,stereo,\ aruco,videoio,ccalib && \ - make -j $(USE_PROC) && \ + make -j$(nproc) && \ make install #Install OpenGV RUN git clone https://github.com/HKUST-Swarm/opengv && \ - mkdir opengv/build && cd opengv/build && cmake .. && make && \ + mkdir opengv/build && cd opengv/build && cmake .. && make -j$(nproc) && \ make install #Install Backward @@ -158,6 +157,14 @@ RUN wget -qO - https://packages.lunarg.com/lunarg-signing-key-pub.asc | sudo a #Install taichi RUN pip install taichi transformations numpy lcm matplotlib scipy && apt install ros-${ROS_VERSION}-ros-numpy +#Install spdlog +RUN wget https://github.com/gabime/spdlog/archive/refs/tags/v1.12.0.tar.gz && \ + tar -zxvf v1.12.0.tar.gz && \ + cd spdlog-1.12.0 && \ + mkdir build && cd build && \ + cmake .. && make -j$(nproc) && \ + make install + #Build D2SLAM RUN mkdir -p ${SWARM_WS}/src/ && \ cd ${SWARM_WS}/src/ && \ diff --git a/docker/Makefile b/docker/Makefile index 05b1078a..8fcec3db 100644 --- a/docker/Makefile +++ b/docker/Makefile @@ -1,4 +1,4 @@ -all: jetson_base jetson pc +all: jetson_base jetson_orin pc help: @echo "" @@ -9,19 +9,23 @@ help: @echo " 1. make clean - remove all images" @echo "" -pc: - @docker build -t d2slam:pc -f ./Dockerfile .. +x86: + @docker build -t hkustswarm/d2slam:x86 -f ./Dockerfile.x86 .. -jetson: - @docker build -t d2slam:jetson -f ./Dockerfile.jetson .. +jetson_orin: + @docker build -t hkustswarm/d2slam:jetson_orin -f ./Dockerfile.jetson .. jetson_base: - @docker build -t d2slam:jetson_base_35.1.0 -f ./Dockerfile.jetson_base_35.1.0 --build-arg USE_PROC=20 .. + @docker build -t hkustswarm/d2slam:jetson_base_35.1.0 -f ./Dockerfile.jetson_base_35.1.0 --build-arg USE_PROC=20 .. + +jetson_orin_base: + @docker build -t hkustswarm/d2slam:jetson_orin_base_35.3.1 -f ./Dockerfile.jetson_orin_base_35.3.1 --build-arg USE_PROC=20 .. clean: - @docker rmi -f xuhao1/d2slam:pc - @docker rmi -f xuhao1/d2slam:jetson - @docker rmi -f xuhao1/d2slam:jetson_base_35.1.0 + @docker rmi -f d2slam:pc + @docker rmi -f d2slam:jetson + @docker rmi -f d2slam:jetson_base_35.1.0 + @docker rmi -f d2slam:jetson_orin_base_35.3.1 upload_all: pc jetson_base jetson @docker tag d2slam:pc $(name)/d2slam:pc @@ -33,4 +37,4 @@ upload_all: pc jetson_base jetson upload_base: jetson_base @docker tag d2slam:jetson_base_35.1.0 $(name)/d2slam:jetson_base_35.1.0 - @docker push $(name)/d2slam:jetson_base_35.1.0 + @docker push $(name)/d2slam:jetson_base_35.1.0 \ No newline at end of file diff --git a/models/components_.csv b/models/components_.csv deleted file mode 100644 index 39fc52c9..00000000 --- a/models/components_.csv +++ /dev/null @@ -1,64 +0,0 @@ -2.526640423322523699e-02,-5.719205187670506341e-02,1.485604175732282983e-02,5.024943251207357464e-02,4.207500446582206066e-02,-4.694954531176059875e-02,5.062081231004390836e-02,-5.370716627670011484e-02,-7.365131322072530906e-02,9.043915203279406789e-02,2.423728365144868502e-02,6.392472262860324217e-02,3.691702935462854579e-02,9.686440492304675132e-02,1.286512236611923321e-01,-3.924323157080102098e-02,9.887338999248301841e-02,4.809807524201450274e-02,1.830639389051905153e-02,-3.155228662614269902e-02,-7.395936907027711449e-02,8.181620389670926641e-02,-2.989074393442464790e-03,-4.001862398010758853e-02,-4.286235385606658801e-03,2.804026827664369420e-02,1.269495987216169020e-01,-2.945565025904075748e-02,7.738621020837090081e-02,-6.623323015143107659e-03,7.964898874290905761e-02,-7.639333053887678582e-02,8.265839180153909671e-02,5.004619553790405975e-03,7.208202722855615134e-03,7.426504230295756515e-02,-1.087507832451984138e-02,-9.107785723943676026e-02,-2.816457209619938409e-02,-4.651286462301721519e-02,-2.242822855668176463e-02,4.931123006466384129e-02,3.655164644256722606e-02,-9.340382102507968631e-03,8.895073304291534200e-02,3.554837917609344339e-02,5.488193292702733816e-02,7.313625695350620126e-02,-1.502935238576064148e-02,-5.251507396606016481e-02,-2.176808853007421857e-02,9.097223248388233707e-02,-4.133903325975848281e-03,-2.764786805717866017e-02,-2.894842561097924274e-02,-6.762066751298961842e-02,-4.132569539376527296e-02,1.809253011486327070e-02,-1.116778882930446505e-02,-3.883742552506650163e-03,2.495197011211833325e-03,6.486277523333078276e-02,5.038331120871365754e-02,-4.301525749513592650e-02,-1.248602240653000212e-01,4.543164176388603953e-02,6.631502989148284344e-02,3.497458708947510236e-02,-2.301632843647784601e-02,-2.974108803313544463e-02,-4.058036778640446846e-02,2.147773524580070051e-02,-1.233332998886565307e-02,-1.504593323853457495e-02,-7.191577303566407120e-02,-9.751550516117070411e-02,8.137950382672659577e-02,-1.415588655353492049e-02,-9.499817682816910636e-02,1.295850761935816824e-02,-1.237493964345879188e-01,1.044393192279851335e-01,1.228914612985557847e-01,-1.153042859788721564e-01,9.338948983842597362e-03,-1.048166978945019590e-01,-4.538046673519658541e-02,5.539537744199602570e-02,-5.192528637542207642e-02,9.440072407831559542e-02,6.122707038319859241e-03,-5.481770402409582438e-02,-9.055773729852735487e-02,-4.187333699242024032e-02,-2.502251831248253344e-02,-7.477014843397850399e-03,-4.379437073757061136e-03,-1.769179912717853617e-02,-1.803740332068900706e-02,1.105083051380985060e-01,-1.467304410024221351e-02,8.281896620452240676e-02,-3.932735576426021895e-02,-7.314770112926689827e-02,1.517629764188566967e-02,5.744356552987572861e-02,1.154287912994790728e-02,-4.208347510595353280e-02,2.429802377340256267e-02,3.108740807622215746e-02,-1.665105033712338534e-02,8.069159062642640434e-02,-1.071597486590169374e-01,9.932873561923219463e-02,8.626064709362839256e-02,1.584736741736664928e-02,-5.413461817500340306e-02,-2.702088633748024951e-02,7.320619398856198501e-02,8.319555910856604730e-02,6.240268407389651384e-02,-3.044554034941972243e-02,4.918680075207678554e-02,3.022274735561640904e-02,4.951141190563268513e-02,-8.273417978383848514e-02,1.296767338292199589e-01,-5.889500973224284786e-02,-5.824627364491848092e-02,2.604639624491221842e-02,-1.077594708220426523e-01,1.446347562558023686e-02,7.015175997538572461e-02,9.496092764647097451e-02,4.195149049964322363e-02,3.673535790846663845e-02,3.485915470774891833e-02,3.484868516819612100e-02,-1.355766199421837848e-01,-7.116365783899045172e-02,-1.090927059840551133e-01,2.773952230112094672e-02,6.691416459259323386e-05,-5.009734542425712395e-02,3.590510478045730314e-02,-7.878874472426432296e-02,3.006636317105114073e-02,-4.419763409472155652e-02,-5.629435162880549320e-02,-1.887857011498421042e-02,9.297433134424505641e-02,1.844553156029017996e-02,-6.708480454291249062e-02,-5.641473042577382219e-02,-1.279830070263752173e-01,8.927264853307022296e-02,-9.402148256205272725e-02,-2.156762363697930021e-02,-4.285083929278119680e-02,5.244006999605558411e-02,-1.802179512740473624e-02,-3.600062483788652595e-03,2.323584432284052717e-02,8.251208933724375749e-02,5.886008204886801487e-02,2.382295645033375917e-02,4.711182517335676945e-02,5.806462662344536590e-02,7.296960147861480950e-02,-6.655917999493506099e-02,-7.636426300145268686e-02,1.433954297218443530e-01,9.810841910041971300e-03,1.817641780008077620e-02,-1.958087000340091519e-02,2.821700799649721342e-02,-5.852661695175374185e-02,-9.044299363176017473e-03,-5.230481790175743606e-02,7.817853668291671609e-02,-6.881036128555759190e-02,6.549552294273427999e-02,2.725491338188594992e-04,-3.413112513472393861e-02,-1.118874895435125741e-03,-2.986864669154743260e-02,-1.134897229684574821e-01,1.405094625775002160e-01,1.869879985681054124e-02,-6.259055059552753875e-02,-8.729802681075510273e-02,-1.266902544383307835e-02,-5.022144262326741146e-03,3.279262264805485327e-02,-3.244578307978012149e-03,1.119866021877022888e-01,-1.341577271550675222e-02,4.493995287201570099e-02,8.493215214519470035e-02,6.099290286038099723e-02,6.703028146601137938e-02,-1.258420087402374876e-01,1.462998839844186286e-02,-3.619744453986616789e-02,7.007705829417373855e-02,-2.688362218570766390e-02,5.726144823849040749e-02,-4.563267575738738646e-02,8.715541061854427396e-02,1.667689154185090714e-02,4.012231834492492599e-02,4.310518255012505545e-02,8.765305414196990219e-03,9.374767682027360105e-02,5.483140597913140069e-03,-5.389713788444699255e-02,6.869241246889511154e-02,7.723502464236696452e-02,3.546820383595341436e-02,-7.237313371007778051e-02,-6.208875488914460117e-02,-6.280219710448635417e-02,9.369112431859066489e-03,5.954381689101680836e-02,2.493414003675355553e-02,-1.028442366651920042e-01,1.481351924334194971e-02,-4.643056966306331745e-02,1.029892703592623013e-02,-3.755189713897710141e-02,2.951587304483683713e-02,-1.046104604261153537e-01,3.921025775391363821e-02,-2.409477039036470817e-03,1.237973625549875778e-01,-8.289244003943153916e-02,2.276828267284421864e-03,-8.399367011992075333e-03,5.560722628235437864e-02,1.455471252431322915e-01,9.065046651577667036e-02,-4.372370263740323754e-02,7.654839428449977667e-02,1.230499576452270538e-01,-1.037156297250334397e-01,-1.984569666178630926e-02,1.563790513802977913e-02,2.746076497100421229e-03,-1.101231362426936672e-01,-6.718434217815029419e-02,1.884177111156563542e-02,6.986147307781376270e-02,2.273913488226931429e-02,3.203574149253256287e-02,-4.352143754729341507e-02,4.141610301665707428e-02 --2.909413197292187725e-02,-3.528429003844857359e-02,8.740834205774083987e-02,-6.791774625255975195e-02,-8.038096903958490136e-02,-1.768094811146545334e-02,4.020371743310002799e-02,3.037444407286270243e-02,4.293696720811400469e-02,1.387080484227288385e-02,5.287082588179042170e-03,-1.283935810050539257e-02,-8.363659305047786707e-02,-3.263495465763952691e-02,-2.753640983123731290e-02,7.198051276200653770e-02,2.778393145495195934e-02,-1.791054165014524871e-02,-3.181039634210734018e-02,4.964194424008804701e-02,1.422652450910587459e-02,-6.357676129249589803e-02,-2.322853169064845874e-02,9.089887234780637260e-02,-9.095488703256508767e-02,-7.768689579336641657e-03,3.142526804922677469e-02,5.991012666386007213e-02,4.306066809185223748e-02,-2.277794100155911342e-02,5.108313563195350515e-02,3.029234348496164311e-02,3.116545005224506670e-02,-8.561290750478487560e-03,-3.731261557137765389e-02,5.807042568730886450e-02,1.063593014313246425e-02,-6.792712214053540920e-02,-7.404942978010234378e-02,-1.218295398496928167e-01,-7.880949107608000576e-03,1.133166200444580586e-02,-7.502298017681802156e-02,-9.090291169387465509e-02,5.844717873574444000e-02,-9.344638694053027106e-02,-1.538879522759577234e-02,-4.774228923538331726e-02,-1.668355020659509358e-02,-2.484877964664097133e-02,-1.024308201082266973e-01,3.105119265432057865e-02,-7.523194163927827971e-02,5.112501496714183871e-03,8.278737896846639910e-03,1.108536264204754984e-01,1.378866017006269949e-03,3.571322734709284769e-02,3.110107358889362648e-03,5.288423722485467715e-02,-4.501285252033878845e-03,1.436201316079096790e-01,4.619714100283099645e-02,-5.997844356707442842e-02,-4.980822133735375579e-02,-7.490027051249233125e-02,1.187151315702915204e-02,-1.228704493156962743e-02,-1.458426284804205386e-01,-2.684948282927354657e-02,2.717947465824097075e-02,7.746271343703244050e-02,-2.826818552414373659e-02,2.925744422856803653e-02,1.224684293622792415e-02,-7.249037025153305495e-03,1.976615361554909939e-02,-2.446888618478509567e-02,3.298022898414386351e-04,9.723692559096750593e-03,3.242668387440354044e-02,-3.065020847647263388e-02,-4.800135282057597930e-02,-4.070286834182501069e-03,2.941593762953307462e-02,-1.265047049960395069e-02,2.388798096221492889e-02,6.432563515363302897e-02,1.718215550951805379e-02,-6.887497989396733356e-02,-5.810064717337333484e-02,-1.879914776677708843e-02,-2.780481946244755739e-02,2.131798065619084845e-02,-2.639858999876219456e-02,4.683885267065889674e-02,1.802571352759486240e-02,-5.368277844913472985e-02,3.167799770028571077e-02,-6.247414970551522923e-02,-4.578888940476993213e-02,4.316522756806562972e-02,-4.443531931708431804e-02,5.607373041150076620e-02,2.969288717005016953e-02,7.918328939156386859e-02,-2.148862387455911793e-02,3.921064712211788650e-02,-6.502828375866846755e-02,-7.870914715320107002e-02,-1.043753020190312658e-03,-1.072954714439815216e-02,-9.281773759039229146e-03,-9.713768242931279662e-02,8.370740738124266000e-02,9.408911708514305916e-02,1.093878469321417357e-02,8.724973899898712248e-02,9.274490768940707799e-03,-1.774286696834000837e-02,7.206457970263920043e-02,1.047467111774348508e-01,-1.557271340836826057e-01,-6.651011024885071510e-02,1.500894729074402772e-02,3.737027683609317474e-02,6.935173320223092519e-02,1.527809026711855062e-02,-1.056085683040423745e-01,9.040533972637056215e-02,-9.582711306279206864e-02,1.371465842805741527e-02,-8.214640816368435142e-03,-4.922465110019622753e-03,-1.143259552838211401e-01,8.463278707412755231e-02,5.054183881886510632e-02,3.128897224099750534e-04,3.212948388906888830e-02,-8.170790549998896424e-02,5.297104129183776572e-02,1.122850742193018669e-01,-3.081771818733168231e-02,-1.680923895534396778e-02,8.558165666624018009e-02,-9.366968613726413262e-03,-2.910571497296729135e-02,-1.265360855745091551e-02,-5.310302472972830423e-02,-7.976673590192361052e-02,-5.413252648376078863e-02,-1.094386969615645105e-01,-2.352745570228421956e-02,-1.653012008048862780e-01,6.648948022332631935e-02,1.093066032384619191e-01,1.364755226271092796e-02,6.274698900393342778e-03,3.461188498313155781e-02,-1.125416459841215577e-01,1.241462071344675774e-02,1.705211459776282204e-02,5.459979925589949351e-02,5.974006622820253465e-02,5.931001898177725579e-02,-1.088493980830523844e-02,-4.552365738329224726e-02,-3.705598612759210264e-02,5.029313093431368731e-02,5.919583372853539499e-02,-1.235250692804127914e-01,-5.212956429216435844e-02,-1.743475201974517205e-02,-6.855199055874205971e-02,1.192389110413398165e-02,1.328256855634835781e-02,4.929660382223500364e-02,1.124023307055195681e-01,-3.089864035320898442e-02,-1.175715480700039173e-01,-7.656639302164011673e-02,-8.691051981119298636e-03,-1.089163454526175939e-01,4.526092846446460455e-02,-5.469482687870131504e-02,3.602231435848884944e-02,-5.848461444756640482e-02,5.580896735594251430e-03,1.152955518613745650e-01,-1.369993556821411940e-01,1.038211281476515147e-01,6.397289293033632529e-03,-1.042479511784360768e-01,4.467474780411779395e-02,1.381043561324574453e-01,-1.261142791050211265e-01,-2.502518887796909008e-02,8.544761641217503423e-02,-6.977013137530647946e-02,6.775581406254538308e-02,-5.740333599171826978e-02,-2.674727495007023920e-02,-3.577818267152339110e-02,3.383240367480486177e-03,-9.549951882233496947e-02,1.440040779396529469e-01,-6.386475846155553748e-02,-4.429943195317136845e-02,2.696257826996984602e-02,7.589921598986901219e-02,1.129522807384430311e-01,9.912681530798879426e-02,-4.938860758514618254e-02,-4.108423018944994121e-02,-8.518930966321090759e-02,7.963751830852645874e-02,1.062787890886729980e-01,-1.137249529071079207e-02,-3.074630213493289438e-02,6.279598697904720250e-02,2.123735561022241716e-02,-9.382031612206288840e-02,-3.161264071523629815e-02,-9.517591050273499498e-02,-8.170908040170148101e-02,-5.153955095025097161e-03,9.813183142571015039e-02,-1.536240468546008764e-02,-3.337426915993722820e-03,8.298432847400982748e-02,2.959397185749289630e-02,1.156408194908403568e-02,9.563856938418745235e-02,-9.968083939662449044e-02,-2.968947885951720572e-02,-6.731532944975172905e-02,4.118726406705474491e-02,2.288561134743570188e-02,2.274738746849352001e-03,-1.933919975378044914e-02,8.176592899964707506e-02,3.576915906116236166e-02,-1.817047433978207643e-02,-1.530924341000009826e-02,5.074150232857332909e-02,2.562043131944210725e-02,-3.634818919528289699e-02,-1.042473875757683605e-01,-3.320454384147656854e-03,9.847897778421450810e-02,-9.597812096499200307e-02,5.096249396154028943e-02,-2.416208269716989612e-02,7.467274843465987466e-02,-1.166795627825942261e-01,4.661271993687689241e-02 -8.132850411769872168e-02,-1.735023565532144152e-03,6.260723253080825723e-02,-5.435265998724476966e-02,-4.923646702797859764e-02,4.823581329606019263e-02,6.045832576174231532e-02,-7.980559209549875632e-03,-5.080632055325123853e-02,7.429096004046267798e-02,5.529985146250182559e-02,-6.344601942930064298e-02,5.450904766817087077e-02,-7.286190529466364529e-02,-6.925509871928502148e-02,-6.926991434396556624e-02,-6.004478971826878592e-02,-1.292469555950146731e-01,-5.766983758577737956e-02,-1.530510150566580371e-03,-7.064531606266349385e-03,-5.815603970648663273e-02,5.315621696775523625e-02,-6.424433127237363927e-02,-4.130664097982063472e-03,5.374995719533342230e-03,3.967159799709708834e-02,-1.179164209934278618e-01,1.652144406420609848e-02,5.204420142954258033e-02,-6.608387950309728420e-02,-9.712497753677166307e-03,5.843805807542187120e-02,-8.151349018784280129e-03,-1.338715715599389788e-01,1.104024980938311529e-01,4.995329443077149018e-02,4.447092557992725521e-03,-9.323467405944992070e-02,-3.165856904822143825e-02,1.609266246983436510e-01,-5.595566124607253417e-02,6.266440568983329060e-03,3.854746418955243975e-02,-6.409576979129015528e-02,3.583150894360360994e-02,-2.033662013440077059e-03,3.527401163160061998e-02,-1.061569594435218256e-01,1.726984584926188546e-02,-9.270246161727947742e-02,-9.874009893597593401e-02,-1.083660965831036022e-01,2.524456109893179201e-02,5.315923856279797244e-02,5.103429774338320152e-02,2.772310319875808879e-02,-1.042296946815367137e-01,1.432943271043717326e-01,6.221538980434625221e-02,-3.756447570128646635e-02,5.556020018048520200e-03,2.145335090620108590e-02,-5.828517183621163486e-03,-1.243440391351477783e-02,-3.144602978631601264e-02,-8.652388237860410281e-02,4.897661932026962928e-02,-9.468151515301900800e-02,-1.064862567345349897e-01,-6.998995208623834396e-02,1.422528789292858697e-01,2.340860204520997870e-02,3.507163761908477922e-02,-1.224683051657594196e-01,1.366074782487985972e-02,2.084484055662605417e-02,-1.218596469369711138e-01,9.307865335371497129e-02,-1.018998822425461331e-01,3.684979492935262606e-03,2.495327192025706062e-02,1.147323006690061237e-01,6.248934262090783537e-02,6.801164020795821830e-02,6.740638833589208956e-02,1.868290907360141764e-02,2.866008199734894693e-02,-3.892681751673668367e-02,2.780531600531067730e-03,-1.288003556318227834e-02,-2.000451652946167239e-02,-1.456886535897666812e-01,1.086467968249175409e-01,-4.421922734133294786e-02,1.095050586184547620e-01,1.911234930644280933e-02,-3.246725860186971957e-02,-1.602148584332322712e-01,2.161936046552219090e-02,-5.540741029956487707e-02,3.643482166508658915e-02,-1.172548290319956887e-01,-1.118473794576842484e-01,8.041044071615273403e-03,2.808837078804540810e-02,2.727895319013934686e-02,-2.628173470879303919e-02,2.491193157663365229e-02,-8.859962203260868252e-02,3.896876012913054379e-03,-9.381949565012870362e-02,-3.787584327558903702e-02,-4.164225819841629178e-02,4.594449898164681273e-02,3.483888624875637607e-02,1.350684953046281112e-01,-3.730274413719637111e-02,-1.987793643847620209e-02,-9.981290008859122070e-03,-6.106994112435471661e-02,5.235564077538579220e-02,6.772592177371179745e-02,2.500081561473826638e-02,-5.045356838354502632e-02,8.217796133739026232e-02,4.644610435226403239e-02,1.130457258133361342e-01,-8.077356344210670502e-02,-4.550362173168347873e-02,4.280909334949126177e-02,1.924906849655074262e-02,-8.597329078657461876e-02,-1.934209089769627597e-03,4.352800139503491123e-02,1.138931476048729054e-02,-5.906649165336363277e-02,-7.411827477290799937e-03,-2.305126353329679106e-02,-2.972427226551569227e-02,1.641405490533822190e-02,-1.242910115749069205e-01,1.183207228568296382e-01,1.218673010459804659e-03,-5.474901831376725175e-02,2.348944133029605419e-02,1.037253616948118941e-02,-6.154979336377946336e-02,5.508618900866906615e-02,-8.148795296898089791e-02,-1.281790225081573531e-01,1.940309142536172232e-02,2.800432343574573921e-02,2.224422999050407884e-02,-5.188399561232292373e-02,-1.729106692158952118e-03,-4.347267428494449443e-02,-2.299809416715290628e-02,1.018913906902306210e-01,-4.072390096016072214e-03,-1.099039692455761608e-01,-1.118293584924662763e-02,-1.447180308410720567e-02,-1.027184727797431363e-01,5.917003082086166876e-02,-1.176819754182385769e-01,7.427465657470749827e-02,1.926998699748615207e-02,8.909273021185266928e-02,-3.241508279760615865e-03,-4.179077554816829770e-02,1.269742782416375011e-02,-2.281092409102243243e-02,6.462691544914735131e-02,-8.535319515252999534e-02,-7.704150806905187519e-03,-5.764630378579765208e-02,-3.028982433259296350e-02,5.866474786459670299e-02,3.701002429655472770e-02,-3.838742174980790730e-03,3.037499329260983857e-02,8.624679920351613349e-02,6.236125481612091702e-03,9.171340672594641141e-02,-4.946343350961700247e-02,3.681067599312592281e-02,2.390450997833136124e-02,9.431647794547503780e-03,-6.414559176048465101e-03,3.070490536012179458e-02,-6.008886368239439724e-02,2.919870600899784202e-02,7.837219928341294939e-02,-2.334272782817255892e-02,-5.485404202879450714e-02,1.503754906790316995e-02,5.343190478001624139e-02,9.042702314239436934e-02,-4.872722117065517861e-02,-2.303557320619962004e-02,-6.447701961703240781e-02,-1.659353234893574947e-02,1.846527299134655131e-02,3.182281231085577755e-02,7.821659118106670527e-02,1.109529128557721345e-01,6.200752135271713622e-02,-2.484309672985626424e-02,-4.285503628811315746e-02,-3.901430994604470487e-02,-5.805582925110744363e-02,4.129015137721630396e-02,-1.504231029867418579e-02,-2.100309098878802572e-02,-1.136593453784504187e-01,2.027755211707364322e-04,-7.377194369753374559e-02,-2.212055435957574825e-03,-5.269475135938311156e-02,-8.433844302855535158e-02,-9.365702162905509276e-02,9.860276854397975743e-02,5.642680618560619310e-02,-8.430741184895267082e-02,4.507425646318526102e-02,-8.921884462764839707e-02,-7.567668946378197647e-02,4.063883765781416829e-02,-3.031772954022366948e-02,5.868613319061935681e-03,-2.172191515758992511e-02,2.322236861994490809e-02,5.346721117629715803e-02,3.947481520866876620e-03,5.961343574762157510e-02,2.257401104706426714e-02,-2.007208125898930946e-03,3.067558108980552103e-02,-4.970796938125326969e-02,-8.182219541096243109e-02,-6.998110523702795460e-02,-6.879918069656802693e-02,3.998733514797586652e-02,3.417625315456217228e-03,6.504457612671589450e-02,-1.386483509106096329e-02,-5.276226723856704270e-02,5.617767123109279614e-02,2.504234948920145212e-02,-2.894355481158328650e-02,5.878704550689561013e-03,-5.584681340458658161e-02,3.497327303912553531e-02,3.646887365918932700e-02,2.761468114036198448e-02 -5.596641905498883629e-02,7.277994592979852850e-02,-1.105831484117647534e-02,-5.397296830571229886e-02,-8.853952079863391944e-02,3.162074710282403167e-03,-1.681639851894154575e-02,7.548835235338358096e-02,-1.080849080200874113e-02,-3.984892082116964218e-02,9.357393575413358588e-02,1.018510790089441065e-01,4.339846785904172277e-02,-1.042451381986353454e-02,-8.586981538093479971e-02,9.002106744513452274e-02,-1.956224712370854477e-02,3.724109195668692807e-02,-1.045409591059134613e-01,-4.113852098081370590e-02,-5.700411714787316508e-02,-3.683264637618158255e-02,-1.115633694763025208e-01,-8.893601257649053071e-02,4.540081970400948463e-02,-3.377933364151743806e-02,3.315508067350014798e-02,3.457580521207341551e-02,2.248384429712494553e-02,-6.982890293260772541e-02,-1.941944167899167795e-03,3.818744447456797093e-03,1.003316126986794621e-01,3.778886538264301237e-02,2.176693091038879938e-02,-5.477389802363073074e-02,5.668405543343413983e-02,8.115909846511779047e-02,-6.627557773881675995e-02,1.927167208900854717e-02,-5.172974157057867728e-02,8.734156658829642095e-02,5.582268184824676349e-02,2.783613202105826578e-02,-8.283759515026897779e-03,-6.443367110351998564e-03,-1.375022747647584864e-01,3.372955910662182172e-02,-2.768970122703102998e-02,-2.874623080854939866e-02,-4.120870571101020036e-02,3.176535329829091592e-02,3.301933069818541872e-02,-1.476454075689163925e-01,1.042964827445096686e-02,1.047992566470501152e-01,1.114651620473840893e-01,1.386673936366172172e-02,-1.821850620017143388e-02,7.195546763612339913e-02,-1.717235689538315277e-02,-4.475544057349735774e-04,-5.704850683758466538e-03,-3.260252230022189779e-02,-1.149506058496987670e-01,-3.269786607482329938e-02,-8.463004164633439186e-02,-3.430249773888283810e-02,-2.085497288189585435e-02,1.237399207886273438e-02,2.466570173274173182e-03,8.539227307144710288e-02,6.626706814274291313e-02,5.396632256715713782e-02,6.782259778706153930e-02,-7.499776545417216264e-03,6.085163809473903562e-02,-1.128114757824389969e-01,-3.968062756328994550e-02,1.739063375845654319e-02,6.894628995507288971e-02,1.022098416986766400e-01,-1.979523899276483534e-03,-1.130978713799660196e-01,-2.997288532842294975e-02,-1.668892306899316669e-03,9.926007239756459710e-02,-1.374951472408477912e-02,5.853416718329686880e-02,-5.218053730119887740e-02,-9.946569850071737551e-02,8.539620934954740561e-02,5.879582657735249462e-02,2.261268434505678965e-02,4.635460710416289959e-02,-4.979929715378903637e-02,-1.259389312096657713e-02,-1.243151975247996088e-01,5.084385326304644015e-02,4.493102991629590454e-02,5.705049262958516304e-03,-6.798036108836591485e-02,4.996763597101481352e-02,6.862339441524567540e-02,1.330250424112221219e-01,-9.275071199790440452e-03,-3.434547271281843811e-02,-3.026825850846024540e-02,-7.339076277194817033e-02,-2.406839608164502292e-02,9.986941146068580022e-02,-4.559829230262957278e-02,1.386408027443351010e-01,-2.905272253557285300e-02,2.565366117505415988e-02,1.721884233066387632e-02,-7.400778308730415755e-02,-7.964888302711067847e-03,-5.561642089484689772e-02,3.784905354040706688e-02,2.136338659408869994e-02,2.830447535135152334e-02,3.053582378133226638e-02,2.054494569424756070e-02,-7.385336380434343040e-03,5.848714295682058695e-02,2.416716007747566480e-02,1.163907257348104941e-02,-2.516363533209573997e-02,-1.580581691400723826e-02,4.273993946542681971e-02,-1.173822996989927758e-01,2.184618289696758919e-03,-9.764761085928166939e-02,4.320309261770050729e-02,-1.965658011545154404e-01,1.347035612021396100e-01,-1.054186316083222247e-01,2.715964924797521080e-02,4.114854061072886066e-02,4.289368876798840130e-02,-2.399137383170751522e-02,4.779948517632756633e-02,2.583928347482457831e-03,-2.777738726059363755e-02,-5.227429310101265603e-02,-6.882566388807430380e-02,-6.717219982073652229e-02,-2.170122004279285283e-04,7.861278434921141478e-02,-2.069340320132237454e-02,3.248384956634237097e-02,5.728181578784753009e-02,3.334357913574605842e-02,-6.986736290951602035e-02,-6.983022999383842189e-02,1.684687490665104283e-03,-4.659937481701906942e-02,4.172214493323629647e-02,-3.055234373387765573e-02,2.141030393263608847e-02,-3.436414787770303292e-02,-9.618143561364489447e-02,-9.371779197314855647e-02,6.202365824770113278e-02,-1.598953565372371044e-01,-1.372162098103537134e-02,6.884695724580999765e-02,-3.060273274919862122e-03,-8.573673431399517209e-02,-6.015825109439357221e-02,8.055879750541516948e-02,4.030503668912324700e-02,-6.903652410842399736e-03,-2.074852296482589814e-02,1.613799328966492377e-02,3.334238288347087659e-02,-3.921673996732365219e-02,2.230796277871765348e-02,-5.246848888699418706e-02,5.746491576380920174e-02,-1.294747542389809714e-02,6.874176253648850432e-02,-2.683824856775038001e-02,1.006910263446978149e-01,8.090893581628713638e-02,-5.144938809934596069e-02,2.046362970811539672e-02,3.235369045931246745e-02,-4.291882002820980764e-03,-1.050362076210353268e-01,9.278287123549319726e-02,-3.061978085843144320e-02,-9.243304349070904924e-02,5.099806739674276473e-02,2.582727506491472313e-02,-1.374059149685126838e-02,-7.229099638082066293e-02,-1.676871977909789174e-02,2.876037380651869438e-02,1.301198888811868071e-01,1.008702302268571493e-01,2.112559396267788686e-02,-1.120260825699306989e-01,-3.062899082859479524e-02,-9.340388698245646881e-04,-4.724234403762656020e-02,-5.779051702344561309e-02,-1.800015857078857834e-02,1.088161964400920570e-02,1.477956979490768208e-01,1.205381192736054985e-01,4.679804569745329990e-02,5.594860317334788419e-02,3.396239870904695091e-02,-2.570781242492946664e-02,-5.965484701946448948e-02,3.155996101456726849e-02,-3.673677858726970280e-02,-4.947547163368096190e-02,-7.202574108503563555e-02,-2.138482615981487922e-02,-1.182371990986864768e-01,8.192430647435738900e-02,7.588730256986503386e-02,3.342450818760879516e-02,-5.702397885550304635e-04,-2.318115201587908972e-02,-1.295845214735402706e-01,-1.384197892020153919e-01,3.827773645610874076e-02,-2.217845439415666933e-02,3.014685056642862210e-02,-5.238352035316111432e-02,2.869049144105468088e-02,1.719173881557191710e-02,1.180552088204738642e-01,-9.421495639233734565e-02,6.070898755135326758e-02,5.790290823799011666e-02,1.615622992295757260e-02,-7.624850656681826655e-02,6.403068822336688282e-02,6.165689806941940590e-03,-2.100956447066235078e-02,-4.094935215058616990e-02,5.666897975447034164e-02,-3.181726054406065068e-02,3.611181220613601112e-02,2.580982332564794174e-02,-8.114394702128155390e-02,1.975690981235997401e-02,-3.537942581078525744e-02,-3.616368825994858982e-02,-1.898142890231768048e-02,2.462323810340927288e-02 --3.292120873038340001e-02,1.609344563710767700e-02,-3.981043194397448914e-02,-9.113753553998399370e-02,6.501243460678274300e-02,-1.144107606293797985e-01,-3.136287213784313271e-02,-2.079649844585800822e-01,-9.602844706324956126e-02,-1.640905209634735407e-02,1.029980110222805528e-01,7.074719348699964883e-03,-5.786942985669468742e-02,1.412064904736154425e-02,1.277624259525506067e-02,3.569571590571868347e-02,-1.041612675616448713e-01,-4.191431080878333520e-02,-3.557009367323037824e-02,-1.505083485922788927e-02,1.029810544886414214e-01,1.750685331327576921e-02,-1.398299958473200044e-01,-2.607141316518147428e-02,-5.849848983465623375e-02,5.517406153980410066e-02,4.428869737738483359e-02,-6.435056093427068968e-02,-1.497061899565934814e-03,-1.936991904883837365e-02,9.903496096434739882e-03,-5.059665833814905311e-02,-2.862120810282819602e-02,7.980238362670082075e-02,5.183244488927767069e-02,-2.081421743168350436e-02,1.239067766211607718e-01,5.822393837015266138e-02,3.827760066534351829e-02,-4.825077735046332666e-02,1.269531541354667302e-02,-7.748846644163003006e-02,-9.833931762129224929e-02,6.325387608896906877e-02,-4.167545590716692505e-02,-4.464447890620358572e-03,4.508560967126080954e-02,1.467178825186030502e-02,5.953650961477093689e-03,-3.076186753742310431e-02,-3.761151209984536781e-02,1.336226209777243712e-01,5.144815073232301972e-02,-5.024723044798489607e-03,1.730143808695233307e-02,1.088987300419134163e-01,-7.508213732845595012e-02,-5.918104014426620041e-03,-7.314657747692983325e-02,-7.043305027347356723e-03,3.334863408944335056e-02,-3.231646344530939713e-03,8.964065441670880652e-02,3.828621495277607922e-02,-3.335182856955052483e-02,3.989373598203681259e-02,-8.320194678832747159e-02,4.578705401978016426e-02,7.536653687686527270e-02,-1.945654865077330042e-02,2.059699357508089704e-02,8.171562489134918950e-02,-2.397446963488406371e-02,-9.923609939703348023e-03,1.080419170718661948e-01,-1.909302738556564391e-02,8.553620361045548293e-02,-8.747633331640912446e-02,5.739514276900870710e-02,2.602772583527960745e-02,-2.325018513032323098e-06,4.461463779061805579e-02,-7.226630226975871985e-02,3.741352363867742666e-03,-7.981110366958203001e-03,-9.465116772799510997e-03,-5.445030336911722835e-02,-8.920890793362096227e-02,6.729011111505114884e-02,3.995532708329588290e-02,7.601670710960947708e-02,-8.213586148000666387e-02,-3.461474957735612840e-02,-5.749791189046608159e-02,-1.149090098947889715e-01,-5.935329814877076879e-03,8.089927525541351638e-02,2.966361950149029592e-03,9.800955139189138077e-02,-8.101722173734082766e-02,4.236687158890607707e-02,1.234900031815150956e-01,8.198986930140331197e-02,-9.599617039362584592e-02,-8.134661001659188040e-02,1.379538455006601091e-02,-5.133318722476739454e-03,-2.787841216641154252e-02,-8.941312910335766362e-02,6.606386989121487696e-02,9.347911271530515154e-02,-4.295644544659846936e-02,1.310867941672802794e-01,-8.018017777225695808e-02,-1.182008423319375889e-01,-8.657718031494167354e-02,7.073934870750984982e-02,8.052131874190938954e-03,1.317715269914167785e-02,-8.627795994210428476e-02,4.596643904730040447e-02,2.557478546047904991e-02,1.402003955389893516e-02,5.316035442280370171e-02,-6.286701976193244390e-02,9.287772931426066525e-02,6.520269054311564338e-03,1.530626644533244346e-02,-3.772832586899984436e-02,7.315883381869926760e-02,3.056696943681264447e-03,1.179671082340174454e-02,-2.545565440408535662e-06,2.145003043460591563e-02,2.001674957240567182e-02,-3.962987817885602060e-02,9.234209613244484874e-03,-2.772576435344882920e-02,-1.601714520513793094e-02,-1.223043512406627592e-03,-2.161406024726773498e-02,-2.136729565707094769e-02,1.222735741737528657e-02,5.970847812403137084e-02,-1.713817845470776147e-02,-4.082616795353569833e-02,-5.362514317567302832e-03,2.155820307192714974e-03,4.714461094354951975e-02,-1.731924323770018051e-02,5.322801221710529718e-02,-1.273797926093159054e-02,-8.495270639516762368e-02,4.283402396577401516e-02,-1.641233773172620647e-02,-6.953219787401260232e-02,-4.452269154479602392e-02,-9.118804613365173728e-02,7.351735832708040930e-02,-9.864265769610036649e-02,-4.165301928550480676e-02,-4.059772244580878847e-02,1.035416067764569348e-01,-9.165516079985412679e-02,2.225769751375782091e-02,1.200198260943419193e-02,4.602389131577185044e-02,-5.779988015148696673e-02,4.476969533012423175e-03,-3.402926900402135357e-02,3.126316179370867976e-02,-2.120214638629489695e-02,5.193091008437260986e-02,-8.012607762544843304e-02,-1.756668909881688515e-02,-4.468980107533125634e-03,2.853023729450197832e-03,7.559473857783510775e-02,-3.395477960200728174e-02,8.239897620095223973e-02,-4.194606512732205333e-02,4.979144518034960692e-02,-2.284852385807700087e-02,-1.419067084537811441e-01,2.689711144189981668e-02,-4.832194536598979906e-02,-3.269460024124088565e-02,4.531007978934582792e-02,7.362707182869224209e-02,4.948713925864456353e-02,-1.372370624315816245e-02,8.510709396570648178e-02,-1.007500605081456252e-01,7.955930698816293667e-02,4.289199874698632585e-03,-4.416649239756512829e-02,-5.557810091671260000e-02,9.373490617913333295e-03,4.056601877407946255e-02,-1.639338096093694297e-01,-5.624522879700665640e-02,5.123112066426470379e-02,9.584186441850325866e-02,7.899135729879359913e-03,-7.095039123514189361e-03,-7.678721789085526939e-02,1.077558015915086459e-01,3.302944592457936202e-02,1.510095265730970748e-01,1.249002235357537377e-01,3.962089967449679284e-02,-7.337073188992104811e-02,-6.009011556693738543e-02,-4.692978090919079637e-02,5.960140318155428274e-03,-1.290357152295559112e-02,1.043452054593820844e-01,-1.142040655465809129e-01,3.056586489052157130e-02,3.275427887582353648e-02,-1.153782451919095620e-02,-4.671504686565648712e-02,6.323117034029322736e-02,-8.606413314204196252e-02,2.827853840590074594e-02,6.987069568270622144e-02,-5.945697408764480385e-03,-4.959119209099619591e-02,-1.887422212118711196e-03,4.022063460448443983e-02,2.497496132982237466e-02,8.829796108808213306e-02,-2.093688422719187578e-02,-6.655035412575797948e-02,-4.427187504977948135e-02,-3.553551750856657511e-03,-6.427809421022390168e-02,6.771650123828219792e-02,5.664561691078503700e-03,7.623318012405430610e-02,1.950349162480535803e-02,2.620456664672194194e-03,1.491624692558735366e-01,-5.366670469774446894e-02,-1.206787823907777835e-01,-9.107645569170841338e-02,3.686971480294680414e-02,5.040914120161851114e-02,9.424454814495957303e-03,5.988503227893265946e-03,1.132773179240492423e-01,-4.296553553670100917e-02,4.837918112033703574e-03,3.618172233589456166e-02,-5.175046897707394344e-02,-7.346566354645184260e-02 --4.591722144838942393e-03,4.308655405192899496e-02,-6.715002463667778254e-02,6.148084203862567293e-02,-9.452694694084343152e-03,1.929131449980992299e-02,4.313117973966167085e-02,-1.578949379024729288e-02,-6.177721530379470111e-03,4.923094565326885386e-02,-8.106932741989331115e-02,-2.971746165044580890e-02,-1.078577155342567706e-01,6.383845515984445940e-02,-4.122805392365069821e-02,-8.223058463979966703e-02,-2.717655138733874112e-02,1.721538471814260465e-02,-5.464857095635339973e-03,-6.692254022180343931e-02,-1.017105406955043645e-01,1.390278046267924561e-02,9.487488719897933798e-02,5.996298932425162681e-02,1.418040612504046505e-01,-1.970683288811900247e-02,1.156691307812341557e-02,-1.001385883270563826e-01,2.796774075125457032e-02,3.590594452264601194e-02,3.442515143724259041e-02,1.135794414393719379e-02,-4.785109489936970745e-02,-3.156318777438604090e-02,1.999771289863812307e-02,3.752914420873293411e-02,1.040190508454173351e-01,-8.500825760682718946e-02,2.975205229567942991e-02,-1.231104204882536024e-01,-1.232432610989496122e-02,-3.254219789055188411e-02,1.160292195569214563e-01,-5.253259580352280522e-02,1.241830932390296377e-01,5.779095328908239598e-02,-2.638249042975625985e-02,-4.653327909425114539e-02,4.844127430807087381e-02,1.354462164659352152e-02,-9.045918182148622588e-02,2.060641128096338259e-02,-1.902349426035949281e-02,1.813070490170894805e-02,1.930071577164581970e-02,-1.211503256737446466e-03,-3.453582519690706620e-02,-7.385699198021281053e-02,4.229303123208291715e-02,9.460645918938608012e-02,5.982728349170289561e-02,-1.026227739992613963e-01,4.100852972137755681e-02,2.907955225671688890e-02,-6.864185237091335878e-02,1.070862253207208459e-01,7.226666824423644275e-02,-9.134964798781471107e-02,1.093638923984680683e-01,-5.053138848266526539e-02,6.386228525584393412e-02,-8.625950998639989042e-02,4.379293765392198307e-02,8.687717256913039809e-02,2.936338875347099461e-02,1.259927635996589135e-01,3.843024774941571076e-02,-1.531583940886357444e-02,-1.494605058009402382e-03,3.228386132466326552e-02,-3.280320381992805207e-02,2.169320464077832689e-02,-5.708906142129481498e-03,1.072475314269847810e-02,3.300600936563523324e-02,3.550073128752809920e-02,-8.950255974937426640e-03,-8.053814396900020600e-02,-7.273095556472702894e-02,-7.402808494557955310e-02,2.346854543688640243e-02,8.055678436419226596e-02,-2.013926991133468958e-02,-8.140891455481449879e-02,2.970008195863280132e-03,-4.668962158802065360e-02,4.713105177556403680e-02,-8.077296416264929946e-02,9.833397529437552798e-02,5.032058736245448305e-02,7.865969851130526214e-02,7.526339852712665701e-02,8.920559459960671067e-02,2.446352187313550336e-02,-1.361168339777602682e-02,1.177608052168125780e-01,-7.791856236899481758e-02,1.282756952171016374e-01,5.508781563773040152e-02,-4.138215560614923877e-02,-4.258649169681721219e-02,5.968894597355417497e-02,-1.033847939035264715e-02,1.246376161651140124e-02,6.267074045873566313e-02,-5.514504795685531979e-02,6.978569053400704436e-02,7.005437279711082543e-02,-3.636523621475817869e-04,-5.241156516413390809e-02,6.865461223512516441e-04,1.751810858856322345e-03,-5.598224816520838004e-02,3.506814559463723724e-02,-4.547240200312043651e-02,-2.179728204898139649e-02,-1.382636782758838723e-01,6.145639015261344928e-02,1.283834098464047002e-02,-5.462191993642601107e-02,6.735167826749918762e-03,-1.281238894265644968e-01,2.869813222339345424e-02,1.008568420044298961e-02,9.092557529205978761e-02,1.245739626478374051e-01,3.559522839594785742e-02,-8.723886234319373767e-02,-4.660758248257541869e-02,4.269653402621573224e-02,-2.612186281625187428e-03,6.763147209783376690e-02,-4.788459847132611746e-02,-6.195459632857731186e-02,3.421502041949640860e-02,5.214759342275726225e-03,3.599354479530132971e-03,-7.372850979034854813e-02,3.598871898642654749e-02,-3.127696053757221886e-03,-1.032843185367619300e-02,7.282395593638595066e-02,-3.442894389183795423e-02,6.423171022521646095e-03,-8.140601738996840198e-03,5.382148862168040920e-02,-5.479394444874065295e-02,-8.385296670219515025e-02,1.236839759801930949e-01,-8.291367272939681454e-02,-6.516595822046003517e-02,1.683342473358179908e-03,-1.648712534812308839e-01,1.152708556132741105e-03,1.253644064511175993e-01,-9.662663133572856056e-03,1.418214150152261599e-01,4.239559704114539973e-02,-1.099905484521185133e-02,-2.478818028860560116e-02,5.561632232242673390e-02,-6.136518831521703410e-02,1.128532215791979987e-01,-1.097015386114122831e-01,-7.956472548681650658e-02,-8.224545141426979622e-02,7.368859934984896970e-03,4.369551794139803214e-02,3.503868314482512514e-02,7.702157864080901029e-02,7.847080306157020943e-03,-8.402038825650456899e-02,-9.549846789230365118e-02,9.600987675655228704e-02,-8.214532106638947062e-02,5.776464419317847016e-02,-4.803347658336944159e-02,-4.378010542422035301e-02,-5.031296398396392644e-02,3.080554497162656113e-02,3.548443162907175680e-02,-1.036696437602708382e-01,-8.041983981912471935e-02,9.610055735536260357e-02,-9.048937944070370443e-05,6.655226263492206029e-02,-9.166423380814911481e-02,-1.075140508532975575e-02,-4.845163727608266940e-02,-1.279638551073008536e-02,-5.352092225159681832e-02,4.353718348939863980e-02,-3.921652842632158270e-02,-2.685937370441019573e-02,-1.505013200806658985e-02,-1.021589435940298274e-01,-1.288228364047991348e-02,-2.512015228500408606e-02,-2.594671665981420489e-02,4.091119903235213018e-04,3.109830902462770719e-02,1.006246951693572089e-02,-7.926796344022411789e-02,-2.279183108804522934e-02,4.235223378421586193e-02,-4.365662183124374013e-02,-1.100429243820279235e-01,-2.738697597722518362e-02,-5.847391037393782215e-02,-2.513838137652396956e-02,-9.724737768232069590e-02,-5.721203136140248086e-02,6.047220459463262254e-02,2.865225185989628623e-02,-5.034190922967247961e-02,6.609744209506721035e-02,2.801489052475265168e-02,2.488446800512076826e-02,-3.365243758908013222e-03,-2.468207966680300083e-02,-7.651159100463453266e-02,2.257607807194964705e-02,3.473560270612666839e-02,-6.018950254802060001e-02,1.120118608142759431e-01,-4.943395630984530852e-03,-2.546156575869686772e-02,2.531701477889657798e-02,5.647040091094839009e-02,-1.873750069126787349e-02,1.094676142508349687e-01,-1.008884214007260277e-01,-3.990174054544299437e-02,-9.272937015003342165e-02,2.688042709831427526e-02,9.248192253657326611e-02,-6.655904198172388209e-02,-5.790281867314381326e-02,7.332721636358233153e-02,-1.451292480379127656e-02,-4.341636255702705643e-02,-3.887316346836940106e-02,-1.138420019206557235e-02,-1.311217448576689759e-01,9.519628133647189266e-03,2.133787849145373644e-03 -1.395874327332146392e-01,-2.151824134756711443e-02,3.222659395947961858e-02,-1.609320999605143299e-02,1.760606826901570110e-02,-1.043837876865071623e-01,1.123525034590055760e-01,-2.941742389710431949e-02,1.089669994834190601e-02,7.983568698577253597e-02,-1.092639512711608973e-01,-4.567819482513868490e-02,1.168113269649730390e-01,9.557348851211321106e-02,1.833140683419431927e-02,1.694138205182771362e-03,6.532802857115400930e-02,1.100211160305684249e-02,-1.356862976435227464e-02,1.510656730799981029e-01,-6.720551214634622184e-02,-1.398405288869121188e-01,-3.151244496976524090e-02,-7.053721981245715533e-02,6.027245230852240621e-03,-4.362087655138183195e-02,-6.182837705275875950e-02,-8.638985821371777951e-03,-3.605849016611013813e-02,-2.695062353603627087e-03,-3.187985343662599891e-02,-5.022531042606133422e-02,-1.926726519899133946e-02,9.560721953428773234e-02,3.182241390892701083e-02,-4.875158553471645173e-02,-5.983967106753204646e-02,6.707016476154786799e-02,-7.421177033239839682e-02,-4.362265577703857561e-02,-7.448219870449176994e-02,-7.008437315787173494e-02,-3.567882958226150508e-02,-5.307353021336347343e-02,4.242315880787386057e-02,-1.396948891986526248e-02,-1.080699055131290054e-01,-7.597034047111848876e-02,-6.603895120503451566e-02,4.533956352846876814e-02,-2.468250054346049147e-02,-2.340023675373505141e-02,-1.055346011719851713e-02,-1.124719556320898269e-02,-1.348212140380811164e-01,5.851035233752582831e-02,-3.719433385049892432e-02,7.192056393187915009e-02,4.734502731320278102e-02,8.716733235478359154e-02,-6.554263725856360190e-02,2.598742271641979983e-03,-1.167024439447604187e-02,1.271791190345411826e-01,-1.033931310777262691e-02,1.069634173867265586e-01,2.797521852599845274e-02,5.052770785821156241e-03,-1.962281325375029792e-02,-8.445465057927808994e-02,-9.714316640506837422e-03,4.158266725397218033e-02,9.583373333573400737e-04,-6.089830361139791232e-02,-3.438571026773663830e-02,2.833819329035761059e-02,-2.416890290395838636e-02,7.231576221525551351e-03,5.450891801903803152e-04,-2.675223253106153234e-03,-6.770917297413209246e-02,2.596703029579539024e-03,6.781234954916368074e-02,-4.112380558542871184e-02,3.906097923977805420e-02,1.057294924581056172e-01,9.352964826728640477e-03,-1.075146711380873948e-01,-2.660742260625469091e-02,-9.333464559807171101e-02,5.551798338936084209e-02,-4.695764062733248645e-02,-1.359399361220974857e-02,-4.140844286933721002e-02,-9.124000359752207356e-02,1.776430607802040318e-03,-1.838623685914343026e-02,-5.318023101604160002e-02,-2.336578272413221760e-02,7.988039860013194937e-03,5.194345184235997503e-02,-4.736601457500981549e-02,-3.087458647408865275e-02,-6.317573196688847439e-03,-3.897463707264628430e-02,-6.321148242277120632e-02,-7.062654421546671508e-03,-1.297482195936242866e-01,-1.290802139094933132e-01,7.396414245823500211e-02,2.396048546024330192e-02,3.327718693453633747e-02,-7.567607745997177726e-03,4.633534010574834328e-02,-3.263309849587479189e-02,-1.188376811182237092e-01,-3.124756351963019207e-02,-4.453187171951700218e-02,1.421799875555175841e-01,3.986901812993168606e-03,-1.031635318604192525e-02,4.202829800403531935e-02,-4.246145718011213460e-02,1.149039692970346138e-01,6.153314105719068228e-02,-9.558899521811330124e-02,1.748116233352704427e-02,8.141916255160883131e-02,-4.247819280427604649e-02,-1.314334849594803734e-01,2.839235096476878939e-02,4.204744706476781085e-02,-8.854847001426441322e-06,9.331871866277135358e-03,4.001012355816112656e-03,-4.732113509811609192e-03,-5.680572399103449299e-02,3.781862741215095275e-02,1.133672468805589986e-02,-1.110616698206585151e-01,5.002037112623190784e-02,-4.921330738375510189e-02,6.512421245664784530e-02,-1.384179761497177086e-02,1.623559475300864058e-02,-3.869831771836643175e-02,1.495094733128926046e-01,1.785508843800255943e-02,-6.172299806987786158e-02,7.494328095099339615e-02,1.071095953484336172e-01,2.168958506425450183e-02,-3.981208484293709271e-02,5.894871133661734770e-03,-9.768259632249687296e-02,-3.640421936246573259e-02,9.943163731056986476e-02,1.495817977473427318e-01,-1.670823523588756132e-02,3.149901375836178241e-02,3.400708070750078915e-02,-1.760930690187273440e-01,-1.427632886032405291e-02,6.023872759079566125e-03,2.208213648660672934e-02,9.683006454515541428e-02,-9.144126613028102923e-03,-9.171759612684113327e-02,3.142035350984060532e-02,-7.295920695239507814e-02,-1.083769836213258064e-02,-2.699453122330477267e-02,-4.276841113641383779e-02,-2.082102117277033135e-02,-1.464842831667199363e-01,-9.553881497755210839e-02,-1.962369187379286312e-02,4.681565443151352451e-02,-1.137485840347561908e-02,-3.127534523209077322e-02,9.832186886291279654e-03,-5.352319833308005759e-02,6.785695162203804476e-02,2.686215926715165847e-02,-1.174693246200125968e-01,4.346439364153781987e-02,-2.693554598763493041e-02,-9.238055463290015951e-03,3.358217275533400409e-02,-2.011087600774002018e-02,1.121782130273762668e-01,6.893178091917603889e-02,7.615309983298035670e-03,-1.099132070416023177e-01,3.124954642239483396e-02,8.748074123091280352e-02,4.444190411859610945e-02,-1.116938825402230724e-02,3.092470340601398415e-02,-3.907393222693893164e-02,-1.518875291440437147e-02,3.682572040453089418e-02,-9.993386898079759106e-02,-3.220663510222962489e-02,-8.688917703321996644e-02,6.111543013977689676e-02,1.932113613239487201e-02,1.496958368888039528e-02,8.032021815338702408e-02,4.335365628749632966e-03,6.447049989287512162e-02,-1.943018094959738512e-02,-1.092378089872833996e-01,9.156521405887094178e-02,8.361110260570228658e-02,4.122322492602691762e-02,-1.058241061245461523e-02,1.967438902900346676e-03,-2.730397476349711444e-02,-9.430336404420688878e-02,-5.863360932709910794e-03,-4.876110755964700033e-02,3.199135168100059740e-02,-1.402758535485595925e-02,-2.325791865482327095e-04,4.474299829751567786e-02,-4.702978064983140416e-04,1.067907782501525404e-01,2.481174104924114068e-02,-4.038924155221446100e-02,1.667639594970042952e-02,7.705913572919118049e-02,1.115840057542974717e-01,8.774622573602484238e-03,-5.471167228541372329e-02,4.206357053178247374e-02,-3.860822756530334077e-02,-9.307789679123708304e-02,-3.807792450204731255e-02,-4.160523331314194134e-02,1.291947169970969304e-02,8.852829536534635724e-02,-3.582905441272889180e-02,-8.666774042433708636e-02,2.689602131364709700e-02,-1.108833942974411929e-01,-1.960456313503322265e-02,-7.334075149456535658e-02,-3.057443035213205929e-02,7.159602669327561064e-02,-6.440967108182979461e-02,-8.265514545967701798e-02,-4.382949880868781956e-02,-2.855287006013931123e-02,2.275207952914504173e-02,-4.750353557446149871e-02 --1.319071697092057127e-02,-2.633724863004949235e-02,-4.511420794656040290e-02,1.200981816137991487e-02,-8.172961297591989360e-02,-7.410818337992952642e-03,2.934997658293966860e-02,-2.650421353477973307e-02,9.502683961811886348e-02,8.197356640733366451e-02,-5.667916040656207494e-02,-4.996179119205171326e-02,5.654977602699190475e-02,-3.488767125912319716e-02,6.886318956644336964e-02,-3.778286031089236191e-02,5.340762874782407910e-02,7.326347083079004507e-02,-3.654107792447461311e-02,2.609320495664742279e-02,4.126175166916104531e-02,2.486977489548731005e-03,1.030772426687526916e-01,-7.612132069107949150e-02,-1.985332939920862044e-02,1.864379703706244007e-01,-2.096794344892402573e-02,-6.151380243170675999e-02,4.903639377125332960e-02,-8.484705545445008976e-03,-7.526198608395079370e-02,2.675293011521015507e-03,-4.094772086956256080e-02,5.584912973601949965e-02,-9.062974831735537765e-03,-4.222371221472527580e-02,3.787276398855448506e-02,-8.711404558372362605e-02,8.313126365382687843e-02,-1.860040082143874646e-02,-8.348376671251066250e-03,1.894656481457058020e-03,-7.239007321254510574e-02,7.460349044472678812e-03,3.669778703488352223e-02,9.578864577240384093e-02,-6.483179851135457972e-02,1.456341957573419402e-02,9.087755936430151615e-02,-3.029360624938359087e-02,-5.863976099681649651e-02,-2.037515261223794144e-02,3.305197575475989774e-02,-3.030000751193489411e-02,-3.674385618316435009e-02,4.028804171230700171e-02,2.430485068238983845e-02,-6.569296999134763282e-02,-6.716849970025766392e-02,-5.475772074264930595e-02,-4.419263827598662181e-02,-8.200599956794839274e-02,-4.584906558630495732e-02,-1.770982348021015590e-02,-5.432203279791614388e-03,8.364459007903583473e-03,-4.630651209470997703e-02,-7.872474100517080908e-02,7.262442255925405377e-02,-4.134219086296134910e-02,-3.661986285096885223e-03,-6.756722623527650073e-02,1.092146889853204267e-01,-2.148894850536532675e-02,1.192516374941333425e-03,3.108018856577673267e-02,2.538239281601654615e-03,2.892636021412811967e-02,-3.971607664893981454e-02,5.717772611683260425e-02,6.059621729558178037e-03,4.163138349924571519e-02,4.424465126600043985e-02,3.656337463074825489e-02,6.391338880559960245e-02,-3.474144085427952749e-02,-8.937134116660057359e-02,-6.163615115633158670e-02,1.275410091876571506e-01,-4.891326385803392365e-03,-3.430695606835262174e-02,6.249834108066318339e-02,-5.115336683131355144e-02,8.869416420109972610e-02,4.801856847093873698e-02,-2.536996355324726629e-02,-7.079597916880416009e-02,-7.384871865550056425e-02,9.564485898809069186e-02,5.536730355093600481e-03,1.535485077515132482e-02,-2.116417015958958064e-02,-3.110318040758599037e-02,-1.077348349466416416e-01,-3.265905162398141659e-02,-7.254833800930853005e-02,1.177895169129919567e-01,-1.529038408425481554e-02,6.240999013012709512e-02,6.113635122777989067e-02,4.905877290154904424e-02,1.481564968046693298e-01,1.310418593573745710e-02,2.131392860274920406e-02,-1.115193661696239069e-01,-1.024930380242089142e-02,-8.125233796452543608e-03,4.760488745355313106e-02,-8.437602851799003156e-02,1.213081624607555836e-02,-3.242228489648568690e-02,1.050500926125587653e-01,-2.265772542426802358e-02,-3.026636514826822749e-02,-4.453359250070383288e-03,-1.017861656452776625e-02,4.892140586202597952e-02,1.634845142179175981e-01,-1.046594967530709852e-01,-3.735044819093158441e-02,-1.113386808944366174e-01,5.424942313299736807e-02,-1.295976524613826619e-01,8.061240425926374797e-03,-6.168644045474230148e-02,3.767228884179676246e-02,-1.625852574722867760e-02,1.022560814776284355e-01,-4.642174044983404108e-02,3.142352593409909745e-02,9.391734649314549543e-02,-3.869434796873903043e-02,4.043260052440804386e-02,8.911848180374559725e-02,3.346127610839434718e-02,-1.521956334495764944e-02,-6.641689650724509619e-02,1.965465599005031955e-02,-6.517362638167985278e-02,1.054948527827179183e-01,-6.681252428962759782e-02,1.130700535850866391e-02,-6.511270675992752652e-03,3.984249264694576104e-02,-3.748311313235262038e-02,-3.223207728288769031e-02,-6.571265904894657561e-02,4.664753638655747686e-02,3.087982439603886506e-02,4.598557129943105226e-02,-7.124540466824051677e-02,1.043691994879299145e-01,9.907633877110400000e-02,-1.683878777122416875e-02,-1.791414781262301234e-02,-3.113318269967651347e-02,-5.230259202637215638e-02,7.403774247839543798e-02,1.983099288582929054e-02,7.393349001649032748e-02,4.932555396691409855e-02,3.202991635840130025e-02,4.083651243939364489e-02,-1.127543608424793342e-01,6.777425922590655030e-03,-8.847619139379153327e-03,1.054648437542191758e-01,-8.497008289434244965e-02,8.014087224855867553e-02,-1.901019734721177273e-02,3.441529278662028984e-02,-7.575798693094437652e-02,4.485094372869957019e-02,-1.375795477083209817e-01,-6.609154653100502395e-03,2.778328236425574478e-03,1.390269062193675610e-02,3.556684086208733442e-02,1.188816068000990087e-02,-1.253200689075609775e-01,-4.604343933074501322e-02,1.174739519721097331e-01,-2.093468969476973765e-02,8.358506630103675061e-02,7.533454704457248803e-03,3.612199171904489740e-03,-1.156645697897350222e-02,-2.683348150230184526e-02,3.649381619017723344e-02,8.305083839561464010e-02,-4.333579845076081238e-02,3.455618934745906257e-02,6.999633275356130502e-02,3.982301254711168720e-02,5.752001231653858238e-02,-2.856673487141282025e-02,-6.592016747819551270e-02,7.225551705926434864e-02,7.976458967293409419e-02,-7.394995255175694504e-02,-1.837287804407775060e-02,3.485389055778331802e-02,1.074525125520874591e-01,-3.285815733870464661e-02,6.348143865492375526e-02,7.863055854076918261e-02,2.393727338770063051e-02,-4.147164657906039775e-02,-7.827608195937982294e-02,1.660883781148590407e-02,-1.212676468205423769e-02,4.428854258682755896e-02,2.837634448684643595e-02,3.207756356464704084e-02,-7.333833708171512422e-02,-1.733009216931612542e-02,1.039380121114501232e-01,1.709545343052007171e-01,6.401204416332702263e-02,3.024370350511097413e-02,1.190147954705199906e-01,9.681281312956784013e-02,-5.307194092249481465e-02,6.783505510091286488e-03,6.842214556115590318e-02,-1.701647817689806996e-01,1.429817089888471004e-01,-3.407014831111124936e-02,1.511369227196395805e-02,-8.769756341678498508e-05,3.387363230320042384e-02,-5.369383752716455627e-02,-9.668044337186992854e-02,-3.889078896144258779e-02,-7.047922620109825509e-02,-5.798585560378764422e-03,-3.892312317006624972e-02,-6.783678413009332220e-02,3.501464248203297464e-02,-4.093262410064453516e-02,-7.450966502869628416e-02,1.641792620710001011e-02,-4.789465349269128591e-02,1.390822218818044131e-01,1.233690587061107152e-02,4.845391265784679963e-02 --7.866948371180335076e-02,1.568588483922593868e-01,-1.447480319982358266e-02,-3.657968580804081549e-02,1.297514298565868862e-02,4.178705044054384365e-02,-5.239178812225961379e-02,-2.052002288204307634e-02,1.222351625093591743e-01,-5.693792349990040025e-03,-4.321197005047294759e-03,1.427510400176069949e-02,3.416570810093905175e-02,-6.825628409473627478e-02,2.446085975687106728e-02,2.504734660527066503e-02,4.214508777851286303e-02,3.199773982985634757e-02,2.732681488575530032e-02,2.925446568201924002e-02,9.808667262332675107e-02,-1.840750526111677166e-02,6.400073826574291525e-02,-5.098977062000764615e-02,-1.666945290248842554e-02,-1.230464920849071719e-01,-9.055181919465013496e-02,-1.800918147010448570e-02,-7.472323425909872807e-02,-5.214841900251991624e-02,1.562761120167765472e-01,-6.235859431055422453e-02,-7.979476295263623897e-02,5.723939077261978869e-02,8.502233206564731305e-02,8.712723867437733097e-03,1.844987721478698675e-02,-5.309206940073418063e-02,-3.908083380752749858e-02,-7.635532685994952939e-02,5.225753981753650246e-04,1.998633097145737988e-02,-1.719991363311156041e-02,9.756136867401425314e-03,-5.578713774620168431e-02,3.759121765496582779e-02,5.952851455108878337e-02,-1.121496596011169189e-02,1.609286964355241545e-02,3.676918545014375150e-03,-2.679621687235632377e-02,-3.993663135197460939e-02,-7.197490099512396378e-02,2.607161025199356691e-03,1.250031420780624292e-03,3.117785914841356837e-02,2.920921260075071482e-02,5.590265564576112761e-02,3.269857762009753532e-02,-1.120049022213039092e-02,1.417448667339395452e-01,-4.884455971480831243e-02,-4.365111176933270110e-02,-7.955771655016255495e-02,-3.606662068600118703e-02,-1.781018730112753079e-02,-5.546929732943201019e-02,-1.875311054180158199e-01,-6.793508748340111592e-02,-9.743833326415853036e-02,3.269441255502184007e-02,1.361478015819306275e-02,-1.324189101209892472e-02,-7.857073876088838116e-02,1.718352385094586154e-02,-2.540984610296118107e-02,-1.374940119444274106e-02,-2.674752215481836573e-02,8.474143510046885930e-02,3.504156675528871667e-02,8.154347751397583721e-02,-8.830903149497462690e-02,5.695667211811619030e-02,4.368906703399570712e-02,-2.901054395359409233e-02,3.401120293986155396e-03,9.329043195930511456e-02,3.188625091282918517e-02,7.906388032329074445e-02,-1.144385358919732809e-01,3.256478876557077767e-02,-6.165407767695502139e-03,3.071131270889778525e-02,7.879978505757657264e-03,3.130907472167494821e-03,6.448453394641799619e-02,-1.276798746927565453e-01,-1.318055587242012751e-02,-4.289367248324273130e-03,1.544408853411111671e-01,-1.230423252975003101e-01,1.150593331834996874e-01,-3.786219181593139527e-02,-4.668984383689928075e-03,-7.876576172410273968e-02,8.395096227500585540e-02,-4.318301941819707376e-03,4.951325895906868307e-02,2.349819641057918768e-02,1.642463413805848593e-01,-3.978399890349737511e-02,6.843368537113388161e-02,3.380330224743034045e-03,-6.522219437817104726e-02,-4.449105578916410370e-02,-8.914746995539071794e-02,-4.156005548641855674e-02,4.099867485559984676e-02,6.613855220256721623e-02,4.237742966436421760e-02,-8.283414374911636513e-02,7.109608145051823924e-02,1.492554708038508482e-02,-1.153879556137653827e-02,-4.757751091626667705e-02,-1.120713388424784407e-01,1.709835552195317424e-02,-7.414484393647824412e-02,6.767733564958272607e-02,-1.959676346258617044e-02,4.222127373483666735e-03,-9.533569514604850470e-02,-6.687892293472924576e-02,1.363184981673677965e-01,5.715719057793083868e-02,-3.809156275814169074e-02,9.288862196185020126e-02,-9.460581907689669112e-02,-6.095095559379988537e-02,2.444087604685461768e-02,-1.103902768107779958e-01,7.303117489160154685e-02,-3.999689798160489806e-03,1.068820354094035530e-01,-2.115326327832068448e-02,-1.180505710536839670e-01,-8.338432647481745785e-02,1.371747181131024729e-01,4.440351708420790799e-02,-1.499217719322288103e-02,-4.716923764606681391e-03,-1.200429950628047471e-01,2.067438410142206223e-02,1.158422169569325519e-02,-3.405426080628520813e-03,4.112782894430705438e-03,-1.588789100243206617e-02,-4.440678384851522165e-04,-7.837668927714723510e-03,8.199454639764726382e-02,-8.688379277682542479e-02,-9.591666916787527919e-02,2.176917495483154941e-02,-3.644394387370583632e-02,-5.934685496178534331e-03,3.718316265109462837e-02,-1.868155137546561109e-02,9.762927595214015986e-02,8.772343839344759475e-02,-6.631715174322788675e-02,1.061893657156662840e-01,-5.449439775070898800e-02,-5.204090754977732519e-02,1.928016119982847720e-04,-5.554809380028939703e-02,-7.793960722296819355e-02,2.096122655659718795e-02,7.142432205620194430e-02,3.842722753315950396e-02,-6.922175592175429382e-02,9.299738115441234798e-02,5.459482186036365009e-03,-1.195128743377808778e-02,-5.763761038456312324e-02,5.759073731209139735e-02,5.518039572972849999e-02,3.902359990540650581e-02,-4.406114975201894102e-03,2.056380836930245953e-02,-6.064787514402606577e-02,3.151480805530865875e-02,-2.879076772605197732e-02,-4.346248923059080338e-02,1.659691687238290525e-02,5.497299753425333169e-03,-2.209268690814196467e-02,2.929252801811155216e-02,-4.070047635620423210e-02,5.677165370147320422e-02,1.438716429056008383e-02,-8.711229877176363684e-03,4.207038264848453335e-02,1.796083063428342147e-02,-7.583978778279325261e-02,-8.171612464639521622e-02,3.187552709672278328e-02,1.411864909080087771e-03,6.878141800411755047e-03,-1.733953505820769181e-02,-5.641162855797368647e-02,6.739654233150110807e-02,-2.972414843514009988e-02,-8.305431912752893331e-02,1.121839567406417576e-01,-1.274124840157005722e-01,-6.891392248249572638e-02,3.789302147049659203e-02,-7.384957910485481780e-02,6.437183837132531838e-02,6.465474613540443671e-02,-1.046445596121816507e-02,-4.915763288376204243e-02,6.617592020691394350e-02,4.557515717045922815e-02,-5.228535426891540300e-02,-9.641104055254329230e-02,-2.285200145242789055e-02,3.805779008224906279e-02,-1.241977755460692279e-02,-5.858436369725345677e-03,-5.110153718979705550e-02,-6.850218967359726630e-02,-4.064898281192055873e-02,1.136831729034996352e-01,-3.379570020450903267e-02,4.251000615726949206e-02,9.042705297977354784e-02,-1.680906936464224288e-02,-1.295254647583916896e-02,3.665978141262477008e-02,9.386996250344482873e-03,2.710494571506895378e-02,1.045548459525013074e-01,1.109506074136556719e-01,1.614493744740753273e-02,2.436298810338468485e-02,-1.456067363970297013e-01,3.379484917020602469e-02,-7.538543609943490487e-02,-3.419444685820989538e-02,6.431210340593360164e-02,-9.809587950980858118e-03,-3.937614233146236042e-02,-4.052890855653058094e-02,4.696264028791997253e-02,1.110303717586563071e-02 --5.981629562308087705e-02,7.124595012137835415e-02,-1.413829062137151871e-01,-9.354544286204101444e-03,-2.807450325824090154e-02,-3.094992488955657095e-02,-7.153680088111491475e-02,-2.327378555897779977e-02,2.330798858689803188e-02,-6.064305273800468798e-02,5.598991175267792331e-02,-5.464038714208002689e-03,9.589748696988753540e-03,-2.195545481194858053e-02,5.791681531533984795e-02,1.965314309598324705e-02,3.560017035471051833e-02,2.442797816457445946e-03,-7.282531503383204985e-02,1.400995618563043998e-01,4.030875417288260626e-02,5.814264474863692356e-02,5.096643185530780329e-02,-4.385565802314113359e-02,1.808820519908188040e-01,2.032764090660712022e-02,-5.907067190416857849e-03,-4.316272771554263699e-02,9.079072448513580973e-02,1.369569825613596303e-01,-9.564410781423095254e-02,-1.015687762228646568e-01,-7.140565166728576885e-02,4.345886783292091105e-02,1.675132004115982565e-02,-7.049224051941804792e-02,-4.022644964918250382e-02,-1.547825996637706113e-02,4.793607115991002643e-02,7.621882740702180109e-03,9.028295938961759803e-02,-1.271092909157477968e-01,1.378276305085386622e-01,1.029130877628567087e-01,2.067806093697491937e-02,-8.074076934284643570e-02,2.550871906133351207e-03,2.977883967749515165e-02,5.482899709642064662e-02,-1.429245274771987623e-02,-5.190437962385131831e-02,6.072230093348367219e-02,1.654928176122400299e-02,3.644552961292664428e-02,2.420116433868917463e-02,-2.319190896231874618e-02,-1.095535621491537832e-02,4.839454363276489424e-02,-1.038015830014489410e-01,4.820052159856206414e-02,7.393131465259342172e-03,6.706838493613691277e-02,-1.960339016229681319e-02,-4.864579383113289829e-02,9.586552557947378150e-02,4.808853203286479749e-03,-3.362064458028430486e-02,7.708325792461788450e-02,1.014699275963173823e-02,-9.791757034846720886e-04,8.677780228228308690e-02,8.195312754327103988e-03,-1.045986275041400626e-01,2.491910610885087612e-02,-3.857010503861349471e-02,8.074397056824783592e-02,8.091268093277871609e-02,5.445008126365739887e-02,-6.673031257626889368e-02,-1.399771655102378595e-02,4.375347537354558336e-04,1.169510901755345250e-01,-2.688668013877948404e-02,-4.722444859069069301e-02,-8.104318713312863576e-02,6.262064964193916894e-02,-4.423733498805891562e-02,-3.785298879786005294e-02,-1.743469481291751166e-01,-1.746498950165716046e-02,-1.003278553844232368e-01,2.845416470107694010e-02,2.057078042683080019e-02,-2.267134178327985181e-02,-1.453847796422535199e-01,3.187385616123945831e-02,-1.200034293105698008e-01,-1.469767766304617279e-02,-5.669294875405510997e-02,2.631333898507003111e-02,3.263051062020692894e-02,-3.356046318585779314e-02,4.299763944156853590e-02,8.676702279051557581e-03,-4.630723379127201639e-02,1.053666761195636709e-01,9.553097973520696673e-02,-5.760303021877656582e-02,7.454495088167052697e-02,9.154809151975987214e-02,-1.134413760116031178e-01,-6.472736389372137078e-02,2.341502631247690744e-02,-6.821870601244030274e-02,-6.267905891993645617e-02,4.242483512132062123e-02,-1.602873447793607201e-02,5.617905936783583465e-02,-1.292651097929056747e-02,1.143969023838691673e-01,9.993824378938999387e-02,-2.633588138323701006e-02,-2.323585265803616504e-03,7.241443830114952657e-03,5.506710400152427243e-02,5.556435817750680212e-02,5.757251009598864100e-02,4.275707662616530513e-02,-5.072825646053933518e-02,-2.470183870151712244e-02,1.742562615589995229e-04,-1.107946213296479541e-02,-4.638169533895541985e-02,2.181166598000806867e-02,-9.600323608721719748e-02,-9.037722855721637416e-03,-1.289178313919140950e-01,-1.173415121987104043e-01,-1.095122704157796556e-01,-3.220813977299261804e-02,9.356941329081890080e-02,8.985287255967462638e-02,-3.249015840184765053e-02,-3.177729561034878941e-02,1.133196911200581858e-01,-7.248201038770027416e-02,-5.567060928359000155e-02,9.025371307525754949e-02,5.936160500842316279e-02,5.582108862595186460e-02,-9.603258325330076006e-02,2.059672408058461912e-02,6.479119329632965218e-02,5.266066355999299287e-02,6.674739342257728081e-02,-2.012233818143751435e-02,2.369128980357972439e-02,6.594970084438087077e-02,3.085392958060620416e-02,-3.385544735623637752e-02,-3.082767539271918933e-02,-8.897503898049655136e-02,-1.117674670246451007e-01,3.129294049296617308e-02,1.805454637202057525e-02,-1.401368274466712610e-02,6.498386543870401932e-02,5.635376407989448055e-02,5.726543221343284584e-02,3.138847697755468152e-02,1.759591282191377837e-02,6.193868912320857345e-02,6.351106165591120789e-02,1.061951513136014623e-02,4.311915049449981802e-03,-2.752515604555690748e-02,3.845775098149887411e-02,6.618850841689934150e-02,-1.017608531947419154e-01,5.527356292029411700e-02,5.194200673878009156e-02,-4.359542905606258317e-02,-2.427322339805129334e-02,4.840672504392001113e-02,3.521914868506161034e-02,-8.194895051210609321e-03,4.617162058561175009e-02,-1.898171529232072635e-02,-3.123138331478893248e-02,-4.283418302205719425e-02,2.605484653473804854e-02,7.877228840626203377e-02,-4.042354343656293092e-02,-5.221126342677816223e-02,-2.181453804868036667e-02,-4.727715708755787088e-02,1.022791878761869500e-01,5.846691552367007383e-03,5.372191866371010815e-02,4.795047893010029316e-02,-1.814907201330984848e-02,-8.263362256652290183e-02,-3.262253422842492645e-02,4.818704694173745612e-02,-2.108859525882788091e-02,5.994974770300920408e-02,1.936413714174444953e-02,-3.703086768029240083e-02,4.427303743699128463e-02,-2.283363913409667406e-02,-4.119809110618020964e-02,5.058338643840566001e-02,1.041307617489155637e-02,-5.843805448764613497e-02,-1.438349882468392682e-02,5.645616642763466586e-03,2.862383769827413216e-04,9.381730317467715241e-02,9.395533088687973988e-02,2.111047314028068056e-02,-1.693972094438362525e-02,8.352965175298540657e-02,6.094100284377762522e-03,-6.420842444984824204e-02,-1.880342436994534433e-03,-6.736683137917684472e-02,-5.746575779070804690e-02,-1.201888639863494951e-01,-4.741194228636676916e-02,3.385896140023808565e-02,1.337591133294049328e-01,4.618338629589161665e-02,4.600131605770964305e-02,-6.022570709663135902e-02,1.415909437704833407e-02,7.082991136051794423e-02,-1.148651952939203774e-03,-7.440010314279983661e-02,-6.287793732038900185e-02,-2.551832812078217924e-02,-1.265407767460647315e-01,8.365253614634967499e-03,1.936921015090117132e-02,3.965525817635347228e-02,-1.077234298246158850e-01,-1.492259222211938553e-02,-6.349009141690811764e-02,-1.357839216632847519e-02,1.081915414024424080e-02,5.600243629867396322e-02,-4.161076128022708764e-02,9.648890365197057561e-02,-5.144552966063679206e-02,-1.146930026472542941e-01,-5.992454685984824336e-02,-1.435397720519924689e-02 -7.979301247115767881e-02,-7.497778658410577876e-04,-1.523929539302528169e-01,1.175319653534381004e-01,1.905141425990685908e-02,-1.468106297639194813e-01,-2.559476621237826571e-03,2.589325673869248903e-02,2.267981128542499378e-02,1.645659436519169572e-01,-2.919466247758267796e-03,-6.957376436612650039e-02,5.456673774105447183e-02,-2.708719420104074849e-02,8.172129450816392748e-02,-1.864174468166227353e-02,1.642037258645551862e-03,4.612944868231722007e-02,5.124720024910956601e-02,-1.737332151738147853e-02,3.439735695096118873e-02,-9.366143307785397554e-02,-3.603602080312854478e-03,6.011891289790959048e-02,8.621646300909523164e-02,-3.573063396665441399e-02,4.206696959466793562e-02,2.113270676520523708e-02,-3.110692611138463765e-02,-1.287719466289274639e-01,2.279176981006700298e-02,-3.311159415110827980e-02,5.781418988662182651e-02,-5.541254391531970358e-02,2.818823887551462348e-02,-4.406082368376976026e-03,-7.517711238747712410e-02,1.897712189901957711e-02,-8.975920397143264534e-03,-1.633745556266184840e-02,-1.484552797775221498e-02,-5.187759992302936468e-02,6.337379880477098104e-02,-1.171887629915571022e-01,1.869757455798350121e-03,-3.561418924508545636e-02,2.002247082030728098e-02,7.972097157402309453e-02,7.899932152019305265e-02,-1.364483784577757153e-01,-1.459983445876354008e-02,7.249085471127861280e-03,4.580320996121933774e-03,3.484559188663031298e-03,4.101621795870606657e-02,4.856806754029049239e-03,-9.979544656743305486e-02,-4.643938157454464977e-02,-2.951698227114989137e-03,-2.399389848032653857e-02,-4.401872265715619376e-02,9.708666228836142220e-02,-4.823850439610041785e-02,-8.897247130051594288e-02,2.978017214101053717e-02,-4.927958246157833344e-02,-4.504402979734810353e-02,1.259876045292288582e-01,-9.913233785807194082e-03,3.568972179284962687e-02,-1.073087990696763305e-01,3.346924745071656881e-02,-9.111693438826232172e-02,3.160434949048364361e-03,9.352079497301775401e-02,1.241515409673101845e-02,1.014152066787931128e-01,-6.694158570668084368e-03,1.493067068508827783e-03,-2.691120553528326270e-02,9.431549523586039985e-02,-1.321723271883640527e-01,-1.773165585670478592e-02,-5.987722788087254666e-02,5.542246371867397270e-02,2.037438929563569270e-02,1.288150827178923159e-02,-1.465962707143257659e-01,-2.686541853376583910e-02,-3.034571286938946705e-02,6.842439528746424582e-02,-4.358283628516416985e-02,-1.681903654345688962e-02,5.773727830861412336e-02,2.295334189125775026e-02,-2.935585675713267909e-02,5.372497790369459153e-02,-7.294670239833248027e-02,-3.224558933265890548e-02,6.750147185547346329e-02,-8.681650020445966232e-02,8.372776892154540351e-02,-2.184570474920644392e-02,-1.485121157577663602e-02,-1.395651465036517880e-01,-1.321731245047201297e-01,-5.935389036442436511e-02,-7.579609519603254519e-02,5.930864288249718119e-02,2.892834196126054316e-02,2.205864968829073305e-02,-2.444052502515136130e-02,3.918534804402502847e-02,3.985201019712107662e-03,1.164174153890672192e-02,-4.182069946484321538e-02,-3.238158951441748312e-02,-5.520699012191668253e-03,-1.207679273829398992e-01,-4.450797351951996106e-02,-7.648343108760277675e-02,-9.028313962443809682e-02,-2.352332072858432863e-02,1.553424644419005574e-01,-1.590791301280285175e-01,-9.966501486508835828e-03,-5.367352764096755199e-03,-2.074841302727620934e-02,8.262634862996472151e-02,5.687985743980868147e-02,2.609594167919845539e-02,-6.810823714266922568e-02,1.111278027050107420e-01,-1.210940594200145687e-01,-2.336208204679052908e-02,4.047374077536356959e-03,-3.154506076829481642e-03,-5.817124375809567399e-03,2.233309914276907990e-02,-4.142999535610106254e-02,6.858321581630199421e-03,4.082428667548240742e-02,-8.533234248534139754e-02,-1.065689398449968378e-02,-9.056772449037693573e-02,-4.670148932559312072e-02,3.829097993402232059e-03,1.041202476938708765e-03,-5.689610109703434337e-02,6.177404493906049621e-02,1.743596601264028878e-02,3.976436916246096215e-03,2.407758890201188232e-03,1.359080468450408336e-02,-1.000978504460150742e-01,4.617457292009057024e-02,1.050011831784587180e-01,-7.061671954244751193e-03,4.287008399061938840e-02,-2.772521325186549576e-02,-5.676012276866434364e-02,-1.416205285871580122e-02,-2.353332154558221423e-02,-1.116695458249165537e-02,-1.057458932218008352e-03,-4.609981346988194922e-02,3.051468977669180311e-03,2.077064567161196126e-02,-3.122027290713622755e-02,-1.302928807870364081e-01,-1.835239970078961572e-02,-6.924880255141323271e-02,3.342455875790485004e-02,-4.280103545417558025e-02,4.181474056942982576e-02,1.466237411584795855e-02,-7.433380785332958235e-03,2.070601262232111411e-02,-3.056515731879174552e-03,-6.706102048178888553e-02,9.531821715537509843e-02,3.806490215093837753e-03,5.426833152544136007e-02,6.216289723857366945e-02,1.493274481993714028e-02,-8.259094925159105038e-02,-1.194770035193864072e-01,2.811479219290997228e-02,-9.972767777593250643e-03,-7.288508321140942797e-03,-3.822906888819373877e-02,2.804479507486429962e-02,3.014357445396378260e-03,1.127142222489866119e-01,-1.205719258563340071e-02,-4.864170060096634912e-02,1.795263784776754759e-02,-6.219102425473838763e-02,-1.622442887547759782e-01,1.284530925724464423e-01,-1.239227569565142656e-02,-5.523161201182229252e-02,1.887401150886133458e-02,1.347636825473127464e-01,-1.226087105148431292e-02,7.648884787528893725e-02,2.178687745672314158e-03,7.233520505173610116e-02,-7.902327471638442846e-02,1.655772495048945386e-02,-1.117718945240914197e-02,-3.879722727469818444e-02,3.354077343009539969e-02,2.296152805709261030e-02,1.060092197747201370e-01,-3.519455083241335158e-03,6.784366188810262571e-03,-1.635850391290202821e-04,8.549103818616621275e-02,2.278704309521573795e-02,4.954549435794255302e-02,1.886001395468125605e-02,6.449036207076597194e-02,2.376128428285647493e-02,-1.021703949214269325e-01,7.067246265318614939e-02,1.996703255513497532e-02,-4.399767706155311570e-02,2.306982477227987466e-02,6.545502102796058042e-03,2.866548954898360049e-02,4.911904310156825493e-02,-8.272214668272213911e-02,-3.776380802346384485e-02,-9.386000136666357296e-02,-3.657546500795515881e-02,6.099473475934667765e-02,-2.566364096767492081e-02,-7.742427892771235054e-02,-1.956546813604509091e-02,4.686862676695226909e-02,5.766825957982750495e-02,-7.935835897081616047e-03,1.179809793857954925e-01,-6.945785342713282906e-02,1.390758356130211426e-01,6.546809371685374801e-02,-6.689369745191935113e-02,-3.633637586241611483e-02,1.019812380650115050e-02,5.129870782605329416e-03,4.582920267942717368e-02,-5.961545328179877062e-02,-4.502491532154205556e-02,1.500189554589489299e-02,1.333531248547751569e-01 -2.650647693137897151e-02,3.220913711067917296e-02,3.306396134917904961e-02,5.503895973979433531e-02,-1.436946744421612743e-01,-6.274778322361562743e-03,7.685772688847644640e-02,5.707728453305030425e-02,4.037469294670795183e-02,7.309100052235892320e-02,-8.841954741296315090e-02,5.953940075494254525e-02,1.891140780997357940e-02,-1.088887380810153460e-01,-3.517766089093404130e-02,4.382177872062220647e-02,-6.290606519293528986e-02,-5.968414278725908684e-02,-1.182993155313764033e-01,-1.007164848774797228e-01,4.427051785814524465e-02,8.984864211990791633e-03,8.371994436848693294e-03,-2.763495749558026598e-02,1.106035452787774709e-01,7.798292751416460722e-02,5.620113482737748578e-02,3.307155273124950945e-02,-7.079194865046523066e-02,-9.663887823432221191e-03,-5.304758675863175538e-02,-1.775265784055103657e-01,-5.530953282393517856e-02,8.622553769072832663e-02,2.514873368237674489e-02,-4.342990479851424612e-02,6.763076143526643158e-02,-4.147386272676072128e-02,-1.065765901059528009e-01,2.457728408440235796e-02,-2.921297357423593977e-02,5.736809115145269367e-02,4.102669951137767612e-02,1.090718509682877257e-01,-2.242097820771817365e-02,6.248554202808614261e-02,-3.508522962155018882e-03,-9.919935132475818373e-02,-2.675191247822620438e-03,-6.075575473655128733e-03,3.600346619256888925e-02,-6.288971795793973918e-02,-1.097191235176276924e-01,-1.462842438216005181e-01,-1.913212516544766581e-02,-6.107828528970089127e-02,9.474108701348047529e-03,6.789738348701639115e-02,4.423917190709575169e-03,5.148771388707391872e-02,5.906654665530720882e-02,5.254852827500231666e-02,6.793892252757555150e-02,-4.499914201940263120e-02,7.663145519484927670e-02,4.918809918199089359e-02,7.095490623043694545e-02,-2.577851705936735557e-02,-2.869858209331432874e-02,1.482287698636162443e-01,4.210802879748747063e-02,3.303620968717402295e-02,1.156923056330110833e-01,-9.044855201998909522e-02,-2.571876616571699276e-02,-3.950697295522664215e-02,-1.674885441832091662e-02,-7.003600644530905139e-02,6.952106143079415934e-02,-5.398256789945102968e-03,-3.431326396631807774e-02,-8.689249199280206504e-02,-1.818556387910092928e-02,1.093107633545442048e-01,-2.915398642971504112e-02,-7.132707060752646600e-02,4.693162673546794061e-02,7.033832078301396895e-02,4.929852658750712158e-03,1.049599801440065372e-03,9.596702868921686003e-02,-5.213773975609833911e-02,1.367032499810464513e-02,-3.339383292286359634e-03,3.282446663177489288e-02,3.745862864498773759e-02,5.635053395203303006e-02,-6.734897684818608177e-03,5.211252000361261960e-02,-1.814124468281577696e-02,2.473897009874142378e-02,-5.462475695144460713e-02,-4.292822121410507646e-02,6.161925659717901183e-02,-4.116561445521747103e-02,-4.886657714445016842e-02,1.074411209668228590e-01,-6.685706804491450828e-02,-6.276553352791190699e-02,-1.056502555557060768e-01,1.851298367015279632e-03,-9.588664934419126318e-02,-1.014408001452020425e-01,-1.184708721463433728e-02,-8.358703062522666177e-03,8.816407871767751800e-03,-3.422159632847901478e-02,-8.068701243807974155e-02,-3.479325542738972293e-02,3.372310942689100977e-02,-8.244884969425336774e-02,-8.751276269279620323e-02,7.166144827411324858e-02,-5.380408421757290616e-02,6.254498988585051322e-02,6.280828987652233020e-02,-4.664255127308430948e-02,9.248120415346113704e-03,-2.520039884433894831e-02,-2.995948958805261242e-02,-5.845059577451064414e-02,-2.316629738942857855e-02,1.006648218749622847e-01,1.341316285718096094e-02,-7.641117647035702842e-02,2.597469306035076614e-02,-1.772409831284615239e-02,-4.484285491714376515e-02,-9.200139032533444228e-02,3.045853818433318094e-02,-5.375189368822175934e-02,2.236039192772071998e-02,-3.075382375620005007e-02,-8.663186513777326736e-02,-5.404254684394840108e-02,-2.018630111219932299e-02,-1.717941167626258966e-02,7.796969967232857124e-02,-2.526316372650074318e-02,4.578040756260674110e-03,-6.947394724793534704e-02,7.064466676502395917e-03,-1.765647544289531898e-02,4.668270772492554704e-02,-4.389597617846817401e-03,4.928397840709732719e-02,3.061467153688916115e-02,-5.278780250685140285e-02,1.331355708771857216e-02,-8.381527568860140665e-02,-4.692543911330171458e-04,3.542922977564268855e-02,-2.945817705078703636e-02,3.784005508147383207e-02,-1.997657479878088560e-03,8.560616005358011316e-02,1.247894870900127448e-02,-1.225749501879055403e-01,6.486916752064396385e-02,-2.914074317344888393e-02,6.238958084584773911e-02,-9.649922077264500020e-02,7.039361180355563005e-03,-1.225569064024513780e-02,-4.592522177499035879e-02,-6.735511658557082060e-02,-2.255907629210824467e-02,7.043677335637756227e-03,-8.544209864907251206e-02,3.854550355532617573e-02,-2.284218488082841170e-02,1.378730911558109784e-01,1.763786403640037886e-02,1.246583453219947402e-02,-3.508880948752629569e-02,-1.062511925481445518e-01,-3.865261687985341310e-02,1.954710509529155801e-02,5.270879612345740622e-02,3.569842589988800330e-02,1.036179979304882592e-02,5.466093463134363145e-02,-4.030392844378730133e-02,-1.233468484008422295e-02,-1.601205550636800867e-01,-8.440569913170024169e-03,-1.579565808381377667e-01,7.099319033043866023e-02,1.514541028722289628e-02,8.564989383304205672e-02,-8.323741839650802832e-02,3.310608968895195452e-03,1.342175084242324679e-01,-1.333174128399363875e-02,-6.721134346076949873e-02,-5.964100706807790031e-02,3.575174369581671924e-02,1.782528439999164771e-02,-7.439854816764357448e-02,-6.109001416640724019e-02,-7.287262290868906645e-02,6.620305170016012319e-02,3.618020566004127164e-02,-5.080308307526846395e-02,3.945043788300590243e-02,1.003252970564733199e-01,-2.353464187644347533e-02,-4.064251122494819124e-02,6.877714899313625008e-02,3.834341109675187820e-02,-4.809947116519472377e-02,-6.300595260379318607e-02,-3.994820315857360027e-02,-4.612799707812732797e-03,6.332039127016569902e-02,4.927364085346629813e-02,1.510080107084113277e-01,1.116687798827997909e-02,-7.530166281616426649e-02,2.898644542038200309e-02,1.221810681747581284e-01,3.409651762297412347e-02,2.875730782596677693e-02,1.854011928750818466e-02,-6.471145758545498311e-03,-3.664440421875135231e-03,1.625537282787745466e-02,-1.282015974735857267e-01,5.840147742370140060e-02,-2.730078077186803495e-02,5.499987136622465356e-02,-3.279066876885383675e-02,3.238886724219314228e-02,-4.378914565090065263e-03,-8.582804239549073066e-02,-1.062608820933717488e-02,-8.825039294773835696e-02,-1.388190624219945782e-02,-1.842982056462149631e-01,5.623053718261904199e-04,-2.281143091772183662e-02,-5.686038118159761345e-02,-5.330280578443560846e-02,-4.138169809586159098e-02,4.308684025896444142e-02,-6.256025778343549115e-02 -7.721641591809896843e-02,-3.991230715490144687e-02,6.779551846497650081e-02,-5.463516326934395040e-02,-7.137370498160106047e-02,-3.322118752856761947e-04,1.772673914995318359e-02,6.609938292795003445e-02,-8.438481291770436044e-02,-4.908562521459150507e-02,-1.263510237147674742e-01,-1.432704631765874259e-02,-2.999265951702792185e-02,4.388121013865806108e-02,1.107958407916427024e-01,8.629911751440762224e-02,-6.101205246072399779e-02,4.984274798640010024e-02,1.333796454417902032e-02,1.520898462750627095e-02,3.522277574371690534e-02,-3.552059837136625764e-02,1.544922752935576349e-02,4.802457454353668137e-02,6.298138603265990981e-03,9.080056278955761506e-02,-9.433496010711445168e-02,-2.424292985764655434e-03,1.115117379995329422e-01,-1.435546634579130121e-01,4.731707079556298629e-02,5.365392953058939485e-02,1.498694932939102498e-01,8.180377211849768071e-02,9.276417759400741592e-02,8.380578444953321049e-02,1.491894580297383199e-01,2.098979577460326554e-02,-1.955076433587846219e-02,5.736784106566523295e-02,1.024291048972818946e-03,-1.127358432066419697e-01,6.341891009625552633e-02,8.626126289755840781e-02,-1.141725826846649340e-02,-6.837125693979496932e-02,-3.607388331012275490e-02,-1.110924683111006728e-01,1.647592801261578768e-02,-5.778247677656220271e-02,-1.620340737604104159e-02,-1.832557183409073628e-02,-5.179920959694127303e-02,4.270962400574836171e-02,6.974416722585181916e-02,-3.403755174088762131e-02,-1.333353072657100236e-01,4.284638571280075664e-02,-2.595809995267789327e-03,3.009429463356871198e-04,3.670791519689453608e-02,3.886701916110311093e-03,-5.230098566205594601e-02,-3.709767952426844956e-03,1.519474806894553602e-02,3.181943493105955645e-02,-6.533597812101595015e-02,-1.864175658627516621e-02,3.552573433076344767e-02,-7.558830391989626585e-02,-7.725906615669791977e-02,-9.211041194226516488e-03,-6.053744373976745891e-02,2.021547524779918059e-02,2.100752435201275192e-02,-2.780937089892291172e-02,1.130312025769247136e-01,-3.426330455904352812e-02,1.007422895504318655e-01,-6.750473916245172679e-02,5.404037363593017229e-03,-8.673561255962757705e-03,-5.670000056138642386e-02,4.749066847509855055e-02,-2.879693758124336062e-02,2.321123389550633007e-02,3.268749815959010735e-02,5.800798023322913805e-02,6.093665681359667491e-02,6.673285248123153601e-02,-3.399756410099115389e-02,-1.022900619666067823e-01,3.656268998909965873e-02,6.125593338448775266e-02,3.299610196632342396e-02,-3.846887837041636921e-02,4.674609375187724347e-02,1.059215456577278225e-02,1.331175961080371083e-02,4.010548782601413426e-02,1.808716686621621161e-02,8.698869385814159139e-02,8.278212276723859175e-02,3.413291536911496060e-02,3.928976650829874018e-02,3.755334548121939953e-02,-6.653808155291957016e-02,-4.878571543804886834e-02,1.430137249353816489e-02,-1.014034168819213932e-02,-1.326316172038670936e-02,1.439846741367368788e-01,-2.607722204692316734e-02,-2.970948623545841688e-02,8.043057447668568172e-03,-9.169967568272827108e-02,8.594445769082423303e-02,-1.163810708200996580e-01,7.250381635347477594e-02,-2.855928870233375230e-02,2.432365227210507921e-02,2.802141103632650732e-02,-1.988345314229179178e-02,-1.243174620764264660e-02,3.218255651617261265e-02,-8.725731692623002467e-02,1.658859086340753672e-02,-3.996246825678433090e-02,1.509599866269467627e-02,3.524773827262490944e-02,1.865945874318722039e-02,-1.213023742382631637e-02,6.195027870110805107e-04,1.265136708370873282e-02,-2.190654105541032029e-02,-3.311387272783451741e-02,-1.581983437171308182e-02,3.188141153010396606e-02,-4.972770877827873554e-02,-1.310655740987968776e-01,5.877245998791968962e-02,-1.459366192268475929e-02,4.576702388720189224e-02,-9.272888283337382687e-02,1.313691892670490191e-01,1.309397710775241364e-01,-1.024377568065934774e-01,1.538937489827765619e-02,-1.774514045638995452e-01,-4.093145727828675223e-02,-6.689425483808718320e-02,7.614616224056833249e-02,7.724361742098156958e-02,-3.128171253390304996e-02,-3.853966141155713843e-02,-4.817376547377347218e-02,1.904161291652753385e-02,7.796255841348063986e-02,-3.394076531100188171e-02,-1.387239019315348980e-02,-8.884209504905019039e-02,-1.725468127124486772e-02,2.310914059643993934e-02,4.906465952784048318e-03,1.170385891937801015e-01,8.317998529551555442e-04,-2.076042877271047637e-02,6.647174824547065486e-02,6.193993582161811534e-02,3.397008345516305816e-02,1.493429965200137634e-02,-5.682749159548337176e-02,5.171186521958693640e-03,-2.344444712074147258e-03,-5.796285313128609479e-02,2.100131866845353357e-02,3.245946099815273655e-03,-4.237115240195658827e-02,-8.795487164496498811e-02,-2.263068132124564091e-02,7.424872875064678079e-02,-2.164551677823859169e-02,-6.198748409677012211e-02,-4.045609940522845033e-02,-1.028842925653493390e-02,-6.792981917875438869e-03,-9.013991107132407823e-03,-1.432936799236315256e-02,-1.060829295151828749e-01,-1.575563156039078497e-02,-4.494046747412699938e-02,-3.901244931887699191e-02,2.105364199352143667e-02,-2.640151491768966130e-02,-4.696277779002558384e-02,-1.095691216830461667e-01,1.205421600835183266e-02,-7.321559714142650277e-02,-8.973002403946753192e-02,1.995819190751677713e-02,-1.151083248394525055e-01,-1.945698783608941843e-02,9.268010856937812514e-02,-7.109737516510051397e-03,7.105505639303386078e-02,-1.136590565735668273e-01,1.927482153968122086e-02,-1.901651979996706598e-01,1.565746854659812171e-02,-6.232190622770731464e-02,-1.832823044042903443e-02,-2.936094374217222691e-02,1.367444878642379846e-01,-5.431624999967631096e-02,-4.801132249740638902e-02,-9.596277353545190625e-02,-7.285980741219426349e-02,1.371558839430589427e-01,-1.370193540022565312e-02,6.083036349317559599e-02,-3.180633096884001587e-02,-7.493303744306364922e-02,-3.395527338986217142e-02,5.628052693834694614e-02,1.185129996434834410e-01,-2.301746200435394324e-02,-4.604733594378826456e-02,3.504568999824749648e-02,4.358637766954905762e-02,-5.456636017788727908e-03,7.588368474893922300e-02,-6.442514449682891498e-03,-8.187043689402548341e-02,4.437265995586488904e-02,-1.395805576512437404e-02,3.605824503231697814e-02,7.322628727513313951e-02,3.246351126031565837e-02,1.102964751039011565e-02,4.978218996146976938e-02,-3.253932049400518750e-02,1.019024931897912628e-01,-5.605496508369453568e-02,-2.812886025355562994e-02,4.962733608517936462e-02,-5.904738976432442193e-02,2.074062141333658446e-02,-1.006374005453382521e-02,-8.373750125982924730e-03,1.715917334232593405e-01,7.007618578072366544e-02,-6.241996127144058826e-02,-1.518065493785735083e-02,-3.178084231567741141e-02,2.215195717749121954e-02,-5.041482422098468585e-02 --4.352881342496252998e-02,-6.753573639550583541e-02,7.392957048876605541e-02,6.458884834435746347e-02,-5.966299642153156946e-02,5.732762464528529112e-03,9.828965268145778422e-03,3.279335709566424950e-02,6.534275231739763989e-02,-1.466473973055934521e-02,1.088716038481987725e-02,9.109089193867381251e-02,-2.902459908893065266e-02,3.052623729228640115e-02,2.981641901993839494e-02,7.065747361506982682e-02,1.714730093561692148e-02,1.888099146772471287e-02,2.080484919439218316e-02,5.438617881337099585e-02,4.731592748157688322e-03,-1.203481177622415660e-01,-6.206759942656938506e-02,-8.095212781417610026e-02,1.610719565513604387e-02,1.096873565287185687e-01,3.539274207592416760e-02,-3.118703638861603303e-02,6.355161492997485340e-02,-2.167563219239770217e-02,3.177282894591246937e-02,1.127186777175268051e-02,2.776004733261077176e-02,1.187430867666461332e-02,-1.258117482553306431e-02,-4.311687365480352391e-02,4.256951203910205178e-02,-3.589196127521113167e-02,-1.148253903181513380e-01,5.166270577570754308e-02,4.709343100015979389e-02,9.434572844134496428e-02,1.311052489225444061e-03,4.448427260224405155e-02,1.295112328867887089e-02,3.361997655016932263e-02,8.516458471175920864e-02,-3.391162480389149636e-02,1.310055876324154288e-02,1.700844228490423493e-01,1.099405858481911219e-01,5.561913845744909102e-02,-1.115328454826351560e-02,5.465982793356992386e-03,9.121111492571312707e-02,4.261812342083527543e-03,1.001900442696194993e-01,-7.169807041615655629e-02,-2.646035245959241916e-02,8.954594235679437519e-02,-4.503603908106861703e-02,1.980483323936004306e-02,6.368491990593623531e-02,1.729784484393908084e-01,-3.601906905957484584e-02,-7.683095490931156346e-02,-7.413124475022439541e-02,-5.477907242893587103e-02,8.928877375412935802e-02,-2.602723801719592986e-02,8.010523233138338792e-03,-5.230915472011425649e-02,-2.721358795355249827e-02,-1.627850899642814542e-02,-2.015174604749157133e-02,-2.447613195371841840e-02,-1.961734145962709674e-02,-2.625821446453765334e-02,1.768111361620419361e-02,8.645736154538298524e-02,2.002025081094517545e-02,-2.882080681562807639e-02,9.299336049510364965e-02,-6.656161217394489238e-02,6.547311628869857680e-02,1.140582233862201672e-01,-7.507006842554347914e-02,2.213832536861708861e-02,1.115123002053725054e-02,-5.428660260008962013e-02,1.711455646872556829e-02,7.419346355121052283e-02,2.680431768495412925e-02,2.340064554129551305e-02,-3.408358986286584569e-02,6.088436860669533407e-02,3.821090952107360911e-02,1.308906697500833993e-01,-7.593534985006551025e-03,-6.897977757100443030e-02,-1.835858616486321648e-02,-5.681636719479840614e-02,2.624874750335469062e-02,-2.115654889329148317e-02,-1.393832077226010435e-01,5.918773149589995342e-02,-7.945622843470794006e-02,5.416338544354086515e-02,1.700383420627615133e-02,2.111431013646429544e-02,-4.108799754170099766e-02,-2.917103958290603871e-02,1.874682054500999631e-02,-1.216378325225257392e-02,6.685349384829954514e-04,-5.008788560452276384e-02,-3.328466009660607794e-02,6.077916115708264372e-02,1.487206181474931554e-01,-3.883119569967306839e-02,-3.309631976608655435e-02,1.004110539795082524e-01,-4.233645961096419580e-02,-5.883234342246596721e-02,-1.573345367753119506e-01,1.933213149072404019e-02,1.442174696949741035e-02,-9.779143371548334324e-03,-3.875884215074509864e-02,-1.668367973025014495e-02,1.171075678565575706e-01,-2.182310767234810905e-02,4.887488232354532391e-02,-1.237145458588131408e-01,-4.154424324622806031e-02,-5.205634158117845162e-02,-1.109146608182382671e-02,-6.168107278980367730e-02,-5.247831887670070805e-02,9.314344411718925021e-03,1.878214836397814475e-02,1.101722916367674776e-01,-3.987325966240979486e-03,-1.340907922700367294e-01,-5.494860290790780366e-02,-7.790994015695694441e-02,-1.046461134331316822e-02,-2.576153771085953198e-02,5.335227733743069106e-02,-6.359462166265966027e-03,5.705041744812289206e-03,2.306086993897236287e-02,-2.468779690189078571e-02,-2.870571810829283810e-02,9.198125828818490279e-02,-2.415572803862861551e-02,-1.339624497071371656e-01,1.137505109160484168e-01,9.975863754800690444e-03,2.761120223600313506e-02,7.584532104862416746e-02,9.873632469678383927e-02,-4.273822872775789178e-04,7.332174640826329137e-02,-2.283313689161552834e-03,-3.904063447574731421e-02,4.722800639233967063e-02,-8.163892195199237700e-02,-3.330588910351044152e-02,-3.266884874437456926e-02,-7.606164268717630073e-02,-1.863874562005498822e-02,1.820026545505208995e-02,4.669769490635839598e-02,-1.303325682342722225e-01,-9.875275539613904974e-02,-6.113509473514078252e-02,-8.405912220161537718e-02,3.431682747540784617e-02,2.714590548201828782e-02,9.743739631460603817e-03,3.106869763861804662e-02,5.324143019505545027e-02,-1.486401359323348895e-02,-2.769315888859393698e-02,-1.221791368948653075e-02,4.269145967759058885e-02,-9.218351458817250488e-02,-1.199972739151300705e-01,-1.584054755267681053e-02,-1.208819608353898234e-01,1.106088004218424431e-01,2.792741319192840188e-03,1.042513037427063027e-01,8.737095560407656669e-02,1.279740399936049253e-02,-1.505152303785460555e-02,-1.639543661315069345e-01,-4.785147061771652388e-02,1.806826138830679163e-02,3.792769120291358993e-03,-7.317073436555923682e-02,-1.724778583605891813e-02,5.159093239871723191e-02,-1.678043029703990552e-02,-6.043558357379374968e-02,8.717291835964726537e-03,5.072626162533917099e-03,-7.340329187670306677e-02,-2.230802677391176270e-02,-1.115466598288589531e-02,-2.686700255080214095e-04,-1.902777841188134367e-02,-1.051031388767902086e-01,3.637325415604533707e-02,3.159208991472676684e-03,2.931214225231515771e-02,-1.914521748103503382e-02,3.014151599361009765e-02,-5.887726043594859737e-02,2.218961669435619288e-02,-9.435996390398622424e-02,-3.484276565725501207e-02,1.926331602721230440e-02,4.172094392438965155e-02,-1.933104913569565556e-02,4.490594910738475443e-02,-2.045003488765539945e-02,1.827108614916235496e-01,3.550719752543061991e-02,2.231713372883166804e-02,-9.381888141278463267e-02,-4.006880539120579593e-02,4.720425861384005412e-02,-8.929474004257514919e-02,3.837388883992855215e-03,-2.662156065636144681e-02,-5.490683985514539095e-02,-1.964327424548380496e-01,9.139658906429612428e-02,8.133281432001296385e-02,-6.937713689978433629e-03,-3.320411911476472272e-02,6.970492270071731999e-02,-6.654552775094285666e-02,-3.580206523770505800e-02,-8.393151544745791615e-02,-2.284922860510643638e-03,-5.921074306272249010e-02,2.436229088777954258e-02,1.950694088294322934e-02,1.064121305748195606e-01,4.198104159840034683e-03,1.554280676837914953e-02,-7.910945184307290046e-02,-1.669844322101717016e-03 -5.975862339473387436e-02,-1.049999481398553730e-02,2.056432111378957150e-02,-5.049159864264321507e-02,6.946431442109365917e-02,1.198627303198174232e-02,-7.217059840431097606e-02,-1.427289618912765470e-02,3.518238744679963448e-03,5.851452345181713810e-02,-4.233416476759715996e-02,7.788084123940461345e-02,6.275886856990187113e-02,-3.124096751405083805e-02,5.424207197914199252e-03,1.046024403635052169e-01,4.616398092099260192e-02,2.217472841269038653e-02,-2.346170489232343725e-03,-6.341009486777979265e-02,-8.337727837247825569e-02,5.372158948857854582e-02,8.446503080658625262e-02,8.626552190596825653e-02,1.722081427512847304e-02,-6.936865415848710026e-02,-1.093142937401789389e-01,8.181000918079463224e-02,-9.084814762290847889e-02,7.764377099207346589e-03,-1.170372215072180699e-01,-1.231274314901255051e-01,9.931767045979153496e-02,1.526542590250855591e-01,7.175775287622301879e-02,4.769101993692659303e-02,5.886029599741181284e-02,2.274083763729533095e-02,-4.483677246192724342e-02,5.504756141567172106e-02,-8.958522093016071941e-02,7.046136260452258437e-02,-1.313047557174608013e-03,-1.445768072395109960e-01,-2.202036273449646878e-02,-4.716119179302843967e-02,5.715972261309251734e-03,2.780312375646504242e-02,-4.641116042080331572e-02,1.508362230101811807e-03,6.223662520377211405e-02,1.391470034332313588e-03,-9.874021242511401772e-03,-5.719494693067467272e-02,3.391445459118808819e-03,-7.925481719201128705e-02,1.101729318246242456e-02,-7.428234026644001120e-02,-8.778344717137925857e-02,9.948498530453991906e-02,2.839296722433905587e-02,-7.598312357245110005e-02,-1.322967599175515485e-01,-2.099401933635890172e-02,-8.698005172534906473e-03,6.099027229859188548e-02,1.010924582766627167e-01,-5.035667017873798890e-02,8.765507594190660723e-02,-1.646594500607815106e-02,-1.111466952960353249e-01,-4.216840414240476120e-02,-9.938421822931930993e-02,-3.536963288170756614e-02,-1.557751509091863940e-01,8.913244622376993720e-02,9.107502743183264887e-03,-5.961121391566157057e-02,-2.101010101708038896e-02,1.882473740617983393e-01,1.117047054836061426e-01,8.872890616374166928e-02,-1.990349955078451333e-02,7.676427756399153979e-02,1.150403715752187916e-02,7.731026474759733480e-02,-7.049222859439170652e-02,-3.822315162791287135e-03,3.850732270021230141e-02,-5.670434765596613080e-02,2.106827871114917358e-02,-1.040699348072962466e-01,1.383234862547500221e-01,1.062733359758379487e-01,-3.987681013288951748e-02,1.497005220816854422e-02,3.595102731891551898e-03,5.248240752405199799e-02,-2.304471427434387487e-02,8.909638708020081732e-02,3.424747382887963904e-02,8.789993288432513402e-03,-1.950604348684061365e-02,-7.535307951009083405e-02,2.603725544746617049e-03,-3.985107820339683221e-03,-6.101645974555095636e-02,-1.597049775102556413e-01,-2.815977295251201329e-02,-4.320933045703999331e-02,4.000368492527597852e-02,-3.523114063956214737e-02,3.004550438055772488e-02,1.548062406874507191e-01,-3.882467873280651821e-02,5.296084621554726646e-02,6.705171116263047848e-02,1.121770498743236283e-01,-5.409209791762219222e-02,-9.420420804164930662e-02,-3.713863919939682651e-02,2.460493132435501162e-02,1.944360550091962098e-02,4.833973772535271607e-02,-1.024248936476353233e-03,9.260154170543466456e-02,1.313444728602293846e-02,-1.290092673782545010e-02,-9.288963690870902928e-03,5.135292270832303441e-02,-7.423333006618917929e-03,6.836977759243631492e-02,-2.580989753396103689e-02,2.013234689463722776e-02,-8.082510860259106700e-02,-4.064573750425109350e-02,1.487333807266895055e-02,3.623746001409745843e-02,-5.941598969999670882e-02,5.033539562066578071e-02,-9.039108404578613531e-02,-2.477978953276329874e-02,-2.160530174150560728e-02,-8.873229199017479418e-03,4.823783780116450842e-02,-5.969414712562573500e-02,8.977367232075983416e-02,-7.104192660382019353e-02,8.914862228084331763e-03,-5.301935716784481056e-02,-6.282295231736725882e-02,-1.339760030277671321e-01,1.336203740768492770e-02,4.782576017442422694e-02,4.481527598735254858e-03,-3.230246343069649584e-02,-3.224437098319413858e-02,2.554104114977161222e-02,5.934383195550688161e-02,-5.079598114251489271e-02,-7.826974490139032570e-02,-1.519952317691874999e-02,-6.493022892974945059e-03,3.225131877298054867e-02,8.111827043283752203e-02,5.141480719223198714e-02,2.471706227593245542e-02,4.341985390881653722e-02,-1.207182771456083263e-02,1.129260697824481866e-02,1.563930530834855073e-02,4.336967626994779612e-02,-4.749002594047211506e-02,-3.140076678423367289e-03,4.958231225135446840e-02,-7.886186506184729439e-02,-8.350676579429484880e-02,-7.349634106990583782e-03,-5.235532128356026882e-02,-1.054844429516742788e-01,-5.311245137431945540e-02,5.991571694790898128e-02,-2.848490850160347032e-03,-2.410251359404494420e-03,1.964723430208921429e-02,-2.530332974325831541e-03,4.735243410460468483e-02,-1.617669811307492767e-02,3.580188346573143221e-02,-2.528486113064147209e-02,-3.100176764952295924e-02,-2.959889353742535881e-02,-6.810702474366397974e-02,3.891298016463526355e-02,8.112628887918393872e-02,7.682828706266389175e-03,2.700750862770123772e-02,-1.041943966721470272e-03,1.048754251669462359e-02,3.607475321487614919e-03,-8.820957338865821618e-02,-2.705880745577733670e-02,-1.034578053335164990e-02,2.217584611458660662e-02,-3.085841178219308686e-02,-1.993795734394067376e-02,-1.565795789158479570e-02,-2.336752367417481782e-02,-2.897174535488384814e-02,1.407174287647334210e-01,-3.825740055130262551e-02,1.335215385373754449e-04,2.793623964246270963e-02,-6.651386540186007834e-02,-9.879341658853202368e-02,6.558966051044773718e-04,-6.177445156935412046e-02,5.389665351038241958e-04,-4.448670936128298198e-02,-4.214956722307958148e-02,4.967061182793620910e-02,8.688783652560236326e-02,-1.485903548122007034e-02,-4.028986555334661268e-02,-6.212507985948995115e-02,-3.812536689890169694e-02,-3.469922611495429576e-02,2.062708782139600558e-02,-6.266446725000464346e-02,-7.623413290190653169e-02,-9.158630690169514144e-02,7.691650492969073771e-02,-5.847790974533237374e-02,-2.627957879980000883e-02,-3.731882574257879698e-03,4.565218861064760519e-02,-6.430450672497554920e-02,5.819496327393459956e-02,1.939869121163106946e-02,8.921013737008109956e-02,-2.739343721496650605e-02,-1.812093636458499413e-01,-5.937987757201493300e-02,-4.236201084430811503e-02,-5.008379516024667050e-02,-5.402684999921416112e-03,-3.854510259095100810e-02,2.839703017008844005e-02,-7.306507062657310869e-02,1.283468432061116438e-01,-5.245372726277169884e-02,1.178318407079676639e-01,7.994488679583031110e-02,1.163505473485411737e-02,1.382554800293141553e-02,4.831073677114578607e-02 -2.994424617493663711e-03,-1.494031329226560567e-01,-7.607596015730519468e-02,1.547080554973596089e-01,4.733602504269616984e-02,-1.139992639409442982e-01,1.343730406426522361e-01,6.020391984055777773e-02,2.722065003190513033e-02,5.745066781668823480e-03,3.316090791769599200e-02,-6.963362706551944059e-02,-5.006718464336340491e-02,-4.207346738495125471e-02,-6.793730766635866503e-02,6.539956081315212444e-02,-6.952218421740583021e-03,2.580377568039764896e-02,1.603304663457985058e-01,-2.511931818663815444e-02,-7.288786613196271413e-02,-2.456946301472025945e-02,4.358499703116752721e-02,-4.076077075339917810e-02,-4.653895894061760297e-02,2.655723778677090280e-03,5.597702996129008679e-03,6.798896691661018843e-03,1.953494993235368321e-02,5.810996753779750640e-02,-8.795222582006462231e-02,1.261274691313542251e-02,-1.399995972517382829e-02,2.632173853029338637e-02,-2.607771336811684493e-03,-1.201911657241108849e-01,2.956482093756104129e-02,3.469729460654205622e-02,-6.462661149639985225e-02,-1.074706603182937550e-02,-4.879371043271234915e-02,7.583679120004410690e-02,6.028948865557743481e-02,-3.786712313960779314e-02,-7.846686442390388150e-02,7.351835678778848027e-02,-1.826535625232242332e-02,-9.599413563004581540e-02,6.458818442256646086e-02,6.038550414434247504e-03,-4.117979281262380831e-02,3.970837454616983619e-02,2.624113315133210639e-02,6.740365201760566138e-02,5.981674224716118649e-02,1.315702430668875059e-02,2.230536879427808677e-02,-5.148546741833706641e-02,-9.648533478611046044e-02,-3.704445597190020312e-02,-2.083954371376496931e-02,-8.465182580066243911e-02,6.925836766066775785e-02,2.025241864391438829e-02,2.262557180326590575e-02,-9.775068903720703084e-02,-4.999862292344917081e-02,5.688006449630531036e-02,7.837350799318673622e-03,-3.579348138520613287e-02,3.574003122787468983e-02,7.032789977126510661e-02,-3.274453400939188186e-03,-1.844567460080264176e-01,4.258632630348306580e-03,8.609618862493470179e-03,1.104350136453415010e-01,-2.917220495537848485e-02,6.427406388043230279e-02,-6.317596724061319557e-02,-1.662129644024346736e-02,-3.633057234629652466e-02,-4.869256699888803441e-02,1.000656551785234633e-03,7.912676884076715045e-02,-9.595161629798266978e-02,-8.508556407454670456e-02,7.989052491981314674e-02,-2.727743318901960923e-02,-9.252200510163924307e-02,6.639626527708721027e-02,1.425424705274542514e-01,4.080632707981555335e-02,-2.912107016689497244e-02,4.503835232290077928e-02,-2.399971558358806115e-02,-9.555226471160374602e-02,3.760224120073308446e-02,1.153120692684192830e-02,-7.728587657597982448e-03,1.487154227291678094e-02,1.317883813550486949e-02,7.856458534881934075e-02,-5.743958675221529087e-02,7.750915313324140132e-02,-3.022409374240675115e-02,6.009096071250294435e-02,-4.090529485490930484e-02,3.351855631434386140e-02,9.985919112870687842e-03,-5.499940426151367356e-02,-8.763364369238541007e-02,9.543649721014037940e-03,1.923753096906162530e-02,5.344546291211733219e-02,1.298958628528333048e-01,1.707011287346966999e-03,-1.318226294264538967e-01,-4.082669683041864617e-02,-9.529822930098946071e-03,1.298160623063560927e-02,-4.053704915813387266e-02,-5.340538069789019932e-02,-5.484924066099755580e-02,1.840969976468489339e-02,6.469202904898828377e-02,6.397013884888545776e-02,1.618397862500657938e-02,-4.206306758686405300e-03,2.272403776142777748e-03,-1.205014484977089129e-02,6.067268621126400974e-02,-5.397514544260594016e-02,-7.101974528051772928e-03,-2.870057931737331897e-02,-6.392869871951506466e-03,3.825067523316905832e-02,-5.543774229611561979e-03,-2.918894986692494378e-02,-1.842557735943864752e-02,-5.489785242355674222e-03,-3.771992437830815059e-02,-9.250471100997465293e-03,1.521628395917438148e-01,-6.592260652045514591e-02,1.124762197029467031e-02,3.769670962476639325e-02,3.774424582627224078e-02,-3.039938499118487528e-02,-2.663749372139116375e-02,-6.151472504113027207e-04,-4.357731985277682463e-03,5.209011243230438071e-02,9.738923059766708967e-03,-9.119665830179668475e-02,1.923517023400245218e-02,-7.322915332132044763e-02,6.259288463249459977e-02,-5.468976892868444850e-02,-8.760653737492178628e-02,-1.366249709234085286e-01,7.069422246084719995e-03,-3.540896990934375155e-02,2.955279505516410915e-02,5.421537865774127274e-02,-7.081512663175494604e-02,-7.168380570874630098e-02,-9.342353736472086556e-02,2.312370541346531194e-03,1.988116711932181452e-02,-4.922788511935966699e-02,-5.095963512230898551e-02,8.501365398033509241e-03,-1.873607233933658606e-02,-1.021272098999165961e-01,7.365883488007027302e-03,-4.023218561639065050e-02,1.733525296381461557e-03,-5.064271800281056751e-02,-5.141601560501096735e-02,5.920957358986163321e-02,-3.908577749791949940e-02,2.211721505305611415e-02,-5.509171685719891493e-02,-1.065366354322851844e-02,1.602431061856876837e-01,4.181956709289132157e-02,-3.985719762855101378e-02,2.138117302779326059e-02,-2.293467173053958391e-02,1.947645355866468864e-02,-1.157043749722068970e-01,-7.680002984437667157e-02,-9.834443951571927162e-03,-3.716936427291206435e-03,4.432191528765568045e-03,-9.742187584992167970e-02,-9.190273427440197318e-02,3.057251355096535228e-02,4.445799209152247189e-02,4.307787046023005273e-02,-4.952239957841100826e-02,-4.058497642836750297e-02,3.844789813530405387e-02,3.588857426279027513e-02,-3.995591087093383620e-02,3.542792917089836874e-02,-6.287650233343601569e-02,2.816224890021531285e-02,4.857482361267202037e-02,8.486609975601629163e-02,1.552669655158902921e-02,-1.693124928671531637e-01,-1.512206371270076628e-02,-4.976666251621159054e-02,-2.216205871252842699e-02,-4.565739526592764441e-02,6.286718161479327993e-02,1.546312250833317692e-01,1.115692733495151912e-01,7.290432911267116789e-02,-3.543407942573657821e-02,8.455232605720694461e-02,3.273606492057929557e-02,-3.767360096013346038e-02,-6.059635133135347906e-02,-8.747323572429199778e-02,-5.172996156566211923e-02,7.272420529743718609e-02,-8.430343980625631950e-02,4.287972754624823357e-02,8.601730566426189772e-02,2.090540689535144367e-02,1.142321137039391254e-01,2.060467994056758637e-02,-9.778894243361842264e-02,-3.171359316863603395e-02,-6.145169905990100628e-02,1.337008919624804726e-01,4.953686033307452830e-02,-3.794046233980489374e-02,5.120709152426618227e-02,5.680838756897986508e-02,-1.048102005078650617e-01,4.475483276250992459e-02,4.717365611714949281e-02,1.763259232547226968e-02,1.477726976216294230e-01,-3.220425925939351258e-02,-5.696670336342607444e-03,-2.112472973100253462e-02,-1.301066587707848388e-02,1.196625560687605561e-01,-8.871017138072755104e-02,-7.281842466010685078e-03,1.030555201716806812e-02 --1.185435981323333943e-01,1.431847254409529015e-02,2.210588023953471981e-02,-1.601926012495384112e-03,-2.520488949844050530e-02,5.316286177345110053e-02,-4.784219080028413729e-03,-2.988108415736444572e-03,2.771091625296097719e-02,3.177236618014973868e-02,-1.136107950399225554e-02,-4.009572822549491156e-02,-2.335774213727403667e-02,-4.725000971368191471e-02,3.876376444939892002e-02,1.521388035589609777e-01,-2.653631612711563476e-02,-5.713305015824059196e-02,-1.740753902877260548e-02,9.105420202012430575e-02,-6.942281606402852211e-03,5.808501063625097494e-02,-1.645258807961582845e-02,-2.667815174765922337e-02,-1.081689204570354668e-02,5.647716471635343127e-02,3.860254176923291758e-02,-1.202014707901123830e-01,-4.103473610807745892e-02,1.013658089549707292e-01,3.680028871420945591e-02,4.987555481965951810e-03,-1.578885248598119878e-02,-1.063964674868557031e-01,-4.932381576069799073e-03,-4.660055387745745681e-02,3.510744121237206056e-02,-2.214436822008129108e-02,5.772674003724768965e-02,-2.524911846180340549e-02,1.024993431343886562e-02,2.248619649685619504e-02,1.867482320003054086e-02,4.145049988483947428e-02,-4.592216610950208139e-02,8.967598158528576613e-02,-7.833199922766129197e-02,-1.590965900114836573e-02,-6.508301007532073246e-02,3.770954362675055238e-02,5.377573421611167581e-02,-1.140413329923237323e-01,1.354814264385405809e-01,-5.753422430105913782e-02,-1.483509747859222336e-02,7.236030296466843328e-03,9.522646407802040636e-02,1.259600607991998300e-01,-1.581565220845424641e-02,2.550802312954834200e-02,-3.038972344215854632e-02,-3.848770642706955680e-02,-1.799575441103488938e-02,-1.069661558915794425e-01,-1.344549286316554411e-02,-2.283188365962028621e-02,-3.289632740023133717e-02,4.449041934338143073e-02,-5.355059369430313632e-02,2.802650660594101228e-02,-1.852985224351564042e-01,-7.216576661188228714e-02,1.410536331447245426e-02,-1.879018110194258934e-02,4.051556202923106698e-02,5.834022469562524793e-02,8.560347365467561864e-03,-1.074471002732375891e-02,2.009954602145522934e-02,3.056173706207864632e-02,2.193048188039190688e-02,-1.742149600410088789e-02,2.321930396417318937e-02,-2.591587425792168248e-03,1.156726844278704730e-01,2.556406546635862573e-02,-1.653143914952163313e-02,-1.978220395721484726e-02,6.303086440474506280e-02,2.152224781206240473e-02,-1.844606107157609892e-02,-9.599885241064178010e-02,1.254868994853377065e-02,4.923197925960624138e-02,5.209901434682538690e-02,-4.898406807575486632e-02,-1.027961817850674836e-01,-4.456682062036174119e-02,6.766726200519437850e-02,-7.419551142340520544e-02,6.354666542997274015e-03,5.758832182500394498e-02,-1.203572119150051779e-02,8.155496601998869777e-02,-2.944770238711616125e-02,4.974973888209596740e-02,-6.453406443652481839e-02,4.495530785867853790e-02,1.614617023784230970e-02,-8.942866903782806243e-02,-5.146617458154810476e-02,-1.674594496703404531e-02,4.051239923633257606e-02,-6.529445020448344794e-02,-9.414686875295454060e-02,5.408852037842411947e-02,1.264915601246260207e-02,-7.888782272550334818e-02,1.288830815836511552e-02,-1.096286419794019560e-03,3.015019879958991816e-03,2.880021946986573850e-02,-7.195981101813705982e-02,-3.835773956671402474e-03,7.708396955047196186e-02,6.001984022320468182e-02,-5.228439999243951475e-02,-5.615294028514712654e-02,-4.894075606589358418e-02,-1.792561594146910992e-02,-7.942360739770343292e-02,3.767798965689962004e-02,3.794838166637451438e-02,-3.485853715902144379e-02,-6.279507437204847123e-03,-2.849841709487016539e-03,8.717860848090669759e-03,-7.135270038281468254e-03,7.210413702156706628e-02,-3.115786605917192365e-02,3.184414507673166694e-02,-1.160036494180865935e-01,-1.910364721892532647e-01,-4.784851303386549598e-02,9.827605331791219867e-02,-3.687640948679384023e-03,3.389461743279947870e-03,4.177501357856158726e-02,6.489044149687947516e-02,-9.328247132120032203e-02,9.031062950604422379e-02,-3.509594869250989152e-02,3.488880696737631237e-02,4.463240498840653769e-02,-9.063944933425475969e-02,-3.444398369545167982e-02,3.856551381922147109e-02,2.782444120011190458e-02,-2.103790952278993334e-02,5.130501278188580316e-02,-9.918083005399516705e-02,-1.683544504578838100e-02,-1.595435653207126492e-02,-4.966286449883498821e-02,5.313216652928525602e-02,2.270627745311855003e-02,6.809435179364831114e-02,4.905540292915540335e-02,3.388920899482306820e-02,-1.298515408970541199e-02,7.335069645522898640e-02,1.363112449107463375e-01,-2.599741006129508390e-03,1.933986011193375779e-02,-3.037919462932352210e-02,6.760532863990166563e-02,-2.525374308501272813e-02,-5.042555283570972907e-02,-6.645324184439613258e-02,-5.566725646487917512e-02,7.010597754000010007e-02,-2.631230108753110711e-02,-2.292315851534404078e-02,6.264420048614320613e-02,-6.075679319312905086e-02,1.456316097087010514e-02,-8.935060772185762779e-02,1.435692082293603122e-01,-1.582642793684116422e-01,-1.243034418564704369e-01,-5.173841332161273343e-02,-5.754781796333857669e-02,-1.051236534502597922e-02,1.477628256969270443e-03,6.935160152243526055e-02,-1.179272267491263994e-02,-5.166891193517197944e-02,1.136213979666998164e-01,-1.493098855773178704e-02,-6.489002602384100205e-02,-2.371438426312823602e-02,-6.435348129778367676e-02,-5.859824126690862250e-02,1.027902575035453681e-01,-1.105975210805496267e-01,-1.162664595319675559e-01,1.114317773439655462e-01,5.885462838204863489e-02,-4.173526434421724607e-02,4.701759813978601682e-02,3.931762892811263566e-02,-6.834919683287421499e-02,-7.373793414926316236e-02,2.159127975467537743e-03,1.041986678037272740e-01,-7.937301263482410274e-02,-2.101863283583301628e-02,-6.259938864554501214e-02,-6.144264509572907457e-02,-9.414564297921163699e-03,1.469725343133655160e-01,3.216515692327970993e-02,4.354536840439934831e-02,5.117467738486031203e-02,6.457030034952444819e-02,3.701237442665872290e-02,1.419288136181582338e-01,-1.210630653714118814e-02,3.136415363960756708e-02,-7.789650630838712564e-02,-5.766397442717572513e-02,-1.196438369852998868e-01,4.962853907112727697e-02,-3.622827246438201781e-02,5.545745697139987940e-02,1.493968341440928167e-02,-4.628554815378078946e-02,-1.296504030437111155e-01,-2.901039996909922369e-02,-5.735777602450822105e-02,-9.674311940797435116e-02,8.327059858979746210e-02,7.641264275436025766e-02,-3.496768763124981361e-02,-3.704242432627866816e-02,-5.300170948010833605e-02,-5.268456360057231780e-02,-9.071790970869786364e-02,-4.515247519891912170e-02,4.794915898310860014e-02,7.129655134384088599e-02,4.993799258598396323e-02,9.124287597519800674e-02,3.577625814644599250e-02,3.193659978208022304e-02,1.194946037571732456e-01 --4.074059599063753478e-02,4.323262119784884727e-02,5.463028604180117376e-02,-1.324338225814955250e-01,-1.344495359021407460e-03,-4.718039221871622030e-02,1.494574855116205525e-01,1.304240598609472668e-02,8.291501945588775069e-02,3.856641392798747237e-02,-6.906274173480490830e-03,-1.609190421765311907e-01,6.017051918243684211e-02,-7.144707942924868882e-02,1.040007372984637635e-01,-3.466406851189007821e-02,2.728838006316665690e-02,-2.908968180781747875e-02,-4.017160245965175486e-02,1.832044642657315592e-02,-2.242446537935274126e-02,6.343816172779022378e-02,-4.025633541206750932e-02,-4.931888635306853896e-02,4.975087501560452458e-03,-4.141241780075274187e-02,8.732931965326237722e-02,-1.566045976805273687e-01,6.133720516539901968e-02,-5.218250344962260118e-02,8.785852304563697424e-02,-8.710029782761691611e-03,6.574996943893320067e-02,-8.346760466426514369e-02,-4.787363843364891153e-03,6.782022881786128887e-02,-5.763104801353753637e-02,4.423845603559356521e-02,1.085270017215545779e-01,-4.599529810308474603e-02,-8.518730258941392686e-02,4.791223444990368174e-02,8.742282664365188388e-03,1.165869713705369910e-02,-8.956294946568481297e-02,-1.968316761606695928e-02,-1.961877375165159421e-02,1.879555819228029973e-02,1.705671129148162157e-01,3.023555022013289101e-02,1.141350558611537447e-01,-3.715347644683156259e-02,-6.274580726667096375e-03,-5.856660895471633987e-02,6.983347492407290802e-02,-8.181473301094798270e-03,-9.691288209140479226e-02,-3.677009219540323948e-02,-7.030812990687343444e-02,-6.587938299431546352e-02,1.555115336712971918e-01,3.139566352163700169e-02,2.172611676707564277e-02,1.187780519153075198e-01,4.576626845914265329e-02,-3.663165786309699468e-02,2.423584624922078129e-02,-4.393431980592921691e-03,-4.096708706856785204e-02,-1.758439842915269363e-03,-3.499267578323901617e-02,4.198249590353889116e-02,5.766309310614761707e-02,-1.399032450598777441e-02,8.020028546665186608e-02,3.991907462011052427e-02,-7.759510625919985305e-02,-6.938056190270806833e-02,4.132237535729218342e-03,-1.285696721300596743e-02,-1.253633073279388811e-02,-2.584338853583657142e-02,-1.365179857618776804e-01,7.756399793160256850e-02,-4.220369512936564954e-02,-8.990851827015558517e-02,-6.132137166589471389e-02,5.315557476765972456e-02,4.724201943298745460e-02,2.388282387883910313e-02,6.198484503280751040e-03,-7.899657695805532842e-02,2.642154086382015807e-02,6.434120993580128012e-02,-1.729111673355907333e-02,-1.633913185491692455e-01,3.443860747923969262e-02,-5.231473546753753756e-03,4.067671478419130487e-02,-2.814707899018565815e-02,1.010520703304448875e-01,-9.411962563457369757e-02,-1.242271621132770912e-02,-1.950617326620507005e-02,4.134098423174909637e-02,-3.807061945828466021e-02,2.422469499570938795e-02,2.419478895583438566e-02,2.664988771623549163e-02,1.005423940651029302e-01,-3.009393704727296448e-02,-8.542113019937330048e-03,-5.991776228597285592e-02,8.536183938063789112e-02,7.356855444824617063e-02,5.924493444483309307e-02,5.582279831870268910e-02,4.343194284266788108e-02,-3.863906731823713026e-03,8.732874479111613075e-02,-1.551829779261565748e-02,-6.224361038331256701e-02,-3.052010361509316372e-03,1.257650057945988165e-01,-8.419157779094688099e-02,2.181970789330135024e-02,-3.133633195670218474e-02,-7.028162131527200540e-02,3.131072190871068972e-02,-6.876630864616871397e-02,1.176256007471520315e-01,1.066425007463799174e-01,1.122929342863681831e-02,5.620971429345783532e-03,-2.107102054768958399e-02,-8.106621723525703205e-02,-2.750076644801239915e-03,4.244314329474659260e-03,5.102995293230210005e-02,-3.962634295969089115e-02,-9.645339476904768450e-02,5.105968522948173272e-03,-4.397465042965772314e-02,3.873328733796253809e-02,2.765132438207014084e-03,-2.890466124721195046e-02,2.444719418595574928e-02,-1.186210485363776712e-01,9.265828990648879160e-02,2.389208104515272435e-02,-2.097822441428605009e-02,-6.311570886266412717e-02,5.342250304969657637e-02,-5.056030295995414536e-02,-6.527228483857003272e-02,-2.366877296088795712e-04,-4.931726970927233916e-02,1.491961905554772122e-02,1.093544879737038539e-03,7.043864992784275303e-02,4.203220335377479133e-02,-1.888763333803354302e-02,-1.918965651903188283e-02,-2.574856173923533928e-02,1.882899049294488661e-02,-9.130086941569956482e-03,1.132331931153200272e-01,-1.903557715766174968e-02,6.203707106934524401e-02,1.243963424277135243e-01,-9.451809138003966493e-02,9.401945047504382813e-02,2.482428724399832198e-02,-5.890305021974256289e-02,5.457125646639111177e-02,-1.140173907283169308e-01,1.985945061447701859e-02,-1.280800633205528560e-01,1.385104198409040643e-01,2.302765609399718208e-03,2.652050227431373974e-02,-7.776835573672957747e-02,-6.376803781705842056e-02,-1.090106170911306696e-02,3.545465384741774972e-02,-8.086338637263837392e-02,2.168737965436878454e-03,-5.364173175715142294e-02,-4.657440556091956702e-02,4.650601199107412542e-02,-3.498884035832029032e-02,-6.739865115928256323e-02,-2.004509936365966583e-01,-6.744052756205570764e-02,-4.049410305933700410e-02,-2.475939103284404558e-02,1.575679604308266590e-02,-3.822537964186316362e-02,5.232413444975433925e-02,1.519901468132130173e-02,-4.317555975599780133e-02,4.959935221933616661e-02,3.905520124348078881e-02,-4.045062848887163998e-03,6.139398428959743498e-02,9.609595760511001686e-03,-1.517802693796876587e-02,-4.135482877883156494e-02,1.039089867642954169e-02,4.378706004520791645e-02,6.389492349632762414e-02,1.241091191079591316e-01,-1.400145560175354402e-02,6.743556274413971957e-03,-6.514046976592535731e-02,1.472224703924118749e-02,5.419654869630263772e-02,-1.421001826684552445e-02,-6.122518778243185578e-03,-3.661608334734201692e-02,-5.346646277463337066e-03,-2.310607930919159753e-02,-1.816923967621784583e-02,-3.850740738422395865e-02,-6.546764005204405712e-02,4.819210412213699446e-02,-6.765487978248518885e-02,-1.221134592734730157e-02,1.535682278387307670e-02,5.530733135390644012e-02,5.992217308285221339e-02,-1.293457802866562756e-02,1.245763232569674500e-01,4.886518170508837022e-02,1.247600481603181044e-02,1.217518176709540545e-01,-4.971059187466484591e-02,-7.216888072970659151e-02,-6.203829758131266386e-02,6.280014412937566515e-03,-7.300824285901435368e-03,5.844851748650661816e-02,-4.071194513487609579e-02,1.825198779094274609e-02,-1.064737143821075163e-02,3.687020850476185191e-03,-1.030955536957125634e-01,-8.051958261236061287e-02,-5.595743069548209103e-02,7.608889415039063464e-02,-3.850824602849420297e-02,-7.990701190067674153e-02,-3.017636433719023820e-02,-1.663762471015598973e-02,-5.159293152153398426e-02,6.483896116113360697e-02 --4.272965347860832597e-02,-6.914460132193596398e-02,2.891660162231575459e-02,-1.386833871551285124e-02,4.044666922751241622e-02,-6.498734135633946196e-02,-1.456969616563458247e-02,1.479155447867466777e-02,6.954686506233002941e-02,-8.958619434261794190e-03,-8.785301464719488806e-02,-6.609464577654822204e-02,1.176998350111144731e-01,-6.131940368143631859e-02,-4.563843228932188030e-03,4.584180071421520286e-02,9.837249870693258458e-02,7.722115384162661633e-02,-5.314741114918539872e-02,-9.369684248173837382e-02,-1.648408360220666308e-02,-1.453954239561362771e-02,5.816398785844049957e-04,1.258449634486494967e-02,-5.515680681484592951e-02,5.476281781963800860e-02,-4.417659958150847721e-02,-3.248655140599816110e-02,1.021197563760338906e-01,6.296969828417842141e-02,8.174139904368331322e-02,3.147799812059787206e-02,-1.676248927368388653e-02,2.210718295564104846e-02,-4.778052418514128208e-02,5.986034100624483545e-02,7.164592913799472385e-02,3.890203817522474433e-02,-1.828468127146691893e-02,-9.314987900097473905e-02,-6.820171574354855215e-02,1.337848687865897390e-02,1.250736668873105306e-01,7.600270708048913887e-03,3.342085191131970168e-02,9.161631128263383883e-02,1.789724590698284212e-02,-1.037227329324715841e-01,-5.274546753624957673e-02,-2.899814729805921115e-03,-1.582037823759141482e-02,-3.930846762291550361e-02,3.690840545066211703e-02,1.575650824438726560e-01,-8.409991808078301490e-02,4.869168094724563717e-02,3.296967810210063576e-03,1.030945306159935793e-01,5.330726447297831627e-03,-2.045359100894641319e-02,-2.810366919929929361e-02,6.492252503335584068e-02,-4.792048372416281982e-02,1.149168921291615283e-02,4.483324886974190612e-03,-4.025799910050401054e-02,-4.011852397219060201e-02,-1.747140046454221524e-02,8.542078634591394881e-03,-6.581070510190020761e-02,-1.009539294916205754e-01,2.884744017857881343e-02,2.570917454116346951e-02,7.064290472371111129e-02,-3.448729134384005690e-02,-1.857685776773104203e-01,5.568229197084119397e-02,5.346261852500419631e-02,4.117975315583281143e-02,5.942863173784794506e-02,-4.964182135668168683e-03,-4.478902606043762280e-02,5.800282636035534491e-03,2.836932713607843992e-02,9.677879850712712423e-03,8.237006832003051365e-03,4.659201429360450980e-02,-4.087343345510777853e-02,4.689766292333046038e-02,-4.475290982238244036e-02,-7.052214272074913692e-02,7.278790603402390813e-02,7.438468846060131145e-02,-6.618816586738295038e-02,-2.960898677391783987e-02,2.222993822853574358e-02,7.608702785409959812e-02,-2.698786092651062410e-02,1.877288565085619837e-02,8.686669766033928292e-03,-7.082911189678985087e-02,8.535987804141862562e-03,-4.642164421302524824e-02,-1.853518773899918287e-02,-1.655907518857434313e-02,-4.435182122314577924e-02,1.841082302328185816e-01,-3.490882052293392546e-02,8.536034113552629088e-02,-5.258638169127428236e-02,-9.243783268338852388e-02,-2.622028397383728907e-02,2.469010183392437302e-02,-6.230652048126884140e-04,-2.728893745861546427e-02,-5.736053874396297020e-02,9.087187910170048399e-02,6.782853283196274563e-02,-1.937134496168213538e-02,-5.051318540449462047e-02,9.508073349967892385e-02,-1.978949240023425427e-03,-4.384537594604367289e-02,-4.839994822642201822e-02,3.582169544170014236e-03,2.671966999294658751e-02,-6.165070619165305410e-02,-4.083783455779530641e-02,-1.041026811387443796e-02,4.232590608851767577e-02,1.207137208343020233e-02,6.731139799832938642e-02,-5.895678452780770892e-03,4.883180179005399552e-02,-1.131591460567044072e-01,2.727094951825933394e-02,-2.434100900411787297e-02,-2.838595079194236245e-02,-3.995090023588047934e-03,-4.417182020200233206e-02,5.408808717072755012e-02,3.204500167574426267e-02,-1.340171547013459946e-02,-6.487299164132856455e-02,6.520421522264049918e-03,-2.815476190038860779e-02,7.464490252194748432e-02,1.569167152273233737e-02,4.833679362209995778e-02,1.611053774398547161e-02,3.840015214689239398e-02,-3.424096598135820818e-02,-3.581793101318862804e-02,1.420058450364978797e-01,3.911018832081798041e-02,-1.531559151323969858e-01,3.864874529987050861e-02,3.188543214350882116e-02,1.010269412919155840e-01,2.278836235793703369e-02,3.657837718613037026e-02,-3.292749169451261726e-02,-7.023887141142717838e-02,-1.665956074439951806e-02,-5.170398141422385963e-02,-5.517858197291702904e-02,-3.651756096369622451e-02,-1.013293245484372107e-01,6.176573910915972299e-03,3.373293000922968077e-02,-1.995502201014037746e-02,-1.408208868439047787e-02,6.646399836844531228e-02,-9.587428170389848681e-02,7.147146339303504792e-02,-3.793402969823451987e-02,-9.098817060521285915e-02,7.484504665689136957e-03,1.379536924040801193e-01,6.620623981112434830e-02,-8.410542705584439505e-02,5.463246725441704699e-02,-6.307855242891192982e-02,-4.829158489209896266e-02,1.295986132840490357e-01,-6.414756035887510255e-02,-3.277495865956341070e-02,2.175638212948974304e-02,1.824008680320562265e-02,9.329342376097120304e-02,-3.679490254556529172e-02,-3.246075832237124703e-02,1.181942314892584778e-01,2.423106010437441407e-02,3.944882440664979062e-02,-5.173213999489642556e-02,-5.501354977576178307e-02,3.321811397901356516e-02,2.224412634599202618e-02,2.103898033225503478e-02,-4.015348968537289787e-02,8.520219131421477576e-02,-1.599559288704210241e-02,1.207969410146557399e-01,5.685865690879517154e-02,1.640499500891122800e-02,-6.286538301018920749e-02,-1.659537451994684554e-02,-3.115150887010440309e-02,9.404734468574250938e-02,-3.437149638625536241e-02,-3.392632214136452606e-02,2.301768918966114533e-02,1.133724495236543423e-01,-4.756360940402586113e-02,-1.464133317021132363e-02,-5.979040896272701244e-02,-3.560612533658434609e-02,1.526283943446424540e-01,-1.245840605547196001e-01,-4.604193120430084829e-02,-1.461093567788524471e-02,-1.765874019753114199e-01,5.269402109985366761e-02,6.818915531762376192e-02,-5.026735573160156373e-02,-5.663270910704099569e-03,2.143148008279227232e-03,1.403075841545340513e-02,-1.621918731699918403e-01,-4.092933725651012183e-02,3.974531088948142499e-02,-1.041761433717745995e-02,-1.506031062802841089e-02,1.090602254994923470e-01,9.168935890156437729e-03,5.797240366486116486e-02,1.260330177713158695e-01,2.120283087906303654e-02,-1.122961261777142283e-01,4.165295283459859166e-02,9.605369084956455472e-02,1.050603342733733153e-01,6.247516217649920489e-02,-1.085452684666676597e-02,-5.707082338193679438e-02,-4.281185257020139745e-02,-3.244463399669483133e-02,-2.954595871175110661e-02,-5.854763777921745405e-02,-6.421620766378019485e-02,-3.568459850262975519e-02,8.554720586549238726e-02,-1.177818703350511226e-01,-7.385913113584097189e-02,6.957921372131978255e-02 --5.970541246520246620e-02,-9.897101812364826612e-02,-4.740714442612232321e-02,3.545020676403470850e-02,6.703619825866585780e-02,-9.613539330376051628e-02,3.113579963553270158e-03,-7.199220168521906771e-02,2.055151485450406462e-02,-1.170974764504728594e-03,2.212006085109403941e-02,1.033594279556767775e-01,8.189286458835792759e-04,-5.937336989848547575e-02,-5.359966537705446177e-02,-9.613201987708944185e-02,-6.553122734984843586e-03,-4.529276554934974497e-02,-2.037319896949223511e-02,3.505679001092113639e-02,8.516395101530363498e-02,-3.901106891363577411e-02,3.643512988452582424e-02,-1.009532256611672119e-01,7.103705101993053328e-02,-7.183128898444927912e-02,-3.120270218590920352e-02,4.323600283330432392e-02,-3.628304021608429186e-03,-8.132552525095587681e-02,4.067917107715856434e-02,1.002368387835037098e-03,-1.248147511345827458e-02,1.162422185661240170e-01,-7.556409056201729424e-02,5.740238551337600859e-02,-1.121950096622311571e-02,-1.106893058276829829e-02,1.090312205661392309e-02,-5.493206929173905656e-02,-2.598737907938190872e-02,1.349244673276541254e-02,-2.986647078830623192e-02,1.755685797103727352e-02,1.079920486782811051e-01,-7.962657652530406166e-03,2.428347280385430990e-02,1.290202961144776839e-01,7.014848096525098942e-02,2.201953659177580125e-03,2.378573087244966605e-02,-5.003832219776606360e-02,1.625522129784352510e-01,-5.154565396709533120e-02,-7.989396791304685663e-02,5.022843078728782967e-02,-3.506029192555074059e-02,3.503003213264226090e-02,6.962903451548227429e-02,-9.538399767211665214e-02,-5.990046046513623074e-02,-3.200344595994690511e-02,1.739766798363389932e-01,-5.269844465006517931e-02,1.397237973355903648e-02,4.008032218703868471e-02,-4.912860969128150790e-02,-5.973476250204753923e-02,-4.510598476277020985e-02,5.426937801171729386e-02,-6.902733754788266396e-02,-7.505447946883297972e-02,6.096229144733067429e-03,1.405825030943130133e-02,-4.010810096898143745e-02,-1.415775206452803969e-01,-3.732175068881471436e-02,-7.509373564110449761e-02,-7.323664057935629079e-02,3.968430512159660389e-02,4.755542558995444480e-02,-8.829913379469539469e-02,-1.484445612260395543e-02,7.625009106855221153e-02,3.265766846757515302e-02,-4.510400506071819693e-02,-7.045685431630888118e-02,8.552095682845503588e-02,-6.693623362647838082e-02,-8.330405510824112625e-02,-2.761570745038289046e-02,-6.847842313934116021e-02,2.625527847227900752e-02,-9.775641972445243966e-02,1.639238631364697829e-03,1.249772971199695909e-01,3.450177948863999977e-03,2.703368549369544738e-02,-2.922615704955026927e-02,-2.821144208819473947e-02,1.423468406659249563e-01,-3.315555221534881997e-03,5.927673105782130941e-02,-9.265797556091351361e-02,1.249034422404778910e-01,-4.323554354117004755e-02,2.098201335446053806e-04,-3.051137287387246932e-02,1.406426684804108762e-02,2.309098105294907569e-02,-1.267419462232072594e-02,5.988726039878072838e-02,-5.399898593239506328e-02,9.715577561941214635e-02,-7.209137810059457196e-02,3.629775341946973183e-02,1.373070320140155204e-02,-1.039069939149543237e-01,1.110709505182101986e-01,-5.163423443736342433e-02,-2.073242906188677362e-02,-1.437731753606059232e-02,-6.027204400399812506e-02,1.162703904636750440e-01,6.609418534452315974e-02,-1.077257337850053703e-02,-1.378178522251063791e-02,9.960217699042544712e-03,6.636043816146698637e-02,-3.973367978259249866e-04,-3.236637107852101403e-02,-2.010780836398924284e-02,-7.014724679498635096e-02,-1.939989891953549164e-02,-7.346391496783855057e-02,3.564925914661228740e-02,-7.483507164560385116e-02,3.806573899454364363e-02,-7.409051677221055454e-02,1.691679826894309513e-01,3.271303975170226919e-02,8.062982410329432026e-02,4.511833329895622441e-02,-9.281753143119859939e-03,7.505963408751200466e-03,7.450130051784437969e-03,-9.691138373564352204e-02,-4.716470529393401029e-02,-1.016230856626936935e-01,-3.894126295877918531e-02,4.406870773065164199e-02,9.514393662411569502e-03,1.716859823539391636e-02,-4.134839318752681764e-03,-3.524851497811486850e-02,2.744412191812298576e-02,7.601917388371050299e-02,-9.966432055213519936e-02,2.796898295843661120e-03,-1.225685896292664015e-03,-3.478652661715268823e-02,1.634274882763814274e-02,-7.514350662634108891e-02,-1.083721615131712085e-01,6.795167521474305972e-02,-6.801072946722730439e-02,-1.090571932332477442e-02,-2.424240853398040851e-03,-1.075849981770271441e-01,5.545914587634022397e-02,1.591765325231564096e-02,-2.440542066264039453e-02,-9.441418862772284204e-03,-1.085566394921805462e-02,1.058492986429104415e-01,-4.212238635397890646e-02,1.444116110542106868e-01,-1.255979286317010779e-01,-1.073425898976105991e-01,8.662387057098999699e-03,4.142412422856189824e-02,8.811429242955691465e-02,7.394220668483085512e-02,6.124589961092003626e-02,6.294407452468067654e-02,5.789425887026480866e-02,-3.886847283152578686e-02,-6.013850025689077006e-02,-3.231592420452748804e-02,-5.009797630590122341e-02,-4.352795036136540563e-02,-1.786002359320475880e-02,4.043466173735485447e-02,4.589000598911179701e-02,4.392955394556105619e-02,-7.894541125485424193e-02,-3.457235350174267530e-02,6.280308675649488859e-02,-1.183321212062650590e-01,-9.909420602950429202e-02,-3.496363470631626974e-02,2.478132873597864785e-02,-6.971648008315081690e-02,7.899563225316212633e-02,-2.958561607573304691e-02,4.378995630968041120e-02,6.560892510719951165e-02,1.311265455463752477e-03,-6.827597960310176350e-02,-4.233286228298792364e-02,1.057836319148608081e-03,9.560692461057984648e-02,-5.023830602301479487e-02,4.456654887276999266e-02,-1.717685860438409318e-01,1.736852604522366034e-02,-9.428076429630022348e-02,1.153960847940425916e-02,-8.044923077450796822e-02,-8.118822519422225215e-02,2.924572993643903551e-04,-7.429516401379279711e-02,1.636145018416006874e-03,1.319777260169404179e-02,-3.782705257396608983e-02,2.000009475350924930e-03,-2.730269007734311901e-02,-7.883504263889354324e-02,8.523855086348558485e-02,-4.749323033208992934e-02,1.522051480079878415e-02,-2.414355177293339705e-02,5.868497682819896755e-02,1.638594972854072115e-02,3.003043246354725193e-04,3.974683676506989699e-02,2.859988282601331339e-02,-4.557793819330469010e-02,-5.588520174421442166e-02,-8.191851281810241803e-03,3.257008327317299778e-02,4.504646011658704902e-02,3.550279024304345971e-02,-5.376085825585817657e-03,-4.433231321403040270e-02,-1.088996279619014879e-01,-6.463848010475564676e-02,5.909460096933852324e-02,9.015521857159233821e-02,4.363590675293864407e-02,1.826183786023637470e-02,5.650257915830458888e-02,-1.037342942956134273e-01,-2.618046071221635102e-03,-3.156754858470802588e-02,-4.398722531037827649e-02 --5.687122029717450039e-02,-5.667289470805672580e-02,6.024655759183296377e-03,7.291485124510779792e-02,5.186318611800153078e-02,6.750248761404484266e-02,-4.135571213593708384e-02,8.023194901685562486e-02,-7.019257451542722237e-02,1.119906859072581362e-01,-6.950229924212517063e-02,2.129523504190491967e-02,-2.484519022096226548e-02,5.510249653263205039e-02,-1.830132404560175230e-03,-6.725293216717809219e-02,8.322596191806712340e-02,-5.140696613516916591e-02,-4.233213461368957181e-02,-7.110640340712590102e-02,1.441380414179887354e-03,6.614204619500435589e-06,-7.725307525117589003e-02,1.172815768938024750e-01,-3.880493321380634048e-02,-4.259761503290918072e-02,-8.480064474097175065e-02,-8.566597242252629862e-02,-1.680478299212453952e-02,-2.618548396590822391e-02,-4.009956309133937136e-02,3.480391333159003203e-02,-1.493267023686513720e-02,-1.333728557458750685e-02,-6.179474475376416781e-02,1.805582246667995772e-02,1.424501719246356116e-02,5.227725390233434188e-02,-3.092490947534285578e-02,-1.788700667391921600e-02,-4.045603394103992889e-02,-5.439541082556707818e-02,-9.462618491958105227e-03,6.637835272541788012e-02,8.303258633329334282e-02,4.606965882950031510e-02,-4.856556867901293956e-03,-2.256416515273781953e-02,-8.818477665929910780e-02,-5.609707528230560569e-02,1.615315966153480776e-01,-4.591197381476742195e-02,3.191232826235790299e-02,3.052384354855516846e-02,-6.069324499729725209e-02,-6.347732667676231344e-02,9.439534665214652231e-02,-1.267178510131587027e-03,7.241600148509652612e-02,9.597712436690900017e-04,-4.019464121277934593e-02,-6.779879661707552407e-02,-8.071877204355526469e-02,-1.990513875333543847e-02,-3.622990600026217145e-02,-3.285517102351361712e-02,-1.245481671353877717e-01,-7.468302804669192663e-02,6.910897556448877821e-02,-1.292874169004782359e-01,-1.978822688582186282e-02,2.226155777195511134e-03,1.140480762849805929e-01,6.028566234058719414e-02,1.507546776774388447e-02,-1.116581916640935340e-01,1.448580147679033314e-01,-1.141275272231050963e-01,2.536050230507611719e-02,-2.366601846034883938e-03,1.721914309095202433e-01,-8.721691262669372530e-03,-4.973411997544100599e-02,-3.983030695485496181e-02,-3.579512667508213952e-02,-3.407373502140138843e-02,-5.040526099479759187e-02,1.046624137254178327e-02,-6.463531013327919461e-02,1.785672902591319694e-04,1.848031705207060782e-02,6.243014934535974503e-03,3.387331625791491913e-02,2.095561858511342523e-02,1.062816218602920942e-01,7.264338255271543532e-02,-3.178564430056411705e-02,-2.500988643296668881e-02,7.640480119413506355e-02,2.748738816365534424e-02,7.008229052952595628e-02,1.838309503250582649e-02,-4.254344917532253134e-02,-5.099719902300345331e-02,4.502186092439042220e-02,5.551593873388450950e-02,4.509280146661254207e-02,-3.977481694111933974e-02,-1.332247989654207487e-03,6.421111499108816956e-02,5.827979797055204969e-02,4.127008237135507129e-04,2.952641303067801704e-02,-4.352228357955117977e-02,-1.996721603947967938e-02,7.506188641813156537e-02,-9.922476213758552044e-02,-3.150434774308091129e-02,5.502771027933871240e-02,4.030259393673482626e-02,1.159696638172283967e-01,1.536751484211784299e-02,1.670637179772720415e-01,6.213529385584105891e-02,-2.003939059616477447e-02,-7.097610892144395889e-02,-5.777339206569839097e-02,3.726429283996546965e-02,-1.130785750528785077e-02,8.621533455035632065e-02,2.454730022725618716e-02,9.087618745246560381e-02,2.487884921664719473e-02,8.337575200957586530e-02,7.024231803188337592e-03,-1.857205340369313604e-02,-1.329297423716082824e-02,-5.686594082667863204e-02,-3.004300531396069535e-02,4.499179782946610695e-02,-7.894313620811344023e-02,1.921545906716912860e-02,-4.238007502102972279e-03,5.786354287352200865e-02,-5.105634028478608921e-02,-4.799579347500167181e-02,1.471665298707587610e-01,-5.835528927693338364e-03,1.043850011502404085e-01,-1.667155673598356980e-02,-4.158805637841573449e-02,-2.625783536068441310e-02,-2.049065415412385655e-02,9.877469016345262476e-02,-2.434852492172921712e-03,5.935544542129499168e-02,4.917703156213234020e-03,6.257852840851831078e-02,-2.769192815152852344e-02,-4.151436774826399584e-02,5.176186091651845123e-02,9.539058749420509242e-02,-6.349309761494561014e-03,1.142172792804594106e-01,3.239958520829297473e-02,-2.074519469896248663e-02,9.415573880462281553e-02,-3.869123780251434996e-02,3.289826301305114054e-02,-2.677474392371096898e-02,1.803102094320814183e-02,1.819889817522229482e-03,-1.390854100904989621e-03,-4.800753382166448452e-02,-3.588061321084037164e-02,3.115554481632098333e-02,1.104330872859149854e-01,1.328002907397665133e-01,7.007206143552253963e-02,-7.511832437821058805e-02,7.640025461152803732e-02,-2.162509720629908483e-02,4.716072798391540005e-02,-3.399187257672090651e-02,-7.479179522258674884e-02,-2.752173996187207497e-02,-1.089618694896627993e-01,-2.276040155392435038e-03,-1.151308643300171242e-01,1.848311100820182809e-02,1.017778089560018723e-01,-5.761405644958571270e-02,-1.835059301858892045e-03,-2.667390609808907229e-02,-4.088487263011377632e-02,3.096720219709618235e-02,6.334349903309995783e-03,7.656220358238312318e-02,2.431170806323927625e-02,-3.348976034512215344e-02,2.993449516611878039e-02,4.351123426666570621e-02,1.542349407427848207e-02,6.914533190533662249e-03,-7.553101847787017931e-02,-1.825807850928569398e-02,-3.230263623384622973e-02,-9.454941691408778881e-02,-5.152454716994793460e-02,5.751476936936328332e-02,1.512777643409466202e-03,-2.625074135154454452e-02,2.426596660337200506e-02,-1.393348182409343550e-01,1.369357253717168668e-02,1.402217732226638813e-01,1.157649802833425196e-01,1.717364816046674791e-01,4.113081069907070786e-03,3.608442885161899927e-02,4.287499503258949962e-02,-3.551477776804849718e-02,-1.788965862282503039e-02,-2.118800973973337595e-02,-3.801810960930179784e-02,8.494753013849992707e-02,-2.767214186659947700e-02,-4.343875766275970274e-02,5.539577852629180843e-02,1.094328283023772580e-01,1.248177532668342943e-01,-7.526068151519035854e-02,-2.505396446229846835e-02,-9.976233503711257750e-03,-5.107197909325629503e-02,-4.491306110185219053e-02,-3.542251031437162329e-02,4.882669049020236379e-02,2.508308220734326022e-02,1.861706316383312382e-02,-1.273541823010106433e-01,1.119346074399801480e-02,1.214930879619061099e-03,2.680023480724365303e-02,1.244957958985457892e-02,-9.713264113549359424e-02,4.896287911063286480e-05,-3.035762848369818676e-02,8.442804609308113273e-02,-3.198574657981748115e-03,-2.884958653624090930e-02,1.959085726844200792e-02,-1.700687634178871532e-01,-3.833311417941357574e-02,1.206835157423344004e-02,8.333170692440082516e-02 -4.418111425775238243e-02,-4.653208705650409283e-02,3.940356374483619528e-02,5.954029217356800185e-02,-1.178442658454905700e-01,3.549618445311510800e-02,-3.032259081017420199e-02,-4.954513789168055876e-02,-1.256121682128358286e-02,3.093767588442936090e-02,5.713325989165268792e-02,-3.063259826031161648e-02,-1.179635476433146812e-02,-1.820603138282994582e-02,-3.584668300006545183e-02,-5.442142861408763577e-02,7.309188686994935735e-02,-1.641815506347543974e-01,-3.373541571935600308e-03,5.103349095380850520e-02,6.259294096219014669e-02,2.509572249408109287e-02,-1.032356831751253881e-02,-7.747416928850181517e-02,-7.269562719357802738e-04,2.802335903870945000e-02,-3.974845071450673617e-02,-3.442537455218434850e-02,-1.462412638104771956e-01,3.647809094606391939e-02,-3.691564694206720537e-02,-1.093443598837955943e-01,2.558092674006179346e-02,-9.992736548280455045e-02,7.159533537340088960e-02,8.600739539485138807e-02,4.759900868777378752e-02,-2.768155907780206482e-02,9.503610757508947071e-02,1.671280576268300427e-02,-4.853566458725983718e-02,-1.182285694391837019e-02,1.611960848311654732e-02,-6.521401729214720833e-02,-8.351289117464768402e-03,-4.230893616621875936e-02,-1.121617791481235304e-01,-7.322607127059187204e-02,1.015835950627083650e-01,-3.613152394636526266e-02,8.306246953079331197e-02,6.788345603884103630e-02,6.080840849466793985e-02,-5.143768065893798008e-02,8.757539986970107326e-02,5.005407500841653479e-02,1.318863888276556803e-02,-8.230804665138150705e-03,1.200681941126384433e-01,-1.517737800246926963e-02,-1.450768383155224828e-01,7.980369665744649343e-02,4.518529989879503661e-02,8.876887229454907824e-02,1.421714441138849025e-02,6.061396870777130408e-02,-5.559879777784717397e-02,-7.157247702155387914e-02,2.136370217621202244e-02,1.080299199814570565e-01,-1.092694456339823317e-01,3.480523584931237424e-02,-9.569648969181357434e-02,5.052615247561347001e-02,-1.363490713170658307e-02,-7.600352304373970969e-02,-1.284552381830109113e-02,-4.368913260606475946e-02,5.433149311502613599e-02,-4.536376619016888739e-02,-3.121915097522926646e-02,-3.413728938183415579e-02,1.900383921777654139e-02,6.974641821143251659e-03,1.028610104940301806e-01,1.048027588910307728e-01,1.182074820808115406e-01,-1.814020575302606117e-02,-5.639868947477449934e-03,5.539666352331514693e-02,1.369648120833073801e-02,-1.295470548737796997e-02,5.573535805272451793e-02,4.192378098192238445e-02,5.081203833534715664e-02,-3.000455213863121445e-02,-6.216213745411529323e-02,-3.722831536169547706e-02,7.934374547366038893e-03,-5.107304858201687503e-02,1.609255219929521807e-01,7.267912035244754321e-02,-3.422632348882658111e-03,-2.421040284246190472e-02,-3.481413497118892447e-02,-4.230211690056984858e-03,4.757393182369268336e-02,-4.732245839828019829e-02,-4.917199242460319752e-03,1.080310941176275963e-01,-1.437187007923968959e-02,5.764758257217094534e-02,1.159436947517571148e-02,-5.512913273443849523e-03,9.064076311021339316e-02,4.556281711831573766e-02,-9.085532373255517102e-02,6.959604722640043428e-02,-5.977836586213179648e-02,2.823102129671904714e-02,1.186125646140258894e-01,3.656115456939942887e-02,5.840618031563839241e-02,-8.295765035469929161e-03,-5.081784457167417418e-02,-5.111641720709964726e-02,-1.148421548592358510e-01,-3.510786705330868479e-02,-1.714831318791072701e-01,3.110995712156971321e-02,1.114796048151044522e-01,-5.355343576779648257e-02,-3.990973839510883470e-02,5.162034882015220649e-03,-3.383860944348049404e-02,5.242304133701526647e-02,5.909230659501681082e-02,-5.236545032197212346e-04,-5.915540062818299555e-02,4.681880899126814044e-02,-7.569352916749101656e-02,8.263639764704600044e-02,9.067407264160748381e-03,5.199172194224527949e-02,1.254477978398621518e-01,3.439632350606929012e-02,-4.861185563307622581e-02,-6.350484511314864744e-02,-3.270959776293851096e-02,-2.066971713339623568e-02,-3.049615683166431723e-04,5.467851109813334082e-02,1.982539350633863162e-01,5.630132252816875960e-02,2.466446333959556800e-02,-5.050511855477488043e-02,-3.457791509518778894e-02,-6.319257468361239027e-02,-5.469991833629787858e-02,-5.035274480308757350e-03,3.399488380850352459e-02,-4.754414250035577522e-02,6.367584161637310225e-03,2.237388523693647005e-02,-1.912933241384438127e-02,-1.160401193840738393e-02,1.005571218154348301e-01,3.897417917602002957e-02,-7.226306723690811840e-02,-7.768332017874750306e-02,-1.068388394789663437e-01,-9.228673075478980442e-03,-6.125395324503969674e-02,-1.654279660972090074e-02,1.471750249943361288e-02,-5.859668572158255317e-02,-2.896794750349575287e-03,2.291806307929076733e-02,1.234874036270332612e-03,-3.971023676233729344e-03,-7.997012596017991082e-02,-3.243752153665312166e-02,-7.922008989389817124e-02,-1.453207885571038138e-03,3.661727412952367477e-02,7.623295247611834580e-02,5.278968119119362840e-02,-1.130675092026560968e-01,5.071841668300031830e-02,4.424614205955718083e-02,7.904041211053336458e-02,5.695595975646022824e-02,-6.894624616714051246e-02,-2.846318431444950364e-02,1.667450193152057725e-03,8.060975804243505904e-02,-3.364074029209835476e-02,2.897368169114949987e-02,7.247985080730622032e-02,-7.200182188757553448e-02,-9.891574195915822654e-02,-1.566837711383827769e-01,1.887508346297011794e-02,-1.401451055202105472e-02,-3.929631315240688511e-02,-1.554682276915669847e-02,1.284109689340492445e-02,8.051210789327302442e-02,-3.576485756772439933e-02,5.594930054425799326e-02,-8.301524352893455139e-02,-5.288599672803415142e-02,2.273072745296284003e-02,1.062257410150715398e-01,5.009731938945241603e-02,-1.621007386802082034e-02,-1.040729259734915479e-02,1.281168512979759255e-02,9.768074458493486087e-03,1.185342272038815425e-02,1.082059823559520934e-01,-2.496317291579675662e-02,-1.280788837778066777e-02,-4.918438162204163966e-03,6.864704812598063088e-02,-1.634705232277404718e-01,1.824953532633578207e-02,-4.399077089765817544e-02,-1.116316131861323259e-02,-4.076700734966601236e-02,-1.390378039112367958e-02,2.608994606516047005e-02,-6.117472995445019657e-02,1.840530896400385807e-02,-6.794714585293523679e-02,-1.362311514500785525e-01,1.053027968974524775e-01,-9.757615425168401219e-03,1.134133559883426234e-01,-7.601944990240176725e-02,4.466748377210850995e-02,6.962600206857058605e-02,-5.432503540087554178e-02,4.963227991951228019e-02,5.534182354033199969e-02,5.646330201236959140e-03,2.538658322843610760e-02,6.802020739039327246e-02,-2.626696611776858578e-02,-4.181333170984480319e-02,-2.106831074886493169e-02,-3.317089878171269185e-02,2.677713000445381894e-02,-5.538465220764433872e-03,-1.501041511270406748e-02,6.331674041539059328e-03 -1.255555224044192231e-01,-4.925382974693595650e-02,6.165445755300001662e-02,1.031828669187954545e-01,2.570489835594161332e-02,-8.379453878973588954e-02,9.176050799837739969e-02,9.271881271218344878e-02,1.341225241889745790e-02,-5.681798838451519729e-02,-1.553505013667930146e-02,7.029723799383716387e-02,4.806310630204024825e-02,-4.306503658855237449e-02,-1.321523322317800231e-01,-6.681819448457042288e-02,-5.271379293224573610e-03,3.833039908702400023e-02,1.010999199538403781e-02,-5.968474370894472775e-03,-8.389395681111681824e-03,-1.359525037548103138e-02,9.029730396131868675e-02,-3.009000994250816963e-02,-9.889961513269314441e-02,4.821724493604276351e-02,5.790202998038736226e-02,-1.126830263472472088e-01,6.792049371172477479e-02,-7.253634096429119327e-02,8.450061707540315603e-02,3.642693874996668496e-02,-2.329665637728691938e-02,-5.310837949739924402e-02,-9.960214366939815211e-02,6.301957159770715799e-02,1.369551111763645662e-02,-6.283536570103209762e-02,3.277923472216002360e-02,-2.811739500694408062e-02,1.114307847972841881e-01,3.026901445052238362e-02,-3.380370855049315976e-02,7.216908179057629491e-02,-1.008953872774326493e-01,-4.408965789545825353e-02,-4.942144235985281714e-02,1.153188986201008071e-01,-3.215168142624195285e-02,5.662663207845397562e-02,-4.730031494986219540e-03,-2.531075187513919661e-02,-8.544480060776910024e-02,1.942136280182554103e-02,-3.861761813828492457e-02,1.259259128501493546e-02,2.021404907327488051e-02,2.625377864992420665e-02,1.417186288191600585e-02,7.907417573455723103e-02,5.505313240782468143e-02,8.145021165877405767e-02,-5.212132936357519175e-02,4.000382684856385040e-02,7.787779458138167910e-03,-3.001793766153662210e-02,4.366761313867902061e-02,2.900486084356775074e-02,1.114799289404056620e-02,-7.787215627505568449e-02,-1.891033659393375269e-02,-1.010101388332183131e-01,-6.794385196245589231e-02,3.260613327683432872e-02,2.090981310108035174e-03,1.338438699444530894e-03,-7.276832798984897432e-02,-7.270496845424392895e-02,3.687932592948968413e-02,5.495900062336265407e-02,5.161183730422280735e-02,1.112332319418726234e-01,2.192425908859983036e-02,1.025764518079969392e-02,2.194754712256935525e-02,-4.429888380181637442e-02,6.675697729969241878e-02,-1.458166932082334381e-03,-2.863102944945445164e-02,1.392813236381473252e-01,-7.662926933451782996e-02,-6.055833057649631646e-02,1.127197285095814377e-01,-5.975208275783566725e-02,4.478282367779354289e-02,-1.068740512531114739e-01,4.415131884288004543e-02,-1.763635795414793955e-02,-1.141483010577347759e-02,2.107634280794180859e-02,-6.548779813896718482e-02,6.505486490528084031e-02,1.431241841188781588e-03,1.083999359866082146e-02,-3.494438253728805732e-02,-9.126602657151822173e-02,-2.329342762200523054e-03,-3.701931463896818153e-02,1.583369187075725770e-01,-3.211129879977117629e-02,-5.977518789965381718e-02,4.151000071770769018e-02,1.110939078704583083e-01,-1.481913835141764405e-03,-5.342846577901227967e-02,-8.778335935213699148e-02,7.828994286925100576e-03,-3.612118622221514169e-02,-4.164316049917491519e-02,2.881046622256965700e-02,9.011342509297848302e-02,-3.799367761161494272e-02,1.269147044537655145e-01,1.183250072875014977e-02,-1.312341185690391043e-02,9.457040245109477050e-02,-1.619743240169405074e-04,-3.099201649909403417e-02,7.720572906182601725e-02,7.742431903591341646e-03,-4.628718827385008838e-02,1.199048246933600709e-01,1.239794936994640939e-01,6.404869892428374789e-03,-5.839797619900422010e-02,9.533122585486161760e-02,-2.031431682898461946e-02,-8.642370473395587227e-02,-1.570949894652672041e-02,3.242439026578496086e-04,3.105580854994397302e-02,1.142708916633410066e-02,-5.239590201340752623e-02,-2.900466916436033465e-02,4.174206393751946770e-02,-5.594779997017110609e-03,2.431924123972864504e-02,-2.130325490660193999e-02,3.462374596639115204e-02,1.375096353528839632e-01,4.478213374407190306e-02,7.770201731554562952e-02,4.153036846948742616e-02,7.006101407424171479e-02,4.062735436435083863e-02,6.519397376072810779e-02,8.030118787782247747e-02,-5.526340161563195186e-02,6.933431080333447849e-02,-4.187848945594855460e-02,-1.032959299362498778e-01,-1.101275115186593073e-01,1.118419380933039220e-01,1.536057821349337896e-02,8.951383838320102482e-02,2.543954018520992039e-02,-8.155262757707601451e-02,2.110409761489505376e-02,-9.262884759580389260e-02,4.257398596247567724e-03,-1.459181204286380540e-02,-1.780697674668743624e-02,2.284543572851466253e-02,-3.181115867873165332e-02,-9.146022661240715834e-02,-5.245224751314604694e-02,9.088283372765545987e-02,1.796597904797772247e-02,-1.195069711372672983e-01,-1.185407178029952366e-02,1.822002870604609234e-02,4.107126786339313634e-02,-1.685559403052285660e-03,-1.643535769132637922e-02,1.750872550439402930e-03,6.667890116101182763e-02,4.693424753700414093e-02,2.460040604342426940e-02,-3.678621316004648117e-02,-5.466205308002493324e-02,-1.321710279736876964e-02,2.394669273735659451e-03,-2.719108309683406985e-02,-8.183312513250028120e-02,3.554278616924079481e-02,8.631328836407370364e-02,-1.237003965419325863e-01,-4.613238943570181771e-02,5.336042245619777857e-02,-2.472610210906620032e-02,-4.647005144453156961e-02,4.505908228037610458e-02,-1.299940655649223925e-02,-4.500641179542263365e-02,-6.705117997879439518e-02,1.019091785370778401e-02,-4.618778368634846726e-02,3.255494910784450385e-02,-5.948620338893454568e-02,-4.074819664433418365e-02,5.332017665677078050e-02,2.883996369806373847e-02,-9.909557056358378468e-02,-9.934097707331848714e-03,4.517013393891254613e-02,9.157256405944069061e-03,9.295002974678352736e-02,-2.374265430011621505e-02,-4.359868757314139254e-02,5.986621639463118177e-02,3.265298350837150809e-03,-2.187331932472821872e-02,-1.384785816350984633e-01,-7.565439046763064446e-02,4.228835204987015442e-03,-1.795128899993486127e-01,1.118602155457213213e-02,-1.788128725726629425e-02,5.941345499763370230e-03,1.954427659059909672e-02,-6.951527659525180702e-02,1.021920915679687542e-01,3.256869742256636446e-02,4.987518229099704992e-02,-7.780255643661693199e-02,3.635270414987876175e-02,-6.985221769773389267e-02,-1.640308896523447885e-02,-4.026162636480790785e-02,5.001231479366315125e-02,-5.365273071702216651e-02,-5.302845876972028111e-02,-4.331214613655276491e-02,2.632457994943426804e-02,-3.352041208734313910e-02,6.146269801149298329e-02,-3.516044549547269610e-02,1.708458730668347691e-01,1.030720585247477949e-01,-2.563362082897213678e-02,-1.660598333079814012e-02,-5.870564203979034001e-02,-1.131517727084965230e-01,1.229810810648660213e-01,2.403404083204035979e-02,5.906037953310397381e-02 --1.532918476252690121e-01,4.029334239348054958e-02,2.057047307606296033e-02,8.307265579978193037e-02,2.831409927126193882e-02,-2.896150448083627260e-02,1.619376337696330423e-02,3.459824615526818636e-02,1.085034989705919301e-01,4.215793429401844261e-02,-9.939342370282763794e-02,5.342405826920001988e-02,-6.495420717127491084e-02,-9.025381612716015800e-03,1.006675546719118866e-02,-6.661992182863493706e-02,-9.194679286929335893e-02,1.125275862952476863e-01,3.673593995538760454e-02,-6.434800905430022866e-02,1.606947164622807983e-02,-3.261967958573630533e-02,2.556211810065034742e-02,-4.081388816178564000e-03,-3.115754261561595045e-02,-6.752741459576206556e-03,6.122206602386294494e-02,-4.881455851587308131e-02,-5.274378633101997615e-02,3.331650548686350410e-02,1.971468635229462266e-02,8.870774954918951571e-02,-3.647194106062662255e-02,-1.774511007238166113e-02,4.287943373501393318e-02,8.098226670293275087e-02,-5.885375316810317892e-02,7.248235587060421203e-02,-3.838563810666352494e-02,-3.670163904140045369e-02,-8.659550946810529171e-02,-1.313420634560055200e-01,-1.961219676784674293e-02,5.322262312215631647e-02,-3.070321753397780434e-02,-1.118774057695611293e-02,-3.904311921364775340e-02,1.747641157286787539e-02,1.041412073429323903e-01,-1.640559593527622650e-03,2.890461174093034505e-02,1.520933594133458411e-01,8.400851137365036858e-04,-8.743993987951406699e-02,7.617312796024157395e-03,2.104964191197439272e-02,8.604474365223482510e-03,4.439691382749959975e-02,6.109465636109466524e-02,2.177971962630031177e-01,7.505984785064528930e-02,-3.631352747902744094e-03,7.123956216268213792e-02,2.099593813659585140e-02,5.444323124232693795e-02,2.975276761251417768e-02,4.327764990553224778e-02,-4.308726327147836821e-02,8.679823364772763783e-03,-7.812606661493269600e-02,6.539329098160626130e-02,6.590435670479344310e-04,3.932261261764369992e-02,-5.449628887160265500e-02,-2.454562131444609080e-02,-1.196901641422441764e-02,1.011566223351247629e-01,-6.248895994425161804e-02,-3.248763801340183444e-02,7.718214448076705925e-02,-1.909701174492540662e-02,4.188390721378359299e-02,-5.517235493460167367e-03,1.014211228624656708e-01,1.462473993932957850e-01,5.460036990028963033e-02,2.663977114001122443e-03,2.451030220965943335e-02,-4.314554286753517071e-02,2.933413701582469196e-02,-7.446628486939671498e-02,-3.021635198630752764e-02,2.467125639398374398e-02,-4.405053654911805783e-02,-5.028728389292268230e-03,-3.030499724251115370e-02,1.054012788219321761e-01,-4.073624854304222498e-02,-4.506163197653169361e-02,1.125247581606274479e-02,4.299910619580186999e-02,-8.772804914180011571e-03,-5.504316204721651506e-02,-1.044517025516864331e-02,4.956291031919660833e-02,-3.027259203336353510e-02,-6.714044966442120344e-02,-1.209708394214546873e-01,-5.578025133600039709e-02,1.150061133959703365e-02,3.131498981792310565e-02,2.219658690570964002e-02,-3.575543779844788522e-02,-1.824447892971346741e-02,-6.473693636881655555e-02,-1.727872190976529471e-02,9.212673090002569676e-02,-6.339240147720671958e-02,1.009129085511595823e-02,8.644011703532925028e-02,3.285635127740339778e-02,-1.389246369989122036e-03,2.428737780368765524e-03,-5.046853055628421070e-02,4.344376253899111424e-03,-1.076419310105980742e-01,1.688303052364943657e-02,-4.792875567179263191e-02,-1.443911809914189340e-02,8.075368180339259627e-02,-1.822709611828463808e-03,-4.503510764050089321e-02,-6.167686234408312207e-02,-1.457013629523963572e-01,2.880878307452607034e-02,-4.499641961846527483e-02,-2.360637380417071954e-02,-4.899073829377884048e-02,-4.756931797797496475e-02,3.864542982693018602e-02,4.245228465857548567e-02,-3.971160483053349372e-02,1.125249952060380781e-01,1.070278364507580710e-01,-7.227525346046131949e-02,2.943116491310709265e-02,4.099609109876577867e-02,1.714758358984293934e-03,3.998079706203385569e-03,-1.734341520766581601e-02,-9.135378894323960275e-02,-1.234758067235089685e-01,3.499890821792880913e-02,2.881129803567428205e-02,4.896538260139945975e-02,-1.801595643918374803e-02,6.625877207476259245e-02,-8.613384627506355876e-03,-1.107678088615168949e-01,-3.005116170484075158e-02,-7.475037476174102580e-02,-4.984185839774869786e-03,-8.986150892002359170e-04,-2.381269540991848982e-02,-7.312804305986773945e-02,1.324530976857434851e-02,-2.014619850648049007e-02,-1.315238271564154737e-01,9.691982120640386944e-02,4.096186152368153088e-02,-7.899233226313670531e-02,-1.211691242276730952e-02,-1.099623403661828952e-01,-3.885958249912021406e-02,1.054967490033433264e-01,3.769417074189237299e-02,-1.131982802634046992e-01,8.099711780576566988e-02,1.607127042897335545e-02,4.675140176221205535e-02,-3.038763000297520916e-02,-9.599456002782874586e-02,-3.629335053011656376e-02,3.971252182924132207e-02,1.812494962247639466e-02,-8.735075365945445952e-03,-3.893311877206789146e-02,-5.050169964769950920e-02,2.269467861064979053e-02,-7.941512099875926156e-02,9.087618199568277266e-03,2.019221835642505750e-02,3.164670644167523646e-03,-4.403096135955334584e-02,1.596222947569925915e-02,7.679817674004298100e-02,1.347090483131984512e-03,7.039379296831521637e-02,7.221800502954776879e-02,3.263150943556555189e-02,-5.797502392479288169e-02,3.796540476959488208e-02,-5.485452922444089899e-03,1.467066987847556389e-02,5.550063512594343579e-02,1.904762739704049176e-02,-1.111540949148405039e-01,-2.278032038463764786e-02,-1.403608212861290686e-02,-9.064627999177117534e-02,-1.818215108363281998e-02,-3.664278919562746761e-02,-2.586547179124299572e-02,-2.417180017030431602e-02,5.670219319779641609e-02,-6.352508419872743284e-02,-4.704782729944559372e-02,-3.504283755787861165e-02,-4.245104457310949764e-02,-1.507628789273898415e-02,-3.093772896502897801e-03,6.656095278862046716e-02,-1.025878025287110806e-02,1.383342879138563797e-01,8.353192482117194340e-02,-1.048704915264489154e-02,2.455857938986814870e-02,3.417686781190439754e-02,5.765929526576284853e-02,1.010264938788510702e-01,-6.862475723864384924e-02,-4.933237655333293226e-02,-3.375022257708647222e-02,-5.990497233755379125e-02,3.501862864086636995e-02,5.695497803284593763e-02,1.913995066750329849e-02,-1.367387208710000857e-01,-6.220069311981969717e-03,-4.611639595936248281e-02,-2.777565141625061920e-02,-1.248416856178499734e-02,7.516302077838002416e-04,2.275528562311058406e-02,-1.432497503831211549e-01,1.135680912469266834e-01,1.272460511855206255e-01,5.005272299724015939e-02,6.070270265104035085e-02,-1.937639428183159616e-03,1.717965803347067943e-01,1.000897266181804968e-01,4.631636528947085524e-02,1.139095643267905300e-02,-1.809788877677445673e-01,1.052090336362120071e-01 -9.970847896969872726e-02,3.203594579947183912e-02,-1.817080521025126599e-02,-6.008489571060686429e-02,-3.786310969451980868e-02,1.155697523717265085e-01,-1.415349914384795571e-01,8.017943880427419456e-02,1.022973475755380585e-01,-8.945551850732307020e-03,-5.499739410364010150e-02,-1.603307107373524998e-02,9.430687382413083936e-03,-1.160765699611628343e-01,4.112198299897455028e-02,-4.248879731586967246e-02,-1.317755447838355777e-02,-6.180194238238641280e-02,9.770656254901657900e-03,-6.613652132744542167e-02,-1.797052223236048074e-02,-3.942869032837710563e-02,4.937796420202768960e-02,4.913187784746135545e-03,-2.229164803001208728e-02,4.264502727345881317e-03,-9.017578419311297888e-02,-1.778300851909434075e-03,-5.320853095607430000e-02,-4.512885958540605036e-02,1.108230589256128246e-03,4.206788222487682638e-02,-4.099480720940774925e-03,-9.458075900218826382e-02,-2.663289096667285916e-02,7.690788320350054608e-02,-3.856781346029444779e-02,-7.681712236928756043e-02,3.330256226853593737e-02,2.338990246032945225e-02,3.964626645535240801e-02,-8.250857070226530476e-02,-2.716267977715176515e-02,1.543355149201801058e-01,1.516164851627700878e-02,-1.131620889584903505e-02,-8.941287382195572953e-02,1.247675066243322112e-01,6.234005124639847850e-03,-4.564287248443843481e-02,-3.915793863366669181e-02,7.115330559649024611e-02,1.089768819536571925e-01,-6.337945820469348401e-02,-4.994470191716492147e-02,-1.202748167828771586e-01,-4.191546521159426886e-02,-1.882390492724205500e-01,2.616759714194185488e-02,-7.710266728781946244e-02,1.043251580070816975e-01,-5.124666264459961512e-02,1.314913902653138955e-01,1.200414860930316269e-01,3.368298177811547556e-02,4.981687991121992037e-02,7.412651745372156320e-02,1.070690977342250022e-01,-4.866772538490751060e-02,-3.435539673037512481e-03,1.195924864945883126e-01,5.893602617822722711e-02,8.289254663188010824e-03,-1.410431328105747617e-02,5.446051012536143321e-02,-3.088004486734674883e-02,4.449594815638024947e-02,-4.573885238707112166e-02,1.697123018009514539e-01,2.704191166126181795e-02,1.040251127440136036e-01,6.607975339776966939e-02,6.904554118120799500e-02,1.230986754496435909e-01,-2.845030391729641561e-02,4.279791937204503044e-02,-1.219621990918449972e-02,-5.524258475293050002e-02,-2.967327441394301477e-02,-2.067654462034434690e-02,-2.934066625084139068e-02,4.948299513358350721e-02,1.054107858675032081e-02,-8.240541267928623570e-02,1.340872216548100529e-03,7.874664594933397654e-02,-1.253264176505140548e-01,7.611272406716265704e-02,6.189132261599198867e-03,-9.241476198415132792e-02,-8.397174786080989495e-02,5.342727918250804453e-02,7.827434495679810866e-02,-5.154392816535784128e-02,-1.080974004424409621e-02,2.401232749818445547e-02,5.095759883875984608e-02,2.869585643106364187e-02,-3.897893988226423106e-02,-5.759101441014644746e-02,-9.215870878014899639e-03,-4.016478322940985474e-02,5.285465151452534149e-02,1.729200624701508066e-02,-4.724911613024802837e-03,-8.832639982424808633e-02,-3.807000179518963551e-02,6.035481916772400535e-02,1.061535036851278674e-01,-4.931794454946218653e-02,-1.076519167702340574e-01,-1.893392102983600112e-02,2.708727931892949276e-02,7.345133106829276892e-02,1.163983086444403542e-01,5.473604530723329142e-02,-1.054924122812443688e-02,1.324845724913949196e-02,-5.428483961155983195e-02,3.934492453945200724e-02,5.819784960174835542e-02,1.683475798345452495e-02,2.480048496151886911e-02,-3.128678387361286117e-02,-6.298176852744696852e-02,-3.255212348212976198e-02,5.748563854475547547e-02,2.599915313101414799e-02,3.424536333397352333e-02,1.212397467680471214e-04,3.463905060282506526e-02,9.016436754006002668e-03,-1.446465320817714728e-01,6.255513815556423482e-03,-3.844909034036235462e-02,3.690503383804720483e-02,1.392685235071695372e-02,4.243319629721538366e-02,-2.206577596451902989e-03,-2.652630311687978470e-02,1.686945579424308644e-02,9.497565141086358742e-02,-7.893559437578248628e-02,7.188551143627296136e-02,-3.518450438822379265e-03,4.708063887423820371e-02,1.252103164684561371e-02,7.645864408098362874e-02,-6.112031063059131242e-04,-4.690403963725300190e-03,5.757760729518385939e-02,-3.392109124697343520e-02,2.851071526097317260e-02,4.503843684950674733e-03,3.515142612056240679e-02,2.834488770674560565e-02,-9.530934084873400602e-02,-2.716079916554214185e-02,1.423133713953021184e-01,2.876409511250343359e-02,-8.073365504580430618e-02,1.216791631803126189e-02,-1.175984060147246130e-01,5.426325457806729230e-02,9.613431198255008303e-03,-4.244410995721270563e-02,6.017015524429215195e-02,2.111036276234120809e-02,2.277178688695839434e-02,2.218008434724877062e-02,-5.563056304751471531e-02,3.782410998809997149e-02,-7.732274152895969588e-02,-4.956708394988565414e-02,-8.130074322814156984e-02,6.821843613565517495e-02,-1.348858617577884667e-02,1.111298658688835150e-01,-1.162514688275007729e-02,3.301091039392968496e-02,-3.334714400264982698e-02,-1.996845635359288662e-02,-5.705093388326500092e-02,4.906892314513923090e-03,7.239188208996767682e-02,1.520573296331508006e-02,4.174121262612136285e-02,1.990548484936960350e-05,-1.855227825993002039e-02,5.206219024725285138e-02,-9.260817595624619425e-02,-5.223458552979991926e-03,-3.439889669182018272e-02,5.111295351772670442e-02,1.035124142002132736e-01,5.297120741158143492e-02,-2.110581525459668401e-02,4.221946218528818684e-02,-3.986786536372836920e-02,9.031344120097803407e-02,3.875426768251194476e-02,7.652078940402096130e-02,6.079437970513168997e-03,3.997841867036915076e-02,8.191119032974193692e-02,-4.677137595241329104e-02,1.944409807827573652e-02,1.369635321442975728e-01,2.899057720601967009e-02,-7.342112304233065767e-02,1.931621070696382669e-01,-4.007192654310078039e-03,7.186150838342484498e-03,9.970083110646828228e-02,4.796199977439726675e-02,-1.208683866812897828e-02,-9.077378029438788742e-02,-2.739909073984445748e-02,-1.931256779844213109e-02,-5.437288659956020231e-02,1.423373320189253582e-02,1.047077044820346990e-02,1.434530763186736904e-02,-5.134029809992474158e-02,9.360098648171830127e-02,-1.461313030222362655e-02,1.098689728771109347e-01,-3.311762086343353340e-02,-8.851363054711727449e-03,6.668249850887592034e-02,1.346425865455039006e-01,-5.849663785523255555e-02,5.162508137911567263e-02,-7.026325835284796817e-02,2.842271740973551647e-02,9.427232609787911805e-03,-9.154354220548346910e-04,-2.434273254000963657e-02,-3.632070728085644989e-02,-7.369924649161635824e-03,4.452428797842515606e-02,-1.114171544903882344e-02,-6.916392638141684313e-02,-2.177046084945671173e-02,2.294166677387677461e-02,1.441265010065000476e-02 -6.413803362875054870e-02,-7.358547215676933262e-02,5.704987406668839828e-02,6.458820573578652469e-02,5.948694698690581983e-02,-3.950487327459933917e-02,-7.537570690181460975e-03,6.212476864929078642e-03,-2.819776028664504330e-02,-6.455707301390894992e-02,-2.650371316224391094e-02,-5.510053436101020052e-02,1.157716824492008578e-01,2.887950849305571943e-02,-2.469991744681329648e-02,9.054430110964295508e-02,6.900787065893793093e-02,-4.780497415893555577e-03,-7.583518924067440825e-02,-5.740273971887164400e-02,5.994338772141602489e-02,4.887661369588415516e-02,-3.197903031275243363e-02,2.207474062099579887e-03,-1.490760241659885854e-02,6.813808887780287149e-02,2.042647814938462758e-02,2.384966163133191153e-02,-4.889172675663319007e-02,2.927718079183059402e-02,-1.608570261317199962e-02,6.889684817696027641e-02,5.852974754897545540e-02,1.082791910443229103e-02,2.881573909881679496e-02,-4.029225169269955165e-02,-8.736164312024530254e-02,-5.995481380633801982e-02,6.055105252765626000e-03,-1.141331508413335159e-02,4.793478847049646024e-02,1.074378353600193362e-02,3.478500012489370297e-02,-2.906985366720857278e-02,-2.533009285197631785e-02,4.289674355472594680e-02,1.559134194826434019e-02,-8.688657546103417673e-02,6.233104702371516231e-02,-7.026335325954116001e-03,-2.474397330842336915e-02,-4.936160133047394483e-02,3.563495303641069090e-02,-9.329117973955548815e-02,-1.021835608202042028e-01,-6.702463085299792400e-02,3.872005472931446673e-02,-5.363683891782244917e-02,-8.665428324705041693e-03,-6.909514382830128643e-02,9.088956641705689052e-03,2.903588503587172659e-02,2.113004672517956178e-02,-4.203844846223895454e-02,-1.343067708029265298e-02,-7.285890021455690424e-02,-8.503346005691138701e-03,5.887369471487431671e-02,-3.248989237758967419e-02,-6.724893744884800906e-02,3.714899171059447142e-02,-3.467488040595038956e-02,1.378510217128586512e-01,8.049450939378102743e-05,-1.168490242618162173e-01,-1.216856075093035797e-02,3.145098002519426672e-02,1.153991256004674726e-01,-4.813671462561842918e-02,-9.734223707592472297e-02,-1.002917764622977825e-02,8.467365202411697780e-03,7.673401888300296347e-02,-3.332642218492639502e-02,3.838306050604840813e-02,1.336222527561911622e-02,-7.893661955865176327e-02,-8.282176610237337866e-02,2.472055702892256984e-02,6.327320018451437766e-02,1.459906844792368263e-01,7.555046800342610919e-03,-8.286515857824675035e-02,9.508108167676335154e-02,4.960990939085856144e-02,2.292667901441150513e-02,4.582108800730556181e-02,2.623592334196607478e-02,-1.950866393066560261e-02,2.204871609294964743e-02,-2.619993399431791090e-02,2.559845690050191122e-02,3.509355936559671596e-02,-2.381021695508235558e-02,1.389246916319579195e-01,-1.171767764082801491e-02,-3.153787978756379673e-02,4.078619633866286537e-02,2.613479230087201177e-03,3.483795346813901850e-02,2.540184664914224566e-02,6.505814377297244877e-02,1.338906201925830694e-02,8.314953183925596902e-02,1.728432544077596403e-02,-3.559461029401588678e-02,9.812039742444345070e-02,1.688236152508425930e-02,4.549287808016087842e-02,1.221845312351012985e-02,-5.197570735631035568e-04,6.737678802245938847e-02,4.378369563662494468e-02,-1.208406269612671835e-02,-5.537559153794380240e-02,-5.382367979315905160e-03,-1.544041129206968344e-01,5.840721297876118323e-02,-5.427245715684751726e-02,1.891471657604870682e-02,6.079825671739716442e-02,-1.238496709430312037e-01,9.623547471211629789e-02,2.835764539773860324e-03,-1.271525546469317036e-01,-1.077475756021957151e-02,8.348295236346728321e-02,-1.196148784419904937e-01,-4.886743260778700360e-02,-9.109030731169656669e-02,2.716685069384189993e-02,5.838936051451738135e-02,4.372529120755098975e-02,-7.014371046400878773e-02,1.128017650594377030e-01,-1.442498090171838165e-01,-1.693782546080549803e-02,4.203603249431448613e-02,1.556090229068415320e-01,-1.088269567983691198e-02,2.072099887379810768e-02,-5.681795657652103509e-02,-9.343332975132123686e-02,-8.793458964442814330e-02,7.284446421976155384e-02,-4.981953176901277036e-02,1.872040655613118054e-01,3.073387880884333873e-02,-1.170182314207858232e-01,-3.518036948177975798e-02,-9.439182654675273149e-03,6.909480977546263170e-02,-2.078150262576973495e-02,-1.023460640288075124e-01,-4.344766488340277150e-02,-3.690486119151005495e-02,8.996846042185396852e-03,7.850355498709500612e-03,-9.101894381661480848e-02,6.166410032731739488e-02,-7.999493184174817639e-03,2.145358593810483411e-03,-1.559857778523249944e-02,2.050332697274762084e-02,1.050134772143317824e-02,-2.071715707494189984e-03,3.817328769788742859e-02,8.342188417535152667e-02,-1.037782326339219408e-01,-8.863031797592944849e-02,1.168190047616206595e-01,-6.900529132385199715e-02,-4.786282786452975002e-03,3.660010225141895751e-02,1.165871798739054610e-01,1.026668239595790871e-01,4.825188219210005403e-02,5.586918429717938062e-02,1.520601558027887545e-01,5.077052203112523282e-02,-3.877126524418365860e-02,-5.359578859168594167e-02,-7.418520426383808097e-03,-4.736948476097081404e-03,3.561952529849744115e-03,2.854565957063220222e-02,2.287656326140610069e-02,8.386825787463164650e-03,1.463459780143498978e-01,-4.099921557085253743e-02,-1.636594972424985084e-01,5.922790544188529134e-04,2.230447203072106754e-01,2.653228792800842173e-02,4.758340689544076491e-02,-4.872472527619611110e-02,5.199649866270215914e-02,-6.402761436925868965e-02,1.389609222334275279e-02,-5.884008068521998092e-02,-2.625946700546007151e-02,1.095481042940558009e-01,-4.833162378487060318e-02,5.538822234993547033e-02,8.862313191143850188e-02,-3.214131088475667697e-02,-4.873769239332641223e-02,1.170550325280478136e-02,1.425852049724432927e-02,-2.332756314228211225e-02,6.803743719076559004e-02,1.849764874801407277e-02,1.396083980971313254e-02,1.160453049298342844e-02,-7.553834552921084855e-02,5.168554746814865014e-02,-2.322328027996883176e-02,-2.447366712164150693e-02,1.557728175500232293e-02,1.757348963417964052e-02,1.021407608884449848e-02,3.679428278074217944e-03,5.272204495681985514e-02,-8.617642342787903975e-02,-9.475958063950329358e-02,6.368145846784761810e-02,-4.571385402441673074e-02,2.835401064818872038e-04,9.590958910563052164e-03,1.167798825935901369e-02,6.060720520732171962e-02,-4.664327725657288481e-02,1.859194344947808983e-02,-1.436356345625462369e-02,1.026380189632639245e-01,2.649692369177326229e-02,-6.261826760383190638e-02,3.680484836594168085e-02,4.862112290855549052e-02,2.812125992774106020e-02,1.086964606195455624e-01,5.887765978574620307e-02,-1.531223245854210213e-02,1.713281885695769335e-02,-2.895015079269896638e-02,8.825176264360457684e-03 --1.185635925322837785e-02,9.339072434350496887e-02,6.914330775932375772e-02,-3.749909919360805421e-03,-3.696813797406051094e-02,-7.778969430971072185e-02,-2.159517848249944971e-02,-4.852682203497604707e-02,9.905528208268066237e-03,-7.071533642322065660e-02,2.668519043544408803e-02,-2.793681082229853424e-02,-6.076284586745540894e-02,2.716290265835356876e-02,-3.149038492585398696e-02,-2.367949550977144213e-02,8.656197914007696115e-02,-2.192298138134493590e-02,7.851464709766989292e-02,2.788436535579339598e-02,-1.254058564977616752e-01,3.057095751109430853e-03,-2.956696312522284070e-02,3.913967389090915183e-02,1.069805364323743978e-02,-3.006793636406486503e-02,6.163815562097818557e-02,5.887267165491637949e-02,-2.616446666028929280e-02,1.561087791232764616e-02,-1.398369024029570175e-02,-6.907017412223265262e-02,-9.219199195664234958e-02,-8.400402143440686975e-02,-1.260393004196116717e-01,1.060249095423845128e-01,7.883033379213069680e-02,2.123594136995761092e-02,-1.137836074712599288e-01,7.212273459885175375e-02,-9.740227972648839161e-02,-8.610851925208570790e-02,-1.253138452507684963e-02,2.105847895318919277e-02,3.717230609428416599e-02,-1.131404355332854827e-01,-1.126838251682225622e-01,2.372673370309605345e-02,-4.289135863021672351e-02,-4.869503541355693632e-03,6.250329151794642435e-02,-4.816781909368281905e-02,-4.241356135633909952e-02,-7.349211408972169957e-02,-3.271883540405598478e-03,7.001380133205448164e-02,1.357273281804412535e-01,-4.111428560996577275e-02,7.263256136978860078e-02,-3.103190611108615216e-02,1.530695400810632711e-01,-1.981521202676097757e-02,2.093758592519721212e-02,2.298707337463140425e-03,4.039527449504861506e-03,-3.493912986133203280e-02,3.556325724466854327e-02,-3.983921202382403881e-03,-5.907950793914294729e-02,3.720428441131398700e-02,-6.145583684151875725e-02,2.282189138748351442e-02,1.751975936006801293e-02,1.100031400795259406e-01,-3.606991662707178387e-02,-5.291465831394391583e-02,-6.661112638344197923e-02,9.790781411981036575e-02,7.495326306613514511e-02,-6.431346185503876434e-02,-4.587139339570388968e-02,5.233130109544152295e-03,-6.006290743873319432e-02,8.733948528020929469e-02,-5.180667896686526330e-02,-9.400855033623235768e-03,-1.941457454581936859e-01,-7.279540132511249512e-03,-1.873410138745812414e-03,-5.786619854980699917e-02,3.606553665080393378e-02,-2.142466849956355113e-02,-9.960586419270306413e-03,-1.110554654728023588e-02,8.123685155686807477e-03,-6.227385512453477434e-02,2.582660758177612870e-02,1.065576865056144795e-02,-1.042689768662127120e-02,-8.042862471670976510e-02,-5.554601534099808113e-02,6.846919387869215834e-02,3.272045214276197295e-02,-9.332662871938623983e-02,2.504254596105059266e-02,3.619367447606522403e-02,-6.800709451371921010e-02,1.540299532249293779e-02,-2.793210312101144371e-03,3.466337477836151093e-02,1.394572445985216291e-02,3.939366257826159429e-03,-7.613522439504756545e-02,4.603472966221919260e-02,-4.501650842319807055e-02,7.568548985680331742e-02,-7.114650076730530404e-02,-5.593964830949464673e-02,-2.542261017512105134e-03,-2.085212936366045433e-01,-2.410140422687669787e-02,3.088681218485569632e-02,-8.099441891833351892e-02,-1.850117812064055664e-02,6.856876666492130179e-02,1.636343735343991773e-02,-1.108314464284971407e-02,-5.448374075404904121e-02,7.459845721379310124e-03,2.090132264609779189e-01,-4.928664571622583951e-02,-4.047246695073813266e-02,-9.023058662687566400e-02,7.422837354369113405e-02,-3.164829269476932688e-02,2.587187918786232332e-02,-3.783595315027332706e-02,-6.351692588289793717e-02,-7.195030629751526796e-03,-1.486766323682671698e-01,3.157229690209871487e-02,-2.894996162558271732e-02,6.245235975204546652e-02,3.148229025872193582e-02,-2.159106089297983463e-02,-1.041069839252900253e-01,9.887406097385666864e-03,7.722430189885558238e-02,6.055912218152583598e-02,9.337833850092358556e-02,3.397866461292885221e-02,1.332129713707303105e-02,1.018396846706328407e-01,-6.445874858085670622e-02,2.102081244885514386e-02,-8.098423023117110042e-02,-8.773498645997586387e-03,3.369351483187668639e-02,1.215149425724626198e-01,6.712371471787975663e-02,1.595681318605333965e-02,-1.581832257343802201e-02,-2.675694523332812838e-02,-1.697536189666834679e-03,-4.712538698547210497e-02,-4.532955222009943386e-02,2.720559366833711495e-02,2.044025192425744444e-02,-7.748155355945765543e-02,-5.783465434988083187e-02,1.960892572134542711e-02,5.353304149567963272e-02,5.065410216133799798e-02,-1.275769282396885806e-01,-6.688591939769644723e-03,-8.588535356975125767e-02,8.580558673833205141e-02,6.601870936757506247e-02,-1.297737303599257389e-02,1.062169933489537019e-02,-7.880015100688037011e-02,-7.744342269957939229e-02,7.872498517879450430e-02,1.012105702627080495e-01,5.343912477220419399e-02,-9.579254673822590327e-02,-3.108408919186591429e-03,1.768630523388280418e-01,-8.427372949831726801e-02,-4.064017941001132200e-02,2.612690377927162633e-02,-9.780717933116238327e-03,1.807696402002705732e-02,2.106136509041281121e-03,-5.982402812802989356e-03,7.649637850200702305e-02,-9.095934615879358587e-02,-1.709689791385590185e-01,-2.822647444532885852e-02,5.082155537691063901e-02,5.160983950932908348e-02,-4.013029938132826879e-02,5.255144349180952690e-02,1.495966654515909142e-02,2.301902066297670246e-03,2.560768301049545087e-02,-1.109194446737027662e-01,-3.994381284699081991e-03,8.528653041592211548e-02,-1.040185964482554515e-01,-3.667066742138066354e-02,-5.883980946224470765e-03,1.237140316722689336e-02,-8.502854200047312710e-03,1.772804723083789072e-02,7.321464542071801085e-02,-9.001377201991411781e-02,-8.136518199401931301e-03,-2.717108241055748302e-02,-5.633428247766606584e-02,5.821721026899470686e-02,-1.033103157511900544e-02,6.396156168018278521e-02,6.739761807028242513e-02,4.046521792383551974e-02,-3.965947090831035277e-02,-3.536245007778737737e-02,-5.243265150747696002e-02,3.889539449806835025e-03,3.840726705560133097e-02,5.386184446732815434e-02,1.324121614434735188e-02,-1.314586522322209738e-01,-3.182198888875944348e-02,-8.838242223758448896e-03,-8.925691726433870876e-03,-1.710547903244027251e-02,-7.674389538223572171e-02,2.813802112183885068e-02,3.929872700835061738e-02,-4.135728300715414696e-02,-3.889248252086494745e-02,2.324427300648274769e-02,5.236181892250261344e-02,-1.148091486773678005e-02,6.259671025807590078e-03,-2.230205028615109275e-03,6.876831914758572972e-02,-3.475045701810511312e-02,8.013157526969844069e-02,6.053774556184270128e-03,-1.235248765225956391e-01,3.180975093412694715e-02,4.188149137916295089e-03,7.984035888046244617e-03,-1.473444712468150244e-02 --4.996224308032367473e-02,-1.357612603460709583e-02,6.838205955815737647e-02,2.640686960707624986e-02,-1.024936977018425788e-02,3.718700566080479497e-03,6.319497507746188458e-02,-9.080589145955707264e-02,1.437143311161840839e-02,7.509783158332117292e-03,8.535358192498299079e-02,1.523837570161727314e-02,2.184641796643184419e-02,-4.401178691051237102e-02,-5.311514998463515158e-02,5.737590373047297965e-02,1.114761241723323526e-01,-2.944608543155325525e-02,2.111965588919698100e-02,-1.095472701856367928e-02,-8.111634955280845605e-02,9.446851280652791838e-02,-9.487253683875021548e-02,5.352478702569891944e-03,1.138490837184240670e-02,-3.968414632716982249e-02,-2.555424958324219870e-02,-1.290546366489631336e-01,-7.557651616447232101e-02,8.063755342851140762e-02,3.177481623300748570e-02,-1.822666417517264728e-02,-9.320448463360324343e-03,3.411144463368037671e-02,-8.136988246283903548e-02,-3.567102333519708846e-02,5.438002327027099958e-02,-4.209572937629713074e-02,-5.720881003480674659e-03,4.236425259993092940e-02,8.001911328912276328e-03,1.489251533672370301e-02,-1.261243462937740312e-02,1.008329285378788409e-01,-6.598248269801751387e-03,-5.772560464634837046e-02,7.929885049526215479e-02,3.506450397050309142e-02,1.348791691708973173e-01,9.653723444168985091e-02,1.405273173907279488e-02,-6.090703031863678452e-02,-6.865165044757802482e-02,8.590340643068167248e-02,7.733881005424181332e-02,9.198902680899320727e-02,-7.553074218925284744e-02,-4.865605777247989511e-02,6.813601864525198337e-02,4.235371530655081262e-02,-5.035483607133783845e-02,-9.295307128954797410e-03,9.353105511747500378e-02,-2.301871278641219598e-02,-9.124271641063644300e-02,3.484619144636617871e-02,4.716911868628801546e-03,-5.043881417254980626e-02,-9.506381831183609299e-04,-1.053742716721348810e-02,3.152142536131719019e-02,5.226789471730961673e-02,-8.559611727347725407e-03,-4.777143003797845039e-03,-7.962119735460251868e-02,7.358886551414071941e-02,-2.476243562008481267e-02,7.666029849054067924e-02,-6.983069867182373214e-02,-3.451208606972921283e-02,4.047251306126700005e-02,-1.793636127908434727e-02,6.594585579885248183e-02,2.469884087995675992e-02,1.244173888966433605e-02,7.457334298954181040e-02,5.599429647180273473e-02,-6.560527913863145599e-02,1.435120208344136672e-01,-1.360398021273751326e-02,-6.312989890369093138e-02,4.691225662211369329e-02,5.559056494620743388e-02,1.532058925991147680e-03,7.128589141267866608e-03,-1.911113161394961957e-01,1.618701875859320793e-01,1.387993843626436630e-01,-1.056368198483061405e-01,-6.101872140727992999e-03,-1.733973908036447881e-02,-1.094060259627328835e-02,7.542728876865231746e-02,3.361210750505796985e-02,7.049572854978750601e-03,1.009632957768418632e-02,1.718832338342210844e-02,2.833329057907219006e-02,-9.161734338075541717e-02,1.445008886271752398e-02,-7.969739708261326661e-02,2.179409798380028962e-03,2.159774686284179901e-02,5.367170477835606773e-02,-2.159901940850717308e-02,7.329395525053867405e-03,1.333983712950048521e-02,-9.460893947507728607e-02,3.268174034367098135e-02,5.994215580586312320e-02,-5.298596500985721519e-02,-1.415719162113202589e-01,-1.682206086642824144e-02,8.814089214370431080e-02,-4.776980829648917737e-02,-3.242297492305111495e-03,-6.872152416010396392e-02,1.601614029148662813e-02,1.000203167496853743e-02,4.572226022164722903e-02,-5.101051471930975051e-02,-4.053415221001374635e-02,-1.542773068726514818e-01,3.209785344220748154e-02,-7.521770478227526868e-02,3.194434081129413072e-02,4.490319508199190623e-02,6.707500474519456635e-02,-6.870075163666297668e-02,1.124141155337478326e-02,8.135067725001751682e-02,8.107634847455379157e-04,-1.117249453483159793e-01,6.044242630614415218e-02,7.271256017954888518e-02,4.475868029432708561e-02,-1.450733152394345811e-02,-2.433322251346121609e-02,-9.451360025887770022e-03,5.418109802574316847e-02,-1.811678334391392947e-02,-6.766531140299399810e-03,-4.105367202297355300e-02,4.311518163747887072e-02,8.881196672799896580e-04,1.634323913564451891e-02,4.790188041811175440e-02,1.070313556783863279e-01,6.547093105232956733e-02,-8.038592832282343004e-02,9.278613254766590601e-02,3.271226519924654019e-02,3.306348097989960516e-02,6.210469558969852061e-02,2.624700051226322092e-02,-1.398660536792196984e-02,3.524792389076970844e-02,-6.299300178391067928e-02,1.402750778524176900e-04,-1.208364092965059228e-01,8.255213985638690244e-02,2.275429635705418507e-02,5.265402355024670356e-02,1.058917800148888866e-01,2.931544585637789122e-02,1.490997792159904634e-01,1.465896358212409600e-01,5.931509180037215084e-02,9.609753445472998812e-02,-1.022074999662772599e-01,9.622353851308812489e-02,9.258333451035770031e-03,1.062796999347850879e-01,-9.075106348564092318e-02,-2.401575686543382104e-02,4.830153138053878970e-02,-9.606284578663659701e-02,3.512870087373401451e-03,-3.337043579015343264e-02,-2.897725207305945280e-02,8.625273067618050016e-02,-3.095602311903140891e-02,7.942270011186311485e-02,-2.887428880415409890e-03,-3.842185548965337295e-03,3.182219649156013686e-02,-1.050857410389593877e-01,3.594542831426065371e-02,4.334868803813105020e-03,-2.996517987289418899e-02,-6.213651168948798598e-02,-3.802248913859443413e-02,3.221528782659568990e-02,-4.313229356652482221e-02,3.227931822721866184e-02,5.501052055536803653e-02,8.273332647222197966e-02,-1.260504116677536868e-02,-1.729358352462841836e-02,3.862275959708707379e-02,-4.404829771416870349e-02,-5.892775777261756442e-02,3.632196334731853382e-02,7.799199365244625926e-02,-9.567636965837773844e-02,6.294212576915772517e-03,1.773254644948754621e-02,1.258952578050239746e-01,5.846256402539804636e-02,7.260246527701505470e-02,-1.025262278094177248e-02,-1.683131761749969910e-02,-4.279399989343773969e-02,1.014633351465110134e-01,3.469114965126984113e-02,-2.807793961416570061e-02,-1.905607377326114932e-03,9.569900155456761981e-02,-1.245336761746009824e-01,7.487458874223823502e-02,-1.797315471184277036e-02,8.724435275220462915e-02,4.347711594480312830e-02,3.418591047379333780e-02,6.572380283100463794e-02,3.464288566469711506e-02,-1.653244332893070642e-02,-7.856203492552426768e-03,-1.087375380558526505e-01,2.550377589560892666e-02,6.471824803493153588e-02,-7.534559777321298002e-02,4.323354923823236051e-02,6.564643862617080411e-03,-3.748376398119569047e-02,6.811469999367995853e-02,1.261600420318522708e-01,-9.192203883353612659e-02,-6.005414107470251339e-02,7.957454369783995007e-02,1.065684282454418264e-02,6.275523229232828071e-02,-5.695212343578368724e-03,7.011008060243344164e-03,-3.382091068060086303e-02,6.915730623042357431e-03 --2.664452283476747771e-02,5.290676113151766141e-02,4.383976956551360882e-02,3.750020135883397332e-02,9.331641408402426008e-02,8.827945801092183797e-02,5.700350041421720082e-03,3.020959136131393108e-02,-3.848090253274372846e-02,3.219715127632825913e-02,-8.564850077995167510e-02,1.191166459323026311e-01,-9.252128452465539082e-03,-1.770525721271454245e-03,-7.849774489740732331e-02,-5.756946210014228515e-02,2.197825165943954190e-02,6.196834626528906903e-02,-3.838106666270790779e-02,7.056334050947814240e-02,-7.484274271738221296e-02,-8.644912782355719527e-03,4.413662449939265153e-02,7.569760928260794650e-03,-3.417242204902655456e-02,-2.822748522121795722e-02,-3.394695829050937974e-02,4.083396347549204941e-02,7.384633160979998945e-02,2.141929519071151394e-02,7.945155664496839076e-02,-2.296861939666071384e-03,2.314195948811592976e-02,-3.054983745155823160e-02,-9.011325127880237540e-02,6.133581524372958005e-02,2.562791771096097765e-02,1.046550275756368137e-01,3.141263452660292277e-02,5.082545263893031157e-02,-1.004513035499225576e-01,-5.913704940045165503e-02,1.145421390445661991e-01,-3.131867918295858783e-02,6.820473384671349448e-02,6.483837997562075239e-02,1.198010273918834745e-01,-8.454655794747346215e-03,-4.696433601534426294e-02,1.240113627290468368e-01,-8.761030580893439113e-02,6.265762270792485378e-02,-6.818227607842447524e-02,-7.339605337090984727e-02,8.893370658794513828e-02,8.878510065106823279e-02,-1.996955565234282901e-03,1.069449770218486306e-01,-6.026499807938584530e-02,-3.179526878002740531e-02,-2.673508242541596663e-02,1.075559648309991623e-01,-3.013467998827684333e-03,7.884606930810589276e-02,4.035773334695693521e-02,7.438353114338906724e-02,1.032233020297288750e-01,2.620062540796343903e-02,3.447000741828534082e-02,3.530947931826808328e-02,-9.617843450514686199e-02,4.707623218546669319e-02,-4.696970513945612075e-02,-2.705033139150157459e-02,-3.676077604582308339e-02,-6.915736370353184337e-02,-5.896401945660630084e-02,-9.320469536097925056e-02,2.471974517534687721e-02,-2.230510607269087231e-02,3.438771760607181593e-02,-2.427019999668516420e-02,-6.377484035600056167e-02,-5.194966546978046704e-03,-4.770819175730077466e-02,-1.530294448492614812e-01,1.123251290453222001e-02,4.303411917842973888e-02,-7.967365460306502500e-02,-3.444035656968035680e-02,8.574955254441808672e-02,3.508535650741455986e-02,-7.039873094853295321e-02,-1.614533069932431314e-02,1.540755253918369427e-01,-4.085658460298850769e-02,-1.630781415884865332e-02,4.917854662274547678e-02,1.490119624847421886e-04,8.323929883486361203e-02,-7.029720712249969139e-02,-5.067179967910760002e-02,9.195379716397761305e-02,-1.256990328254414535e-01,-1.208274457307525161e-02,-8.007087313907463796e-03,-3.362169122456008102e-02,1.065002910164035699e-01,-2.826883507203993562e-02,1.311036794574843567e-02,1.783078504850514612e-01,3.452480403785820710e-03,-5.813860816045348170e-03,-7.563916368585253680e-02,1.597919438405465825e-02,-1.305138762283499995e-02,-6.033579892030010300e-02,-7.604028257082967590e-02,2.673009850290564843e-02,-3.566008506027028985e-02,-1.113230732272380875e-02,8.364479782494961546e-02,-6.057617294096438038e-02,1.140410724104496537e-03,-7.520213702054687199e-02,6.435500842885033719e-03,-3.191501830316357396e-02,-1.508044344815525836e-02,-1.224374615846045428e-01,-8.215430397737012724e-02,3.120044095439461138e-02,8.452758107542518440e-02,4.357857435614529568e-02,-1.400982151516526331e-02,-3.168299998732979073e-02,1.018798746970422803e-01,7.246948314080567388e-02,1.660137082087549676e-02,7.160775632240251598e-02,2.436763992187057290e-02,-6.682315545856935476e-03,-1.532948493969458863e-02,5.214160256886857248e-02,1.684195916491291328e-02,-8.450233911621300720e-03,7.008787730401004612e-02,-5.388855785715876423e-02,5.230931325067612880e-02,6.830720183677826962e-02,1.024769734620305378e-01,3.814507542832090009e-02,-3.469077177956825242e-02,-1.764788803750493712e-02,-3.193920178338886112e-02,-5.061118299019111422e-03,3.413064520048519901e-02,-6.259664107581754122e-02,3.388408407768178643e-02,3.213010953596971003e-02,-6.449840656339154987e-03,-9.004845132569629096e-02,-3.239185877769877597e-02,-3.584063923526194934e-02,-2.851297696509325769e-02,-7.212805465909351288e-02,-1.201724611010775295e-01,1.041896012128086524e-02,5.451699937635863524e-02,1.640949436755581581e-02,-1.108215822066190293e-01,3.334967560236503509e-02,5.602902593964249889e-02,-1.533501156338404336e-01,-1.978498363167648100e-02,-6.893511255622104816e-03,9.623319835786285636e-02,1.091392309866989208e-01,5.094265370197154780e-04,1.596623899143079228e-02,7.065256118040558198e-02,-4.950744592056605109e-03,-8.219280784073419721e-02,1.318017250404512897e-02,-8.937768440121968139e-02,-1.195422362492275584e-02,3.416560328008676206e-02,8.799572610093678304e-02,-3.505826302317751469e-02,1.268256857445168931e-02,-1.098313316781501098e-01,-1.857376883612778945e-02,3.764784956923447612e-02,1.500055700068612594e-02,-6.010345420336637989e-02,-5.005094081913425558e-02,-7.242553953546454482e-02,4.676369324025161922e-02,1.309189692647455272e-01,3.474806567581136679e-02,-2.796358281478658961e-02,-1.331132408925448651e-01,4.347959343865082094e-02,6.359785970922482434e-02,1.040237867824608947e-01,3.606614224806539010e-02,-2.589651309603639706e-02,1.203447911543026327e-01,-4.800729716983028611e-02,1.886160070420042287e-02,7.192901183726738493e-03,-4.941339631788532402e-03,-1.667598896712387327e-02,-5.179155346866952392e-02,-2.817268556886501005e-02,4.519923594809685530e-02,-8.370214797002874241e-03,5.672903151363006130e-02,3.706721452443044462e-02,9.214703771273655142e-02,-6.456676508279722559e-02,5.962525542292099884e-02,1.087139693744829938e-01,-4.398596303200922458e-02,-7.801045709802340022e-02,-9.199464915025296063e-03,-5.389846871444333581e-03,1.776504788598430931e-02,-2.920675171475959481e-02,-9.021644565305046715e-02,-9.743224056347629669e-03,-5.094860477527615727e-02,8.221911537934183112e-02,-1.068357732436596430e-01,-8.084457022316579589e-02,7.349675009710816831e-02,1.437187300172047301e-02,-1.920760105517937575e-03,-9.793294067346046405e-02,-1.103317462773768520e-01,-2.789967334498546711e-02,5.320635286148685250e-02,-1.236631820015561545e-02,-1.044855758707015636e-01,1.811771817628278491e-02,-3.303831145049476031e-02,1.697968562691171829e-02,-2.989293399306829080e-02,-7.475693674650826792e-03,-3.259281777229311450e-02,5.814015594507056239e-02,-1.853807929595506498e-02,6.966211705279413968e-02,-2.727691826246790574e-03,-1.258789261685391847e-01,6.360634014408557413e-02,-2.893810266109990426e-02 --6.388037809431335035e-02,-1.153515094903277416e-01,-1.155342530396062661e-01,-7.491765573669958989e-02,-5.547958907856133381e-02,2.051989153296066015e-02,-9.887511841333573859e-03,4.306420611794734010e-02,-8.983613130127526825e-02,-5.532151509648697285e-03,9.172830481580537820e-02,9.229464222092947923e-03,5.215018772063848701e-02,2.834331589568708787e-02,2.303333042386376817e-03,-1.025906952104528869e-01,-2.189665427977462164e-02,9.335803473072355751e-02,2.017354948783217639e-03,-3.426172785629734430e-02,-1.993806386542108788e-02,-1.406218218097426975e-02,-1.374777387287364760e-01,-3.673813280108147172e-02,9.180219784566376329e-02,1.403713355248493305e-02,-9.857733199277624037e-02,-2.346153047275203324e-02,1.635692131541484073e-01,4.240827042913677264e-02,4.802342954250312518e-02,-2.927007900251167929e-02,-5.381650495213006341e-02,2.350537724855241567e-03,1.042940446786645159e-01,-3.794763561657213402e-02,-2.493780358777404962e-02,-1.479922328490026855e-02,-1.466986217412755145e-02,1.576696160554414902e-02,1.572237979023005661e-02,-3.586831123811771299e-02,-1.412966499347808047e-01,6.652307147342943761e-02,-1.252187811523114425e-01,1.816948072395637714e-01,6.388181664901404488e-07,-1.718230535851746760e-02,-3.501256389859711687e-02,-5.477100648647499626e-02,4.938156591054795874e-02,1.458764975876846934e-02,-9.205831459057947641e-02,3.354449407145135287e-02,-2.842167242138602529e-02,-2.213624915060641643e-02,-5.734832787564418666e-03,1.214570940315398695e-02,2.509556460487577523e-02,7.000782451645802540e-02,-3.088931028271158646e-02,-1.311865434988565213e-02,-7.980080276703631348e-02,5.268384926111447167e-03,3.589063664319779023e-02,-7.681422675947065237e-02,3.343383371927617642e-02,1.050950514356415871e-01,-3.704754299272714629e-02,8.772293058348946959e-02,3.516171653904630323e-03,-2.360558832560642750e-02,9.291967277166086892e-03,-2.596943614852882318e-02,1.609620484068727958e-02,5.995095861694484107e-02,-7.034670198999393165e-02,4.761501561819140443e-02,1.382442067395903695e-01,-7.629435302444870226e-02,8.365195375277054840e-03,-2.070848554203342032e-02,-4.897212451733807398e-02,-7.715048825077880368e-02,1.030240804624568962e-01,5.832357512846542136e-02,4.234215565322248243e-02,4.827441821661963839e-02,-1.023815147106809931e-01,-2.830975318562906767e-02,-2.226606736102113362e-02,-6.142284604626334354e-02,7.222293515807855918e-03,-8.161261502303981952e-02,1.279874749932979061e-01,4.782793002688361800e-02,-6.231762887822350772e-02,3.699506929579997327e-02,8.566448457788972126e-02,8.055963222509367194e-02,2.110362497353296743e-02,9.017281332203917754e-02,9.164193936180209388e-02,-4.684370117856402616e-02,-4.747489697694120409e-02,-1.176359391312654795e-01,-3.251100398883932652e-02,-2.083320078998964625e-02,-7.544758033325400792e-02,1.568312544094831174e-02,3.689860835281044520e-03,3.501449016009095666e-02,2.420775016924879564e-02,2.900155672314758473e-02,7.888769115479341010e-02,-9.764140374061729133e-02,7.968290602823717428e-02,3.408246284166549511e-02,-9.319587738753982303e-03,4.766807501482760168e-02,-1.300537425827750782e-01,-4.652920420808965424e-02,4.973605643230179923e-02,-6.550158666679983810e-02,-2.194848719665696418e-02,-6.217673093672555773e-03,-2.739485067987168510e-02,-5.575602819361563656e-03,2.566377684664802178e-03,3.916921534776129921e-03,-5.075796307287044895e-02,2.565046185391179484e-02,-6.192439769117179769e-03,9.330035365553390747e-02,-1.797679107630918705e-02,7.182500186743159820e-02,1.585703811552786890e-02,1.033958920005853960e-01,-5.205505971035401247e-02,-5.094277922050327589e-02,4.157902687658512203e-02,6.003360464894751114e-02,1.222671609117099295e-02,2.267666708960785690e-02,1.713965022717370279e-02,-4.168417006922818524e-02,-5.680647770883921754e-02,4.012949433950973449e-02,5.138565544803583296e-02,1.794800120726400827e-01,-4.141637545162744904e-03,-1.586633422340458233e-01,6.668483522009044373e-02,-6.701669514848854203e-02,2.317042204729558019e-02,4.399510066646641904e-03,4.061965197600453381e-02,4.828140167527392168e-02,1.126711521268102673e-02,6.740838277285288171e-02,6.041852358774411902e-02,6.718972408935267038e-02,-7.615024572547260395e-02,-3.329714028038788937e-02,7.759871327831541210e-02,-1.202697560074815070e-01,8.672701959937760552e-02,-6.598776528881394876e-02,5.704598792506566929e-02,2.076557734725544002e-02,2.585266526239564377e-02,-2.166719943068628682e-02,6.607239436621459294e-02,3.047617118606527065e-02,7.130755002933790443e-02,1.939047007261158334e-02,-1.563923300411065864e-01,6.966623151564325933e-02,4.020494047389513298e-02,-6.201666956277197829e-03,-1.717208392401020078e-01,5.934393374094452944e-02,-6.833169513375564330e-03,1.305379014092622469e-02,6.983850139593918149e-02,3.211469800669860403e-02,-3.975411348231263492e-02,-1.891041678205115928e-02,-8.088958924092563840e-02,-1.011632444632989680e-01,8.104937776842691122e-02,-7.010787454738079405e-02,-2.015964442728302927e-02,-5.968294769442411551e-03,1.839004937992860106e-02,1.420430669824957609e-02,3.222801314552999064e-03,4.462867951621672657e-03,8.364511260112237126e-03,-2.422487095730982767e-02,-5.588429479690724955e-02,4.625479370300129150e-02,-5.276956108653333907e-03,-7.757667106267690516e-02,6.821060531440321928e-02,-1.125470210413108407e-02,7.905350834677646321e-02,6.176001684473080011e-02,4.725345526772240130e-03,5.417664171756096658e-02,9.397399247644415421e-02,-2.205640946047970896e-02,9.518101131538725390e-04,4.303948651882367332e-02,-6.915725534416991560e-02,1.093676915163880881e-01,-1.013448566851956334e-01,5.597657883919895938e-02,-1.652561820252974323e-01,-5.241541967046417505e-03,9.108460870709393642e-02,-1.256157906374830505e-02,-2.731423504852529410e-02,4.116676356872598502e-02,-9.958435966754190447e-03,5.112749313169826643e-03,2.164863002582954540e-02,-1.303962008295909691e-01,-4.873409214421749180e-02,-8.560807826858278513e-02,4.788246621217179255e-02,-4.619849908906856206e-02,-6.754559203691264113e-02,-2.718072453646807468e-02,-4.803540272822234142e-02,-2.719908458052756189e-02,-1.489914989839852960e-02,-5.556134895749299113e-02,-6.230788016956433084e-02,4.071760878922802557e-02,3.912489109421217470e-02,-3.742815302373292408e-02,-4.909197567711692800e-02,-2.548569029224620475e-02,-4.182220062166705499e-02,1.023177312333718031e-02,-2.723411876216795535e-03,5.226328799320979768e-03,6.554730992876742213e-02,2.895320638520763185e-02,-5.450562755056666958e-02,-1.471605952952428908e-02,4.725812246650924753e-03,8.097745990773858038e-02,-3.601007657641351611e-02,-3.101552866578412282e-02 --5.354120248703231333e-02,-6.194170865782683277e-02,-1.335302818959854115e-02,4.523200602495486972e-02,-1.666309296963420367e-03,1.141777065824965513e-02,6.461021312769768543e-02,3.691817542431489324e-02,1.537420581756015481e-01,-7.323643438726878951e-02,-5.998299990110429747e-02,1.560273244607384702e-01,1.663186959480425497e-02,4.237608018257554843e-02,4.068250514364395165e-02,-7.516572683410793765e-02,-4.154410442721048757e-02,1.738926120373550377e-01,-1.946097413913834995e-03,1.059620713210355086e-01,1.224981786693376054e-02,1.186820771243594363e-01,-6.730489937767367148e-02,-4.241022550496278964e-02,-1.543003263667919352e-02,3.226772201117104194e-02,-4.479926159752856824e-02,-4.177058328752601690e-02,-3.048735195215497573e-02,-1.529094969987844672e-02,7.407147431343506783e-02,-5.401757752796931128e-02,3.714670273488555885e-02,2.372716256835981710e-02,-1.156946791760211923e-01,-6.371080937198869698e-02,4.900408226100806675e-02,-1.750756168969712090e-01,9.302979574524117279e-02,1.672619097526141865e-02,4.492627550386794899e-02,3.189661899677227741e-02,5.081495898294728641e-02,-7.982723850223377715e-02,-3.006216658806394257e-02,-1.256082801146871965e-02,-7.042858023067058437e-02,2.347993757249766320e-02,-3.105415636788974240e-02,-1.502787400889233904e-01,2.762797010815810916e-02,1.588376181972830231e-02,-2.735049197937624785e-02,1.302627156742613979e-02,-1.311981003349614247e-02,-3.737298825143069159e-02,7.443011156358222968e-02,2.253857327947461631e-02,7.260523745422700626e-02,4.439190995046950194e-03,1.250197554538832502e-02,-1.915124615513201858e-02,-5.426973952812630397e-02,-1.012946069941213501e-01,1.227870259866184061e-01,-5.747217112581296666e-02,5.976007552130884609e-02,5.861881418184233805e-02,-7.675500885376818072e-02,1.275407986899578135e-02,-3.328362316379546160e-02,4.651357828510763875e-02,-3.580558763911413644e-02,1.069102284090704263e-01,7.352219895128703220e-02,1.713283683755706735e-02,-3.320538875398461320e-02,-3.888278083780408179e-03,1.328129632986993491e-01,-4.789405990594643864e-02,-5.098727486782882179e-03,-2.284016727422740964e-02,5.965025368323184640e-02,6.646986643325146493e-02,-1.309049527261864632e-01,4.963992358367918384e-02,-2.344641984277326049e-03,6.408492852264210948e-02,-4.034700617992342808e-02,2.331864724128313374e-02,1.037137658316472366e-01,-5.359797815264071008e-02,6.598836364351433337e-02,8.554993094591430147e-02,-9.151248964874171554e-02,4.032920216407436670e-02,4.529183665401571857e-02,3.395194761362088642e-03,4.970383907025694575e-02,-5.006058925017986549e-02,-9.585126298140744794e-02,1.207255749099105449e-02,7.295199694362526632e-02,-2.494229831656175761e-02,2.740541642499805253e-02,-4.247166785983105819e-02,3.355872834012406380e-02,1.754114114079897979e-02,-6.821327882825829747e-02,3.903536974983698449e-02,-3.946598364182567903e-02,1.831902542092818387e-04,-1.907414266638335354e-02,-1.367705531589293727e-03,-3.063706571038515153e-02,-3.622055108018617381e-02,1.181676628557168929e-01,-3.199190180070163314e-02,6.581808508561649146e-03,-9.326214624637266248e-02,-1.765726527766181206e-02,4.416406705648942554e-02,3.103306838252856137e-04,-6.932204839254033080e-02,8.741025409598399132e-02,1.269737059535374843e-03,-4.071569003188303670e-02,5.037994400392352412e-02,-1.512256682184562676e-02,6.597522411886738020e-03,-2.480652916234456912e-02,1.795352171809506264e-02,3.668954615695257243e-02,-1.311242149731072504e-01,1.131472590760309108e-02,-5.668410213909469419e-02,1.002514546172226725e-02,-5.823787956359918749e-02,-1.144536807474495349e-01,1.496157533373548565e-01,1.034374230937878079e-02,-4.792260807214433760e-02,5.326251056481876717e-02,4.952479640843142350e-02,1.294855600736633250e-01,-3.292599594106811883e-02,7.834509161374188235e-03,-1.253306304491215162e-01,1.661392291695482776e-02,3.695727056790819309e-02,1.095148753739194347e-01,2.890833852380949448e-02,6.217716876631153300e-02,1.114021883176337518e-01,-2.965146917665376081e-02,-4.874049365087942953e-02,8.491528109843705235e-04,7.486221304604812055e-02,6.160981699738804496e-02,-1.153807362849206125e-01,1.654598943064776451e-02,-1.185079655297250484e-02,-5.296197661390901479e-02,7.246359872734395302e-02,1.679865288448258165e-02,-5.911512975869332581e-02,1.390627496381271200e-01,-6.438229166453135477e-02,-2.625658071046526741e-04,-2.040627831163426692e-02,-9.878650490320436961e-02,-1.888746649748501705e-02,-5.166839797788935068e-02,1.424806301765787174e-02,6.012631460795613003e-02,6.927218084815688071e-02,1.015386813864990051e-02,-6.837419966584443176e-02,1.227268303524824411e-01,-3.915837271943812398e-02,1.844353192637434788e-02,3.769368809130879683e-02,-1.766817101908492449e-03,-6.404404709142935692e-02,-4.016022513261251203e-02,7.831702715835753281e-02,8.438515638842881972e-03,5.382431933325173418e-02,3.841174272229729014e-02,-2.875605527406400498e-03,-7.612838720841084983e-02,3.642914285760839865e-02,2.781640068245532990e-02,-6.215282426191681697e-02,-4.479350675422663630e-02,7.661965372353165410e-03,9.415211805414434865e-02,2.135511591764244210e-02,-2.110446658845295531e-02,5.406107442573047928e-02,3.631032842493112783e-02,-5.207678418630681816e-02,-6.970853105582877629e-02,-5.447299131078148227e-02,-7.835702368284823416e-02,8.117467069417415998e-04,2.066414269643397812e-04,-7.896438362138238909e-02,1.921455765788254721e-01,5.183661272534629411e-03,9.368621565783535193e-03,5.014222318860570016e-02,7.345509039771479579e-03,1.561659156963584455e-03,-1.153670454727157924e-03,7.240442148276192424e-02,3.718470050629945484e-02,-1.787331188869601545e-02,1.672265391216110064e-02,3.420752065747165171e-02,-3.280012557039736071e-02,-8.538986998862990729e-02,4.232637019681406015e-02,-2.011520709336295232e-02,-1.249941712759810863e-01,7.651931743310988465e-02,-3.544597784428538817e-02,1.967047576796436201e-02,2.254140720854832386e-02,9.006826493665462396e-02,-1.405275222032671412e-01,-4.742767999653069860e-02,-1.541160837131251397e-02,4.518569980155158483e-02,-5.762012103744994229e-02,-3.409506023728837293e-02,-5.388651424683190555e-02,2.269631955875222110e-02,5.301891736738813254e-02,-3.171005756575683010e-02,6.092575035861640120e-02,3.039329246160777515e-02,-6.154771892217916762e-02,-4.191200800850733779e-03,-4.782933033386833899e-02,3.703854870302797364e-02,2.840235387199798248e-02,-5.747443361545574031e-02,-2.683392667142247417e-02,-7.382841674435142165e-02,9.512991124623287886e-02,2.124862042675523810e-02,5.135418640858348260e-02,-1.079445615097000405e-01,4.898008690325576092e-02,-5.281158122731539895e-02 --6.354885945335488062e-03,-6.224038048076456324e-03,-7.967621943315641997e-03,3.949923335855097528e-02,-1.383183809418692956e-01,5.736767482453301870e-02,4.836093121497138381e-03,9.012820521326879442e-02,-1.197727204862688091e-02,-1.180279306299851783e-01,4.224401379659993572e-04,3.902538291289501388e-02,8.427862451832333901e-02,-6.159649588730439773e-02,3.145882341473304339e-02,1.230007462044489797e-02,1.269185322826382190e-01,-7.736446027043455964e-02,5.356571997022711862e-02,-2.471295013506745117e-02,5.241506498060684305e-02,8.068834662679237602e-02,2.190549829446872265e-02,-8.967867563342116211e-02,6.936712258943522266e-03,2.855479611446084046e-02,-3.050537248883788272e-02,4.191070059795119068e-02,-2.974879329791890042e-02,-9.569279095109625710e-02,1.228990170106746341e-02,-9.081372603055111459e-02,-1.291211923068794412e-02,-2.714286719428584543e-02,-1.034552482170638527e-01,-4.431695022376917759e-02,5.279383817674335178e-02,7.406476172755328824e-02,-4.623530375299544221e-02,-3.390909209510697530e-03,-4.734350727654322577e-02,1.256844635278354938e-01,-2.286636801231773514e-02,-4.350375793008871145e-02,2.848258507284806909e-02,-8.033813989769965358e-02,6.289776183102278873e-02,-1.283235295226869618e-02,2.450298029045870976e-02,-5.364183602548486599e-03,-6.102180973238063508e-03,4.863188401009613249e-02,-3.115393984713596409e-03,1.798003911317513082e-01,-1.601157967462285003e-01,6.820964599345154256e-02,-7.979157970522733623e-02,-1.478046430412552070e-01,3.709868435455312280e-03,6.373995739555113338e-02,-4.415844307830530530e-03,-2.496457727501307997e-02,5.019724127089239540e-02,-9.113177205605749523e-03,-5.385933770126817566e-02,1.204134060350446483e-01,3.058236193043549142e-02,2.525949118381211680e-02,-6.601364392070857567e-02,1.049903939176587975e-01,1.643295824922827442e-03,-2.084798451565922361e-03,-1.013818831911506707e-02,3.377893298196568000e-03,2.499670400579672228e-02,-2.966387777553416982e-02,1.398072626657408668e-01,-8.438766718722540139e-02,-7.241988503174683761e-03,-5.278092027853457241e-02,-6.626825189392028526e-02,-3.869555011681474060e-02,2.038529682571652707e-02,-3.925174318571990340e-02,2.752170383228274425e-02,6.629431346203517976e-03,4.164799181056233479e-02,-3.615621288802032054e-03,2.043237546852089484e-02,1.398449976706427067e-02,-6.080904913655114929e-02,-8.268617078855251823e-02,6.439988871810707616e-02,7.070631699957802274e-02,4.290649151039309384e-02,-1.386003079425503388e-02,5.255136578710922768e-02,3.184709946165990152e-02,1.598963003552600695e-02,-7.363948674713920520e-03,-2.460845959096433916e-02,3.081628337304329057e-02,5.203240313926801863e-02,-2.461635102412666060e-02,-3.708485769020985340e-02,4.245739087161681108e-02,-1.522143515719841800e-01,-4.227342382103858182e-02,8.920148765185131889e-02,4.168933727914474197e-02,-1.556614403082553777e-01,2.857498548409932379e-02,-5.821813078503813854e-02,-1.360278427661935907e-01,-1.731853634758782554e-02,-3.417577813859496577e-02,-5.164713467330827278e-03,9.247027454104798580e-03,4.752902376081102886e-02,-3.497685535350805730e-03,7.592852320322906849e-03,1.031694284980403872e-02,2.932183978911440808e-02,-7.996212538455001184e-02,2.734441336495356534e-02,-6.349834826844379110e-02,-6.955114382766042214e-02,7.026290969917048379e-02,1.080525478371224465e-02,1.743690728583537769e-02,-8.915639423586761703e-02,2.270049245825279949e-02,-7.606537921170872241e-02,3.840702763394675257e-02,4.937006608015394316e-02,-2.509466233288546594e-03,7.394328505114500238e-02,-6.485188145835248108e-02,4.953723950270207815e-02,-3.490380771893978468e-02,3.381610836525666269e-02,1.080588232662556269e-02,-9.422030621622992305e-03,7.779654993105977745e-02,-5.485960243762216110e-02,5.573881650036795893e-02,-7.498246082354777589e-03,8.561935120924576981e-02,2.016939333140277055e-02,8.838164047201080287e-02,-1.564922033096492748e-02,-1.212527833002408906e-01,6.859271267051382515e-02,2.453090150116263302e-02,6.058571193949584266e-03,-3.546602216127193227e-03,-5.412275989913317709e-02,-4.515676801534908996e-02,2.011242119459055386e-02,-2.015351585683568345e-03,8.364537038925901058e-02,-4.916992658669996907e-02,3.711497867368490633e-02,-2.612807423140268287e-02,-7.338932605270465093e-02,4.551599145502527310e-02,-6.801730425688573556e-02,-6.513748822686589446e-02,-1.191550239064084228e-02,-3.513211885606284945e-02,2.656336829278527212e-02,3.001098364488630210e-02,-4.375153900249626610e-02,-2.146838004740666758e-02,-9.017904287005044928e-02,7.600152077387010985e-02,-6.366837514655053487e-02,-6.061059902443336617e-02,-4.141980472199810470e-02,6.008941989589290655e-02,-4.927203253978984510e-02,-8.092043626835765391e-02,5.377575516519605969e-02,6.399941965146152545e-02,3.100947587709553324e-02,2.740814756638600483e-02,-6.401661421949984832e-02,-8.229795095648773096e-02,3.807796615712345095e-02,7.663174633800645674e-02,-7.981453475018951416e-02,-1.198968106443735054e-01,-6.292455282426138996e-02,-9.380042988692435758e-04,-7.199655035774746303e-02,2.143451427091540989e-02,6.988059756700702729e-02,-9.261294297517842777e-02,1.963992338610131769e-02,2.750259620242637322e-03,1.783261760279396257e-03,7.096725168469267264e-02,8.217117252503082503e-02,1.061498075008316250e-01,1.029354341997248783e-02,3.041047333690729410e-02,1.394341479180188220e-02,-7.962531807962733299e-02,7.440862166491549190e-02,2.754270775942396776e-02,3.574546244055203104e-02,6.807938965873799875e-02,6.537013458270451993e-02,-1.033537079228046135e-01,5.455234887782675623e-02,-8.399533010258822263e-03,-4.125058182221574138e-02,-5.888501641242239841e-03,-1.121588231666893198e-01,-7.001112730826025699e-02,5.009496456338034809e-02,-2.498380598825581875e-02,7.398690123363003968e-02,-1.405575586301856250e-01,-6.748387158134371011e-02,8.097503458470699592e-03,1.569841283932436882e-02,-6.826790450957063994e-02,4.315338310505325431e-02,-5.853726753229554491e-02,-1.415420387672508551e-02,5.631061272702049536e-02,6.422809118392688865e-02,-3.362672334904094618e-02,9.835886165163031103e-02,1.031769826117689554e-01,-8.364234426974652414e-02,7.919608514303570074e-03,1.735565832615868984e-02,-1.199644907224174661e-01,-6.782290436518366439e-03,1.121931394137065663e-02,-3.071560759806061039e-02,-5.381549385515271844e-02,-6.076435675105477940e-02,-8.140817044455819773e-02,4.328687779304296601e-02,-4.080346216961248940e-02,-4.828196395975423161e-02,-9.862065756918049486e-02,6.516475014210126870e-03,1.694055096419604389e-01,-1.015230931241042356e-01,5.000295227401747172e-02,1.249538079176842342e-01,1.096251008295007490e-01 --3.389202035957789017e-02,-3.675655554687570076e-02,-7.382544333535634440e-03,-2.677919816277089804e-02,-1.100620835481970136e-02,-3.261894336872109973e-02,3.079359889146493501e-02,-4.670160923355008015e-02,1.131563155111394714e-01,3.478569687968131874e-02,-2.069930205028658321e-02,2.375241877455451511e-02,8.126422002982583437e-02,-3.111106066900090616e-02,3.039388463393055684e-02,-2.070204466720043657e-03,-2.380898374378660753e-02,-1.882828416489172318e-01,7.718439783017595757e-02,4.504171883677374894e-02,8.101437328081934064e-02,-3.472473822649840836e-03,-3.103145701651323363e-02,-2.856097096213226771e-02,4.538594491436735712e-02,1.806914164942109713e-02,-6.107749756685987991e-02,-1.032760877983721348e-01,1.798781032964668486e-03,-8.499300244010943306e-02,-2.187642990913939850e-02,-8.289517295644921457e-02,1.734728996992049993e-02,-1.535218613046034075e-02,-1.063201399204416892e-01,7.472087649886895167e-02,3.343450890380825907e-03,6.297103318105350123e-02,6.802352540524869706e-02,-3.483152224369901774e-03,8.282116518613151512e-03,-1.555596637511554026e-02,6.206876314891335289e-02,-1.451800464782330644e-02,-8.684617034169023198e-02,4.084721026785032016e-02,-2.608864647181251273e-02,2.201654681131525945e-02,-4.549750368782348731e-02,-9.912799962219032146e-02,-3.162476162180931361e-02,8.755887107025039917e-02,-2.898986233122004957e-02,1.259216583077567009e-01,-1.179612001887053035e-02,-4.370183148131179252e-02,-1.121951628318448996e-01,-1.261893791887621491e-03,2.725109792410076656e-03,2.754724450137469721e-02,-9.562186769345132342e-02,-1.035803022914411481e-01,-5.584936474355532005e-02,-9.934815769132979146e-03,9.749056142149006843e-02,-3.224500653425277363e-02,1.038398015284743292e-01,-1.266786056107112313e-02,8.560343858739147271e-03,5.511360183269112950e-02,-3.755883135169604814e-02,-1.122758167058868772e-01,-4.832459314702780562e-02,4.460614936924989227e-02,1.128194231002765113e-01,-1.367417162838800715e-03,-4.181693720441351236e-02,1.111593810869369869e-01,-7.228353688714275749e-02,1.803564828127658670e-01,-3.276277962335226446e-02,1.287008487121617424e-03,-1.929323775829240584e-02,3.838266212100803487e-02,2.454941210889837605e-02,5.087813106462139162e-02,-1.256764826679993008e-01,6.084344877637060806e-02,-8.255719861272979923e-03,8.446586023512118319e-03,-5.394739097091388880e-02,4.610392944740977772e-03,-5.944078172386950409e-02,-7.824971864370437735e-02,1.099506427945692449e-01,7.718678754626069616e-03,8.597161004769506010e-02,8.237172749406820524e-03,1.898451210029043768e-02,7.015275302542370306e-02,3.094948010146950784e-02,2.787615090550338293e-03,-2.083032365908179390e-02,1.528680494820418290e-03,5.572961872890286328e-02,-2.271832375556784300e-02,-2.355325675804713706e-02,-1.168966711392588342e-02,6.579983146373126479e-04,-6.074224876405580342e-02,6.306492377766045876e-02,1.722938542177304688e-02,6.342123835146684652e-02,-8.405037119140736090e-02,3.047153428193314501e-02,-2.433572873019905491e-02,5.611879321666998766e-02,7.487085952269015299e-02,1.092195173664533281e-01,2.743321376572246256e-02,7.431193334749308654e-02,-1.503792487554723306e-02,3.107047397395653818e-02,-2.410998722932595345e-02,8.763351413521013045e-02,6.829081239631165645e-02,1.369174849396831081e-02,9.118449495782190994e-02,-6.057189149934342348e-02,3.407998366766065712e-02,3.977708492590664724e-02,-3.232594611693531683e-02,-6.788599360971547814e-02,-4.602034637269861173e-02,-4.519396821606978809e-04,2.706493670745866234e-02,-4.182605275327475752e-02,-1.045231101927350525e-02,6.924563142401393701e-02,-1.125479024724097971e-01,-8.085119658577702428e-02,-1.421732677061311734e-02,1.549814399044939506e-02,-1.022578935181461951e-01,1.162545322396813310e-02,1.490692209325217982e-02,4.895306022718345917e-02,1.235770703715860647e-01,-5.007371310303407319e-02,-1.122327512497619667e-01,8.890422192377922528e-02,-5.845067023073862583e-02,-2.041514041089047549e-01,3.668604262650428105e-02,-2.053271754119603484e-02,1.076426749103047247e-02,-4.076096517760904869e-02,-6.704332657602821155e-02,7.323017562775459288e-02,3.714232027518638013e-02,7.182286056256390050e-02,5.111686343325137771e-02,-8.364717850239937469e-02,-4.001557352995603611e-02,-7.943240463021335185e-02,-1.482275434256706548e-01,-5.426077431927496530e-02,1.156892542862179851e-02,7.570668930674512298e-02,-5.649093858977900090e-02,-4.649483108654371005e-02,7.388632859663918362e-02,-1.841091161632091022e-01,7.516640738653664056e-03,-4.207978149131635925e-02,3.325949170564652946e-02,2.671912239572942632e-02,2.013827782285615431e-02,1.818897690152958269e-02,-6.446502081736672773e-02,7.611836946312608432e-02,-7.433939532697650399e-02,1.965991187315805494e-02,2.892365658360605829e-02,5.941596312649818218e-02,-1.132833062923021585e-03,5.906281471249447829e-02,-7.225856017762331829e-02,1.900823709335016168e-02,1.548373924713646044e-02,-1.713719372704491581e-02,-1.722216323862033049e-03,-4.464478707293805665e-02,1.056122469063012909e-01,-6.725640474089306098e-02,4.967213419679858410e-02,-9.286592392917547567e-02,-5.831979577572767026e-02,1.215002042176226843e-02,-5.726000071725932672e-03,4.712080602029725118e-02,9.454038716026078384e-03,1.358908967740101448e-01,-9.691593528618183684e-02,-1.002932500012230083e-01,6.335724489722874275e-02,-2.638735984832897910e-02,4.183004561553954614e-02,3.309430432478128320e-02,1.708113905392728837e-02,5.467118714432489535e-02,3.236235272283308206e-02,-7.347998874242429868e-02,-3.277658231916401321e-02,-2.298039022879003190e-02,5.651683977026786265e-02,-6.168936296590288998e-02,4.286459521300006736e-02,5.599835374252618014e-02,1.477352517655574349e-01,-6.564046290351201185e-02,6.353227136238696893e-02,-6.200956829029331818e-02,9.035255490152484248e-02,1.456672407392628141e-02,-1.042934028786781930e-01,3.667657393696120011e-02,2.697738700056468396e-03,8.769862568502932026e-03,-1.447933825146608885e-02,2.970130483644455727e-03,-8.416569720072254734e-02,5.728980638390222813e-02,7.898615923304166953e-03,-8.497521898551592401e-02,-4.623259336156753524e-02,-2.285881172446843634e-02,-7.260606508432314310e-02,1.813023343172939436e-02,5.949255150048761431e-02,-1.999530486138388630e-02,-2.505378561496526824e-02,9.685307546766382808e-03,8.263858059313668181e-02,1.926728701664522005e-03,3.874164821983046292e-02,4.860012110410076580e-02,-4.041951563663442881e-02,2.136158438865973230e-02,1.061206067319245272e-01,7.555520663364628450e-03,-4.823589567783563570e-02,2.108618760520446223e-02,-5.971891127529972698e-02,1.067814145045189039e-01,-7.934843286080123237e-02 --7.137787257300325094e-02,-6.009815689098177427e-02,-4.905013503237175276e-02,-1.368588584693031240e-02,-4.980370075799670643e-02,3.004604102113803826e-02,-1.017846038150725940e-01,-2.091895337158548862e-02,2.780958551405865453e-02,-2.183594088337812919e-02,-7.009278921461825629e-02,-1.013074594038487625e-01,2.454875876721464290e-02,-3.488633587327021057e-02,7.576775724279594115e-02,8.311690402137371370e-02,-1.483999509816432902e-02,-1.176701346269803591e-02,-7.583109159619799700e-02,-3.595864448303803113e-02,-9.359248426100903739e-02,1.313355202499519869e-01,1.853084141122081710e-02,-5.373755945174257809e-02,-1.252074822941754567e-02,-1.646683839868468696e-02,1.567334058240596462e-02,4.733896159350153843e-02,-1.306863947355052513e-01,5.249843876605118920e-02,-1.695863376618861329e-02,-2.412294112301494042e-02,-8.032313952231220910e-02,-4.854510070798516430e-02,5.538050339223121732e-02,7.129811814103939605e-02,-1.271565135153005510e-01,1.043171939611646920e-01,-1.179543250159644993e-02,1.997765936602504885e-02,-5.638042268210635699e-02,-6.719639975561124412e-02,-2.478245812266496872e-02,4.139764753632328015e-02,-7.743777294951961765e-02,4.418409909847202666e-03,-3.104098104027602431e-02,-7.799945944191805092e-02,-6.843751357088659337e-02,-7.303161170523642109e-02,3.554660684185530911e-02,2.266844049522935556e-02,-3.530646997202178949e-02,-5.725421463974720432e-02,5.621964693402226737e-02,-4.073365958014915633e-02,6.309338362037142378e-02,6.550904697475669336e-02,1.547836026294439199e-02,-1.535545148940191743e-02,-6.619423292026994921e-02,4.994347127610637010e-03,1.600851133690158259e-03,-2.770081256783876039e-03,8.742574067750855105e-02,-8.746464037893485199e-02,1.167301514677078489e-01,-1.482331660020922608e-03,4.588351711598495641e-02,-1.072284510530454593e-01,2.347803208891000373e-02,4.898870948156764521e-02,-3.601608776479375829e-02,2.328745831826739901e-02,8.251453173990019874e-02,-5.452341440052394655e-02,7.136369517725241196e-02,-7.491400703427625665e-04,1.296893284602300411e-02,-9.218345218640770777e-02,-1.259763525684934049e-01,-2.286554069055432917e-02,-4.488150913628141669e-03,3.599549920681172077e-02,-1.404730262534335805e-02,-5.429806948448285403e-02,5.335810096117643564e-02,5.133733908601855606e-02,-2.477163312117084543e-02,-5.236634690005410007e-03,-4.576630004071943758e-02,3.902633591015694514e-02,-8.219210509605720527e-02,-6.952561828273516598e-02,6.272858151298746354e-02,-2.778390786720974504e-02,-5.326827579460184647e-02,-6.243511201050871984e-02,-1.077551632402039272e-02,3.834458803032877831e-04,3.047175291411267917e-02,-1.402893664471431372e-02,1.950092530477527675e-01,1.304282946083571565e-02,-6.715345083243000335e-02,-1.061199727975603718e-01,-4.444928551735985792e-03,-5.721531482388434592e-02,1.110760702783978454e-02,-5.090997740409775041e-03,-4.473334272207288298e-02,-5.086689476653768333e-02,4.118439235627180395e-02,-2.726300268545254341e-02,-4.915719479028418659e-02,-7.042800631046948479e-02,2.906146977176918972e-02,-1.456950563992882516e-02,-1.813432016063832447e-02,-8.195663850177475895e-02,-4.919841722073719364e-02,1.227911045640963511e-01,3.424931040720370634e-02,9.627578309577497495e-02,-5.130966513980442928e-02,5.572509794337356104e-03,-2.449416951718662999e-02,2.737124446309027737e-02,9.576745201251499195e-03,8.977394975114103004e-02,1.111918143766561645e-01,4.405589403285441186e-02,-5.432582332765540506e-02,-1.030286654444879796e-01,-4.068621413354366056e-02,5.302130972376968565e-02,1.714812814608793201e-02,7.532946095017023502e-02,-7.212424502510748903e-02,-7.059084057257916656e-02,-2.239397600259727950e-03,7.133343147304047061e-02,5.412965290713385447e-02,1.358960153239516938e-02,9.737230946429745182e-02,4.324443616803121115e-02,1.575424282570362169e-02,-1.312493457624416826e-01,-2.809488340603056786e-02,-5.004863820936393537e-02,-2.867101504179670260e-02,-1.056658100608370010e-01,-7.462927208715813832e-02,1.241890130548053278e-02,-6.135694541123841439e-02,7.333374625497905697e-02,-2.338266707551116264e-02,7.103251777115574678e-03,1.521448495698344283e-01,1.243281103985333091e-03,7.847474994826854344e-02,1.556806349395821118e-02,2.990284423529396968e-02,1.832746786887340731e-02,-7.502442748314626375e-02,3.731230168328792535e-02,-4.428442368709668842e-02,1.510586101205189935e-02,-2.572279391777832977e-02,1.160780197532019359e-02,7.300320633533968762e-02,4.809926111678797245e-03,9.121870421611516511e-02,-3.307411845184821120e-02,-3.007197137243212970e-02,6.793971407034808674e-02,1.139055914907536049e-01,5.114860660739664611e-02,-4.034255969722434082e-02,-2.372926025107228998e-02,-1.448616495592243425e-02,1.217261392293251254e-01,1.621898948105896798e-01,2.292448342673776562e-02,-1.752376891818511681e-02,4.714878727954361325e-03,-2.274743755283105406e-02,-7.950142390155405525e-02,-3.220901752606672441e-02,-3.410648312366469648e-02,-1.047642329693429103e-01,8.168435860027301043e-03,-8.446245547497183148e-02,8.755558584108913411e-02,-1.675036563581616678e-04,6.400791310062833861e-02,-6.469621715613325941e-02,-2.025901254838653048e-02,4.210036370371748188e-02,-2.643877753005526715e-02,2.412070585381926105e-02,-2.902794920456268010e-02,-1.285809247055116067e-01,-2.991208829089747959e-02,-3.905424307099278347e-02,6.545579241201383847e-02,-1.492838754290978440e-02,-1.054088017521376819e-01,9.049848108300465044e-03,-1.587992254476524312e-01,2.491787632608494230e-02,1.118045453971171221e-03,1.111924322325734057e-02,2.007398197021131175e-02,6.342558732520944820e-02,-1.686949415970087907e-01,5.886317282293314357e-02,-1.647747585346518925e-02,-2.546237925352577008e-02,-4.818761224552896294e-02,-1.207543627058156738e-01,6.545394013314752291e-02,-2.261238841656214651e-02,-6.011095959295254670e-02,-7.466241143358011423e-03,2.559482331711391417e-02,8.757452525754244599e-02,-4.203291454725245624e-02,-5.761639078204206649e-02,-4.523389140045214196e-04,-6.599494645782248009e-04,-4.131629593165527664e-02,2.678957538362280316e-02,1.574741200330245783e-01,5.153589557532517462e-02,4.797035571275947569e-02,-1.987719889830012049e-02,8.205929720692840823e-02,4.562093989996917481e-03,8.027050122218300221e-02,6.282492101073411384e-02,3.526091454631367222e-02,-1.313335098952500557e-02,-4.810598051431572153e-02,1.272144901739359069e-01,-1.620274736624053788e-02,-1.132002057317783861e-01,7.196418676983662444e-02,-2.518384322131306252e-03,4.491234927924157849e-03,-1.283585452078549703e-02,1.468866124000961504e-01,-2.242854502178823900e-02,3.936935509975161146e-02,-1.190733501592221646e-02,4.877701163792311223e-02 -2.000988053283735482e-02,-1.608771617249776242e-01,-3.804637511179604758e-03,-2.536470691631478522e-02,9.355280097911447545e-02,-2.139357000981512852e-02,1.232758300050429412e-04,3.269408849613193724e-02,-2.020738246205533439e-02,-3.423665502073428707e-02,-2.972143910031115055e-02,4.131785612832661325e-02,-1.395473778153449584e-01,8.370096943441590609e-02,4.780675423266977919e-02,-5.708231340578406865e-02,-1.616570834457962957e-02,2.331378915543982216e-02,-9.935433031921703240e-02,9.135912991668007632e-02,-9.648274178885418717e-02,-6.877471829981361695e-02,4.147237691614981403e-02,-5.330533830466634843e-02,-2.169767402006354123e-02,8.530176306133685493e-02,3.770787612390358312e-02,-9.222371192412558150e-03,-6.642081398626989430e-02,-6.154863649873785342e-02,-5.887210485918138031e-02,-7.979702441346178676e-02,-2.104870563745623452e-02,1.963015978191212305e-02,-6.617936135673661846e-02,1.225451303580871454e-01,1.008975560570748858e-01,-1.187277418728298062e-01,-5.724895658919585545e-02,-4.219982652098665937e-02,-4.006739181277098327e-02,-3.668745273619201941e-02,-3.519334697806571211e-02,-4.650510815410585641e-02,4.786619940905800785e-02,-3.727771027228692080e-02,1.509621028138681542e-01,-5.824617790508455212e-02,3.808334056756497443e-02,8.546325461752851682e-02,1.664778034657365957e-05,3.655531423541970304e-02,3.518555617123780843e-02,-1.011939293054402722e-01,-2.660929502858672135e-02,-1.238547873833252544e-01,7.414635192206674164e-02,1.955666045213866333e-02,5.447295189268498705e-02,4.830961691972501804e-02,-3.072870858513414641e-02,-1.853541026292758812e-02,-1.526426229561688164e-01,2.102457333316046229e-02,-1.094318712554773754e-01,-1.349488858829156201e-01,8.206692636681386932e-02,4.193806622505578952e-02,-1.502310191496937508e-02,6.104542556213829463e-03,6.949207589553889342e-02,1.119853969212424444e-01,-1.277329743626957337e-01,1.867560604258476642e-02,4.472333466858560008e-02,4.732330984174602101e-02,3.929283647343013997e-02,2.704365258251999887e-02,-6.359465549781570526e-02,3.543759868699326507e-03,-1.250332433807016183e-02,-1.056586711305333021e-01,-4.262680326383088553e-02,5.643256511790682717e-02,-8.472613661079264613e-02,-1.126146819063036930e-02,-8.669234980547144143e-02,4.224290223404026517e-03,4.337623015265191367e-02,-3.548472539871970477e-02,4.044709622285987821e-02,-5.927916677193020845e-02,-6.709838599804472670e-02,4.756499563672429709e-02,1.022038997996086718e-01,-1.806983392055644400e-02,-9.998585963318273873e-02,1.078285163628992793e-01,2.278300583704196899e-02,1.214222387810892471e-02,-1.060792437207630085e-01,-8.558310470724314958e-03,9.492405753452887174e-02,-1.082068615882315898e-03,2.470472234631812536e-02,-2.773981617485308040e-02,-4.527301600210966587e-02,-5.410382689355795749e-02,2.889617371594910972e-02,3.618873264921745880e-02,-4.046882988834541589e-02,2.667465351428564835e-02,-3.764075996687632486e-02,-1.883231275065161414e-02,-6.869454968452773369e-03,8.941770773522631291e-03,2.427142197720253206e-02,7.042071208034533791e-02,-1.882248220593601279e-02,3.844439376269500469e-02,4.946789859948868412e-02,-9.617961059361707782e-03,1.083242689375809725e-01,1.176086767158918134e-01,7.426065571122165108e-03,-1.457545300802778540e-02,1.969492107161456562e-02,1.666846874083025704e-02,-1.179475542556910062e-01,-3.843358267595130273e-02,-1.289787341001338156e-02,-1.670980082957283785e-02,-4.840946077128294861e-02,5.257540395802715733e-02,-4.001672630998026614e-02,9.192824921561338139e-03,6.909289632322199171e-02,-1.244106484400939530e-02,1.409662127806561756e-02,1.966573545513928059e-02,1.875434824950800508e-02,1.663699282822486344e-02,-1.249302810545971504e-01,9.557825055644655898e-02,-4.636941627797433463e-02,1.317327923273461654e-01,-7.116332605489545174e-03,-2.796938350729221562e-02,4.789295980955207699e-02,-2.546507869106370836e-02,-1.775856537869571067e-02,-4.862646888182759093e-03,-1.633276981416717857e-02,9.040239048233811303e-02,-2.782378898175660864e-03,-6.508721129289218865e-02,1.183996262379335279e-02,6.397112966253920563e-04,-3.046663453151493561e-03,-3.387765192689694976e-02,6.190195432538124487e-02,-1.076990569521022040e-01,2.602916212897470050e-02,-1.121461022857352663e-01,-3.662901392640784820e-02,1.410364917054342249e-02,-9.210314092194629709e-02,5.161895417692043825e-02,2.138804773656230451e-02,2.212275435122909584e-03,-2.423190938137280773e-02,3.276851520364290615e-03,1.433145329236888144e-01,-6.391978706243237052e-03,-2.470186116780766564e-02,2.286178708877974197e-02,-6.934338747241662604e-03,1.106887619246994803e-02,-1.608275699738601419e-02,3.285583749293440742e-02,2.994210410971644490e-02,-3.508773098381601230e-03,-1.466120936497927046e-02,6.210933649656382588e-02,5.073324381732288724e-02,-4.322872649132052103e-02,-1.783589358861110696e-01,-8.601873556894412687e-02,6.661955075232386947e-02,4.496926423113090099e-02,1.135345619176686763e-03,1.266895939308051652e-02,8.386461762532915198e-02,-2.323563968860123397e-02,3.506383260771200883e-02,-7.630987690980199634e-02,7.148034754121539158e-02,-1.360818394335591708e-03,3.969338175871945512e-02,1.703863791105225331e-02,9.211363557652413325e-02,-4.835305674360253214e-02,1.189134736882757615e-01,1.196220285849067602e-02,-6.101231567121832844e-02,-3.154743109345589658e-02,5.372116220501456374e-02,1.199605956101982424e-01,-4.486753795171145170e-02,5.132371823319509252e-02,5.039206898382039834e-02,4.163570229532610745e-02,-9.610454199579097911e-02,-4.611144755640568527e-02,4.533472686317498862e-02,8.913316717662900235e-02,-1.012848810701578450e-01,-1.911692817137184905e-02,-4.471806468428212583e-02,8.376652349097096772e-02,-4.290174571386010105e-02,2.690717595629485426e-02,-6.471785269775962024e-02,7.808140058135580597e-02,1.046744050600619258e-01,5.074177714350792956e-02,-7.445171406190430330e-02,-3.854605510096786088e-03,1.672052242872749603e-02,5.692634078960167633e-03,-5.851336037974554094e-02,-6.042593077257524259e-02,3.464413047090277031e-02,1.021441676351534655e-01,7.520440026424302093e-02,5.587594253538764183e-02,1.264407105701970124e-01,-7.092685992846476523e-03,-4.311309764808402001e-02,-8.084019745026740922e-02,-6.385849323460951465e-02,6.626986820792310495e-03,-5.012340904138182923e-02,-3.944167012722103738e-02,-4.104500940830536904e-02,6.988567172106878445e-02,-7.826416036171492119e-02,1.961603470546637723e-02,-1.861101346769153744e-02,-1.702393834537621609e-02,2.415128449417113313e-02,-9.469902169211895371e-02,5.650097981095246469e-03,6.109058094518141774e-02,-1.174378338103329450e-01,-7.685773662772062143e-02 --3.882524897469938341e-02,1.581016113825487943e-02,-2.388827520053318082e-02,-5.896173044842167171e-02,6.825060968339399792e-02,-3.652745876818148940e-02,-4.485803942271467809e-02,1.016213821722378174e-02,5.583835965878917312e-02,-5.955150419163458830e-02,2.438782387506333027e-02,3.477281521396040354e-02,-1.020141703113988246e-01,-1.832796688171701993e-02,4.752400514148755195e-03,1.796623843452892222e-02,-2.740034469791838290e-02,-5.680979052215338193e-02,-1.442247180278143776e-02,-2.970110867364965726e-02,-4.294325781664522917e-02,1.285167550745324599e-02,2.043885085963416740e-02,-1.455116481967873299e-02,1.995738380729768571e-02,5.612016338123659159e-02,-1.253419486640965086e-01,-3.129248565357867218e-02,8.827534337896812688e-02,2.986329304215856045e-02,1.025077086541817811e-01,7.696350173343380341e-02,7.146380716926696575e-02,2.040055081324565478e-02,-1.017209032281376907e-01,1.834576998173163484e-02,5.808497726793891702e-03,-8.194433518188617982e-05,-9.803323573499672650e-02,5.695172014572163349e-02,5.820719235839008632e-02,-1.574834695699405332e-02,-4.016027098258043854e-02,-7.241545932429355181e-02,-8.586614498949271534e-02,9.959771058589808601e-02,-2.827953338498681579e-02,-9.067068941465993015e-03,-1.118487045785013234e-02,-8.403008679203756437e-02,-6.006363216846816178e-02,3.298507974214621996e-02,5.364862329114304662e-02,-9.740716140078022056e-03,2.864717889619604330e-02,-6.712147710788013422e-02,-1.018511219522743455e-01,-5.116308440347035191e-02,2.943056782057720810e-02,-4.010199899294946552e-02,5.689015705600986711e-02,5.142268456373164792e-02,-5.815239370300884225e-02,-1.958004181797785501e-02,-1.922346170462277404e-02,4.299488285076312477e-02,4.650978652067754016e-03,-5.489390617890367585e-02,1.760137363511120323e-02,-5.158916420395907171e-02,3.388854522444446210e-02,-2.144028033263141453e-03,-8.344376297856391511e-02,6.720154729801450233e-02,-6.779211750105419709e-02,6.367330468780377228e-02,5.010889047191009055e-02,-8.755284482449253447e-02,-9.565245553244462740e-02,-7.527675469739400493e-02,-3.834887694536229241e-02,-2.083517408906869137e-02,-2.017155788769142352e-02,6.079169779908109167e-02,1.801436909954593768e-01,1.014869050457733179e-01,-9.639497589796794386e-02,-7.738402720088656539e-02,7.352581299444920643e-03,-7.526203352883582831e-02,-7.848011510317333406e-02,4.218259209977891222e-02,2.568843803055580535e-02,-5.935039154439582243e-02,-3.690053831474310320e-02,5.883757099418290837e-02,-8.748670977163376661e-02,-4.316867757970527936e-02,7.240173523105891858e-03,2.063846578036101359e-02,3.192102236478456395e-02,-1.155868809560652993e-01,7.819932695334136374e-02,1.121261961223152664e-01,-4.354387931705797077e-02,-2.349108327526036946e-02,6.536105231448893771e-02,-4.027240025793555299e-03,-1.414678777331734028e-01,-1.371747370409571488e-02,1.055025386650811081e-02,-6.174223379249646482e-02,-7.252612880188501199e-02,9.715589951849115322e-03,1.347826428887156092e-01,-1.116781157608216640e-01,-1.213252639456687265e-01,4.661942749996846047e-02,2.462681434352863991e-02,-4.309349791636276489e-02,-1.204166575999841143e-01,8.158091641219636370e-02,6.869446675496054810e-02,-3.441867766391845662e-02,-3.345577978209866160e-03,-3.638426274191216420e-02,1.090634468305236998e-02,-1.472131692133854877e-02,1.244943389477629059e-01,3.009356620945921948e-02,-9.148914099758224949e-02,4.871384303856818310e-02,1.194547397289848212e-01,-3.357423317430316412e-02,-1.317292667698189909e-02,-4.822468789553422536e-03,1.325159138183911733e-01,-4.085169746951620995e-02,-5.265850669900544456e-02,6.432773863947634924e-03,-1.424626938304136003e-01,-5.136458462754033873e-03,-6.125615890382783008e-02,1.152035239448006426e-01,6.545557702789910060e-02,1.728568067573234279e-02,8.848440965214472709e-02,-1.501045101249255838e-03,6.578929381281510558e-02,4.200436227054058641e-02,-3.088600023229742808e-02,-2.421226925463031143e-03,5.000512345245728546e-02,-2.775012091334297382e-02,1.343548471147715251e-01,7.158209813322681525e-02,2.702102781883448240e-02,9.516351604284366272e-02,-3.256722930648534714e-02,-4.237966581533965382e-02,-1.759682115318554965e-02,-3.220605406814315563e-02,3.755691599767076866e-02,-1.395240359926341045e-01,9.782605413876843681e-02,3.928177629449908226e-02,6.494249207102946453e-02,3.189696928007550170e-02,-2.143982947608628342e-01,-1.664762600398286135e-02,-5.587458846966322085e-02,4.446926108892457208e-02,-4.835198993917045468e-02,3.484638943839628300e-02,-1.689143123153984821e-02,1.202678195689691892e-01,4.425189002196061594e-02,-1.394870674473958627e-01,1.952472102735066523e-02,5.181209194486208203e-02,3.250115047866077023e-02,1.051017592500237685e-04,-3.199160715323757630e-03,-2.837562216593898443e-02,8.326490173693261443e-02,-6.183215058241332829e-02,-3.707016464401835520e-02,-7.707293522312690104e-03,9.379034505243656616e-02,-6.553852579136948409e-02,-2.003590582396995445e-02,9.686809631557160086e-03,1.061100469746742783e-02,4.438813499943147778e-02,-9.135732101485714851e-02,7.932013027781364967e-02,-1.395370517481495654e-01,3.158245555442783237e-02,3.095284139470564147e-02,2.328350098532447343e-02,2.480470381550944903e-02,2.213155875429526770e-02,3.572240246381881701e-02,1.385399078160926001e-01,-1.083440149200196739e-02,7.538524984793282258e-02,-5.355051740948274519e-02,4.677000734391683484e-02,4.392064519923415122e-02,-3.212776314405636074e-02,-3.335878337636738467e-02,-9.835918264044909987e-03,9.142171832056671110e-03,-1.336109735339418431e-02,-7.902839512693039109e-03,-9.630019333135649354e-03,3.624628688107954522e-02,2.095738121237566975e-02,4.824039613903989554e-02,5.106126099863181333e-02,-8.119319880520341204e-02,3.384954453166839211e-02,6.159921474195251268e-02,3.484087371385712756e-02,8.662289973256478981e-02,-9.497894647969736676e-03,2.510940616450753338e-02,2.946593539173004983e-02,4.257788268844439616e-02,-2.351006097609572437e-03,3.272099564638029923e-02,2.086600841064538414e-02,-3.932465959025801053e-03,-5.409264497485595513e-02,4.456599124573637494e-02,-1.993280086220099862e-02,-1.100861928628155961e-01,-3.614126321739172254e-02,4.819017321953159017e-02,-6.945291704857843584e-02,-5.359349170370957677e-02,9.723601092729676487e-02,-2.864278473453352528e-02,9.055619151874226558e-02,1.723191440095739671e-02,-1.264791906077428474e-01,-2.198789538546298827e-02,-1.916652212771885813e-02,-2.469814252022157000e-02,7.487497052579616952e-02,-4.543505289045320084e-02,-8.919653612615963123e-02,-8.489571277450966291e-03,2.751651815986605812e-02,-9.364412568733042341e-03,-3.285128277529853619e-02 -9.386459756428375267e-02,-4.642575421397898000e-02,3.406424853284941573e-02,-3.733168053903161698e-02,1.267877721594979634e-01,8.928629869386195406e-03,1.038080233981714673e-01,-4.161603780647164796e-02,3.101776164031067040e-03,-2.421799647949269413e-02,4.573404615530358985e-03,-6.337758928655190527e-02,-1.851151180973190091e-02,8.713092180716558444e-02,6.873389919624320332e-02,-6.582054373520965229e-02,-8.455566825380186891e-02,3.861897061057308067e-02,-1.492075246667972505e-02,-8.502482945050501839e-02,4.763764744870011003e-02,3.031456506251949123e-02,-5.940542306384938992e-02,-9.130592420721257707e-02,-5.171102730433575767e-02,-5.657032986475203640e-02,-4.079385246746362187e-02,2.474582067951671543e-03,-2.566094561889124825e-02,-7.072996908894400858e-02,-1.371681238883833143e-01,-1.722468883413250454e-02,2.855097241283480539e-02,-3.864003624844583595e-03,-4.194589903376299417e-02,7.007201596517867448e-03,8.717454241722967512e-02,-2.051230297989075496e-02,-2.234859915280203863e-02,2.377317573639783216e-02,1.424749973491980237e-01,2.247733831355123654e-02,1.074055585579368755e-01,7.091685929982146329e-03,3.300853429446491683e-02,9.723602740002965095e-02,2.375803477028846575e-02,-1.186111832455276627e-01,1.450441683131834837e-02,4.777672837452734411e-02,1.570003035883564199e-02,4.432414314757311333e-03,-5.755479637039696839e-02,3.601835931951095915e-02,1.037892490964055364e-01,-1.007651506257187246e-01,-1.224559603654662605e-02,2.329211321003509319e-02,7.186356220226851332e-02,-1.355578679923380089e-02,6.371310755538323045e-02,4.550719609552530964e-02,-2.042431015637891784e-02,1.432692656709920118e-02,-1.412750073609021217e-01,1.271157518031406919e-01,1.657968815142527030e-02,-1.442724524500257205e-02,-7.936324627732437020e-02,-4.610752054586556015e-02,7.334848369429597392e-02,4.984729893382622984e-02,-3.176780203311760364e-04,-5.262687689025415116e-02,1.098021715131432541e-01,-2.045780813338114040e-02,1.630137489963765646e-02,1.228744763948017710e-02,-3.134112989751310085e-02,1.847488348549300094e-02,1.007598073654134563e-01,5.221778714942441890e-03,-7.848086988983424273e-02,1.927621936149368637e-02,6.185282558994869770e-03,-1.056281873221141115e-02,8.164379172442523536e-02,7.804776201005421410e-02,-1.036632695480771069e-03,-1.245741649680356944e-01,-9.641615067675959536e-03,-1.319448815441601475e-01,-6.697670973512367953e-02,3.852841821364993979e-02,-6.264557719247743073e-02,2.952460952249936349e-02,4.698180603702525188e-02,-6.104533706467007992e-02,-9.325593206829219106e-02,-7.894383060580148492e-02,8.553398286619927349e-02,-1.012705133547929168e-02,-2.037717405109457092e-02,-3.970462696356404086e-02,1.592274660253255314e-02,3.145850059057457571e-02,-2.354770897762526946e-02,4.160832557326197417e-02,-3.244622612173825305e-02,-2.462644777150327369e-02,-9.464269707507882989e-02,6.644742945350563812e-02,4.382262771514482819e-02,-1.664422359409435323e-02,1.186755879520452056e-02,-1.512888843832576810e-02,-9.283002781747572152e-02,4.771213008907776443e-02,1.367108141882275585e-02,2.184203270550343426e-02,-7.804347115472719254e-02,-1.943369986874032493e-02,3.956853384890757219e-02,5.047692761169085551e-02,-1.142743887613502235e-01,2.854733996760025483e-02,5.401649858827396733e-02,3.719903607062763351e-02,-2.324345864712653101e-02,-5.937852601759416576e-04,5.973656973692919453e-02,1.389340988956629386e-01,-3.254866151585451839e-02,-2.119663667486592143e-02,-7.472231993791011995e-02,-4.733805286497073939e-02,-1.284289913531493355e-01,-1.149131279450554474e-02,-9.501907323868866162e-03,3.688266544324381169e-02,6.563954685874058559e-02,4.123940651307004407e-02,3.949101610907342075e-03,2.885663799555944140e-03,5.443248073533974579e-02,-6.702619704164781977e-02,-4.729598743355219875e-02,2.084357280317835348e-01,7.256202089194560556e-02,-1.634069515844920051e-02,9.346172782297096360e-02,3.132347588359188362e-03,-3.723676208223289374e-02,7.913327477259729514e-03,-2.103887431732972407e-02,-1.266635265940011523e-01,5.107311404491084866e-02,-1.676487642004052467e-02,1.554326245476828940e-02,9.007871924605909131e-02,2.412537201730319270e-03,1.050787911914486672e-01,-3.640576726744528674e-02,-2.604445753655332263e-02,-3.084918069156212736e-02,4.431265533072636931e-02,2.084674809381661248e-02,-4.645308727251199615e-02,-1.347204054021028385e-01,-4.767711862683936264e-02,-5.341701864689863549e-02,4.040429524533536293e-02,-1.523878063778298479e-02,-6.518806076513704306e-02,-1.499273354734387970e-02,2.886004665344459388e-02,4.710984338691334011e-02,-8.675667174663113934e-02,-7.674973125209232083e-02,-2.140825143030201591e-02,-1.554973178510233867e-01,8.393573394398011478e-02,-2.270186695290939138e-02,3.702195662871475557e-02,9.758955554396595011e-02,6.187446308138512541e-02,3.020360270254619783e-02,4.152994985848173365e-02,9.728966795516519328e-03,-7.589250621509271988e-02,2.270920914168458871e-02,-1.500054849364532150e-02,-3.967618075416100981e-02,2.191978438741676999e-02,-4.479572230564914104e-02,6.675975252105730007e-02,2.403076853723389375e-02,3.200770511719977507e-02,-1.261569245471746703e-01,1.803156077173849459e-02,6.763856657257519278e-02,2.616054848047663639e-02,-7.968394731456018842e-02,-1.252377167125435023e-02,-1.891676360900316697e-03,-9.419561725358618562e-03,-2.076367020176633424e-02,-1.514849903459337677e-01,3.700973541276002948e-02,-4.268179627664071829e-02,4.389517263100659733e-02,-5.757188640271331675e-02,1.103774757620001878e-02,3.331218076560006625e-02,2.941635722698136834e-02,-1.385763392490657608e-02,-9.832077929807581107e-02,-3.218957098337083683e-02,6.802253450326306572e-02,1.262863486567526172e-01,1.382690228377862485e-01,1.433886892568684013e-01,3.280509710815365104e-02,-1.120448846376916746e-02,1.193977525029824571e-01,-1.354191383499827976e-02,7.008808039897926334e-02,1.679930673643855246e-02,-6.087444125862968847e-02,1.039240560235597799e-01,-6.565425481448988743e-02,3.218460324328772937e-02,6.498991666187125399e-03,1.515201260710473397e-02,5.513186194689193714e-02,-2.049138017312522597e-02,2.504514299925022364e-02,-3.098981459351827714e-02,7.337150637572201206e-02,5.516202304077077317e-02,8.073631991315227338e-03,-5.553999983862696610e-02,-4.835169091183476320e-02,-1.413311305184050681e-02,2.104741708673276562e-03,3.054319542849985580e-02,-2.271069781928325426e-02,-8.622871056408355772e-02,6.434589730243334138e-02,-9.767635499558130485e-02,-3.471243302096992112e-02,2.190320504383303987e-02,-3.954455347856369873e-02,-1.451813621509324276e-02,-1.601928584457477239e-02,1.530447796848570796e-01 --8.680912913379159462e-03,2.097513139784024441e-02,-2.152782825494662516e-02,3.121336871603849067e-02,-2.654058570472966547e-02,-8.939995833907131428e-02,3.918604622664263737e-02,9.528169730620761335e-02,2.642348329763061221e-02,-1.918683219255745698e-02,-3.242338280060746980e-02,-2.017985748813150293e-03,3.405967986673105741e-02,-6.684988396654245602e-02,9.927694550720574107e-02,-8.291396465821526673e-03,-5.486788163623392001e-02,-5.392240814823166806e-02,6.384887634609429097e-02,1.265358209846845217e-01,-6.439530265124637776e-02,-6.284938157715777574e-02,1.487662802751069796e-02,-1.088194952065169030e-01,-4.171566779332254449e-02,1.952592980965394201e-02,-1.971881612389023153e-02,-5.991462892287516273e-02,-1.389008456926885127e-02,-4.468676156214261563e-02,1.729802032835804637e-02,2.322563403227521570e-02,1.747329139224991848e-02,-1.562362642090191905e-02,6.445821045138674932e-02,-7.531665956355132474e-02,9.978673931992920743e-04,6.147745743203796714e-02,-1.390739395842272019e-02,-4.574195008578364752e-02,9.930718370409347129e-03,-1.805727922239671809e-02,2.614968242164477860e-02,-5.427773333699939529e-02,-3.489088933578449181e-02,-3.517344080675434415e-02,7.148796845623585361e-02,4.990586074767418279e-02,-3.943503816487663877e-02,3.163862760160152110e-02,-3.921978845941234693e-02,-7.627999078741956596e-02,-9.074361105933882732e-03,-1.670411684222616183e-02,1.401712250699578000e-01,-1.664058391752737398e-03,-3.486083342485225139e-02,-4.641586841107615824e-02,-2.227360735069666697e-02,1.047994936996958504e-01,1.457287868156900701e-01,-9.492846584502724616e-02,-1.049481058421979224e-01,6.311015052350586740e-02,-9.479951603239347713e-03,7.082323338442925165e-02,-3.859762748399986343e-02,-3.173355593024747290e-02,-4.580954855843968887e-02,-3.890696996435009936e-03,-6.534850786654582133e-02,-2.330585887943160614e-02,-5.690724521524613555e-02,6.702399891501632789e-02,-2.409330042747820366e-03,6.833499242287766062e-04,-6.951061902568650419e-02,-2.209111756867685600e-02,-1.053545660629308239e-01,2.073790579584157412e-02,-4.023465731075379082e-02,5.822553106080138874e-02,1.443337254691064164e-02,2.671846441957932347e-02,-6.368080651868043840e-02,-2.182317463848092859e-02,9.139924292659938609e-02,9.552637784933083742e-02,2.972030803484504088e-02,9.541114160207375683e-02,7.744099181110458696e-02,6.199838299192528962e-02,-6.522456449490564301e-02,1.046211717932242491e-01,1.140368360966754757e-01,1.543697020910857753e-01,5.529964690657862558e-03,3.934545348320279967e-03,-2.821857051880262424e-02,-6.852868956924514088e-02,6.173290308161263895e-02,-5.271663776968311954e-03,1.456211860402095992e-01,8.654356704813852275e-02,-4.425091406208143952e-02,1.399791839572748359e-02,-3.636754777849488668e-02,-5.886445117400574203e-03,6.966250901991434541e-02,-6.225124547192745217e-02,5.098178408228311309e-02,6.143820254087210830e-02,-1.055950580715541154e-01,4.157887920298318496e-02,-2.204389226845073282e-02,4.261771368136772764e-02,1.898937640102189850e-02,8.978545030074393252e-02,5.610504389615775245e-02,9.969620474014027900e-03,4.036314669552289527e-03,-4.199316397410260415e-02,-3.791926352975195230e-02,-3.020632653910819421e-02,7.453790642135366129e-02,-3.879408906937189400e-02,-2.920674465967807928e-02,5.417951067874685056e-02,3.165554998844255508e-02,1.635472454066050907e-01,2.661640292336751173e-02,-4.658946893567576103e-02,-5.771569122691835863e-02,3.863760225054819336e-02,-4.832666902398198572e-02,6.364885862494500113e-02,-4.391975934123694414e-02,-4.730297637788425519e-02,-6.631578998996373597e-02,2.040412706848557245e-02,2.548086245461526900e-02,-3.588326796033155380e-02,4.028353843339196882e-02,7.674428475377532977e-03,-9.848775117717530059e-02,-2.083445715235176382e-02,-3.786866590104638608e-02,-9.355840327216422003e-02,-2.535028636441544192e-02,6.166805957547728811e-02,-2.799247101517017902e-02,1.045009542920821016e-01,1.653017644777697928e-02,4.764303749388808934e-02,5.756500129146206735e-02,-6.008503657715399612e-03,9.473800362878032066e-02,4.978067747269478149e-02,-2.037535676454131292e-02,3.989740276553283005e-02,-9.075138999088058767e-02,7.575271541965276911e-02,-2.832762027284055698e-02,-3.042306643391763654e-02,9.561198922356912988e-03,-7.069392225031885524e-02,-7.361454301466377470e-02,-1.513608633120407665e-01,-6.838614161058945151e-02,-4.017303151133967687e-02,4.771664614391150222e-02,-1.024069643480033237e-01,3.588947068620652187e-02,4.324687068514827815e-02,-6.601502685291776862e-03,4.895723089992972538e-02,5.249679528245852073e-02,2.365969968198794460e-02,1.291297104921777447e-02,4.510833222059484199e-02,-7.294692938892080591e-02,-2.009708086485868100e-02,3.257737963521599445e-02,-4.426564955210638558e-02,1.452069380247693504e-02,4.815425143105848266e-03,-1.425746458566818919e-01,-1.569434909841136913e-02,2.637496165841128615e-02,5.587022366385199779e-02,-2.968869710336428241e-02,-1.305792704592970838e-02,-2.939782029099848384e-02,8.733414786181714629e-02,6.588081560909454792e-02,5.640307104341574229e-03,-5.495008578468589533e-02,1.253105854371366901e-01,6.144308187404168980e-02,7.730710039671087608e-02,-6.788274107225045206e-03,-4.451333067549688438e-02,-2.100687892248126465e-03,-1.326242611608209332e-01,-2.877010489254147943e-02,5.202136389981033143e-03,-2.710806468924779888e-02,-2.444032538479176675e-03,-3.956031709686554793e-03,1.313465167713861459e-01,8.418675370366460187e-02,-9.638192624298863587e-02,-2.083787880964910535e-03,1.208303312493954618e-01,2.539356258260508370e-02,-4.476510413512550801e-02,4.587349663838047320e-02,-1.955163654334902273e-02,2.599351058934881373e-02,-4.610513548820913826e-02,6.359518943197309260e-02,1.354369577627184407e-01,7.594429112359898926e-03,-1.503666983425422377e-01,-1.504190585090191615e-02,3.836477071320464061e-02,-1.502362812636944267e-01,6.171339693788176900e-02,-1.257107481515264691e-01,-1.006451399442851195e-03,1.640710780762549836e-01,-9.945809642958443331e-02,4.494148563994137396e-02,-1.568168038975454592e-01,-2.533625953973640127e-02,-8.476711818441380608e-02,-2.834073684028352164e-02,-1.810924991771391615e-02,-4.392945617546403780e-02,2.251248659155452927e-02,-4.664147930523505903e-02,2.068397102252398159e-02,2.027748165898012045e-02,2.291045487994410046e-02,5.383924478243738015e-03,-9.985980754811504823e-02,-9.836102442256142164e-02,1.166476707939726257e-02,-1.465594796870654110e-02,-6.687854902000800461e-02,4.601888523666040898e-02,-1.366019042238315190e-02,8.552501707650816964e-02,-1.217728159655014422e-02,2.322979075783879882e-02,5.423975034502564324e-02 -6.425741544823337925e-03,3.196506635600615542e-02,7.948929571181911902e-02,2.589781186176682432e-02,8.184573565586844568e-02,7.791563098844683788e-03,1.277181041291751418e-02,-7.508140872403856747e-02,1.046482897091949305e-02,-3.523926279054068189e-02,8.013761304720724843e-02,-7.263175373501351562e-02,6.817322572993306240e-03,2.843946972158395425e-03,-4.950325484739158399e-02,3.364603238942287522e-02,-2.642039866759081715e-02,1.306836134704339458e-02,-1.142133212730764985e-02,2.928148780888398944e-02,-9.509620219471379143e-02,3.823713782458338512e-02,3.461343046747388658e-02,-2.352600890876131723e-02,-4.621157707854006702e-02,-2.982833192643673942e-02,6.261780905533642638e-02,2.173862469983128820e-02,-9.946837556011986492e-03,5.712668096759386527e-02,-3.808170224024169659e-02,7.341008884411286128e-02,-4.336055893829494912e-02,-1.005167106380334269e-01,2.659300823411946471e-02,6.782752583771090027e-02,-7.139241686419396660e-02,-3.613433529232053243e-02,-1.254241560450854442e-01,-9.913276019879660450e-02,-1.998034378591228080e-02,2.502821845492251424e-02,-6.023207159530693383e-03,-6.015112558178756230e-03,3.769942515739510558e-02,5.417176642890686455e-02,3.457800681638335816e-02,8.242569876673451479e-02,8.216136012880861927e-02,-2.064150587862803032e-01,8.157835219901296564e-02,-1.339953051163661132e-02,1.676836605303425390e-02,4.477419222516233754e-02,5.141355660413274992e-02,4.457519871427388647e-03,-3.677625730806845239e-02,5.661756412136272065e-02,-1.086424885211853958e-01,-5.067430141873663363e-03,-1.439628785738445715e-01,-8.805313949070484036e-04,2.795131401068906563e-03,-8.668259583348195130e-02,-1.072892116673420659e-01,1.110635237945220288e-01,-1.552338328962913835e-02,4.376433997074743315e-02,-2.580853435666976073e-02,-4.963038302287260672e-02,6.798581258453378906e-02,6.086054814452045852e-02,-8.612531505337585325e-02,6.245911464380779488e-02,-3.781810842783370208e-02,-3.592448981339316227e-02,-2.605494685025196849e-02,7.026174299965681769e-02,2.289377082552036813e-02,3.087219340444744692e-02,-6.065129641574359444e-03,-1.819478021642462542e-02,4.169028719282306256e-02,-1.857782847068729926e-02,1.260953113986515717e-01,-2.043674090061887647e-02,1.755732705067162072e-03,-5.805146060935221022e-03,2.676485930018184314e-02,4.559610310227010799e-02,1.944857131849226164e-02,-7.100207767353504396e-02,2.609289725394178111e-02,-1.114744212431059978e-01,4.132757926554235517e-02,-2.683803917397890870e-02,3.057251331672055511e-02,4.909618863753963114e-02,-2.396645330423919501e-02,-4.416344869373638182e-02,-1.769448864448346787e-02,-1.211097561970749942e-01,9.875846110275705858e-02,1.236891762853898485e-03,-7.058905229780832249e-03,-2.665387497664895022e-02,-5.635647199144586811e-03,6.411246392465058674e-02,-1.195022562203692032e-02,1.120466315543139010e-03,5.996072650921278197e-02,2.464120830715198218e-03,-2.784796636404796716e-02,-4.137190567413917552e-02,2.308696688982514456e-02,-4.841166976003201394e-02,2.806336994257689621e-02,4.328233386871067900e-02,5.360588743747669166e-02,6.885080030366785842e-02,1.016162150624725855e-02,1.182390360996968259e-01,5.926281026951023961e-02,-1.273386121407456550e-01,3.861898604748362218e-02,-2.756710013424482769e-02,4.073386988271914672e-02,1.516736741678213485e-01,1.479210753902902159e-02,2.714710910848081027e-02,-1.269180797619805234e-02,6.571296059626714758e-02,-3.363400725345814385e-02,-3.376857031091012179e-02,-6.683205347541212177e-04,-5.318756921386836450e-02,-1.128909411762861192e-01,2.998473408451453326e-02,3.968703036934946582e-02,-4.065360217710223614e-03,-2.759803196318457927e-02,2.097965864396190289e-03,1.747956348056038667e-02,-6.447897129435287145e-02,-4.062726365749286944e-02,-6.381114583174225807e-02,-1.855708672731231357e-01,-5.920898813515886039e-02,8.418745084756118491e-02,4.845090075530394252e-02,5.408235352123878792e-02,-6.143599283641171249e-02,1.113498164275817243e-01,1.399551555507879408e-01,-7.327791457241253359e-02,7.562556695479673297e-02,-4.105760064692775407e-03,-5.897085637295900601e-02,-6.123158190745890683e-02,-1.988422716497987019e-01,-4.049105522461177165e-02,1.622038359148492148e-02,1.391554113268781215e-02,-9.985185526013408419e-02,1.702851962015172194e-01,1.165790496991880587e-01,-6.131442049342777229e-02,-9.291093856873750001e-02,3.994836491778366860e-02,-7.791422987850971094e-02,3.090207089901392257e-02,-2.369701978247343993e-02,-3.272178806786109340e-02,-2.695378127783805075e-02,5.923372623362481437e-02,-1.170866584792270848e-01,-1.411382140575552155e-02,2.805064828834772858e-02,3.469228591640387965e-02,-2.789821949683528313e-02,-5.662724081551725498e-03,-1.120864879836595968e-01,-2.666119194177955093e-02,4.041033995106263488e-02,-4.087216836342658088e-02,-7.390385876847621605e-02,1.057564158577576074e-01,-2.382447472757060272e-03,-3.557006118535752104e-04,2.241876836566614184e-02,-3.602651362420837555e-02,-5.495156104635107175e-02,4.603101452246349118e-02,5.258680600280056516e-04,5.003312890510291239e-02,7.290994019502837487e-02,5.397198612854172545e-04,-6.314460589250350808e-02,-1.500382808393149747e-02,4.906664553621043617e-02,4.496524506647914254e-02,-6.118901068131446519e-02,4.563795163972280550e-02,-3.472096342994020202e-02,5.679615723994685395e-02,-7.412336533428087737e-02,3.881604405718752648e-02,-2.092636408377333834e-02,-5.726744701034854057e-03,-3.955656680450672646e-02,1.553883599207364630e-02,-6.411455048830241865e-03,-9.911117435903567252e-03,-1.257746759454284602e-01,-4.708541689338850006e-02,-8.911617983159484530e-03,-3.109509533600583048e-02,1.181088275787409332e-02,2.915762949358817469e-02,-9.760825045084348117e-02,7.546918732733100266e-02,4.540034112595212751e-02,-8.752198229145875807e-02,5.130673713070704745e-02,3.423636411948944441e-03,4.892608981903835003e-02,-5.120833497916888605e-02,1.901400703616487609e-02,-1.204244418396816230e-01,7.389498732651104962e-02,-2.897972257797907375e-02,-4.204797536904798100e-02,-6.879811587558484764e-02,6.278615067023052276e-02,-1.071428562386617378e-01,-2.402645532132028500e-02,1.026992527061100224e-01,5.665867813636941974e-02,7.450125420589176484e-03,-3.766966515943213911e-02,-2.621962695669409135e-02,6.052835054131498749e-02,5.154702493080246667e-02,-4.339493392282900430e-02,-5.921460162655585446e-02,-1.752936777510008537e-02,-1.194323285505436427e-01,-7.426851499338134321e-02,-8.700703133830704761e-02,6.053085449281550567e-02,-7.059652974692810778e-02,-2.396101087344143510e-02,-1.045811241739050546e-01,-2.813059215828671994e-02,1.336903844533303687e-01,8.578449770636830535e-03 --4.838793462423109787e-02,-8.224921971102040763e-02,-9.116809987766422152e-02,-3.708050794129029437e-02,8.591709924623401606e-02,-9.779253561840289821e-02,1.311563794320507181e-01,-1.284481608188963592e-02,-4.934047276559781064e-02,3.466758570369348619e-02,1.717295502100158258e-02,-2.313240623056670581e-02,1.284880018740118882e-02,-1.837237430460633703e-02,-2.887326809476532113e-02,1.039938288768302646e-02,-6.854892232124137896e-02,7.835165382192241235e-02,-3.208891751091867695e-03,1.665757576126079831e-02,5.538603832398105581e-02,3.599964467662011142e-02,-1.972944955554054025e-02,3.555479845871335753e-02,5.491123480187978106e-02,9.269219398444970837e-02,-1.640244724570328466e-02,-1.522877977851741678e-02,-6.108474610179470687e-02,-5.084174361899540528e-02,5.211941429064100711e-02,-4.064136174871901125e-02,-4.770321700230252537e-02,-6.674256398729772055e-02,-2.031701834347062613e-02,5.861745556545305824e-02,1.298448779027249617e-02,1.542679328771862923e-01,-1.557123331872181660e-02,-9.228057142265322366e-02,3.878859745009357457e-02,-1.524609684184437958e-02,3.288628221986329009e-02,2.466431239484427607e-02,7.619337137205231536e-02,-3.230059565646473119e-02,-4.394797564279713520e-02,8.070227241926722361e-02,8.850212601991508388e-03,-3.159107171673191522e-02,1.296147672406340601e-01,-1.060164408427088090e-01,-1.173941130575590491e-01,-6.887322233350008649e-02,-1.021005691713107733e-01,3.097906158991839409e-02,-7.015958733456878656e-03,4.351317723435568774e-02,7.243770082226269613e-02,7.988081313710522491e-02,4.453169160571687768e-02,-2.749396866076088408e-02,7.614029387454393505e-02,-1.334971846627147803e-02,1.025573922621484529e-02,-5.811458164316549851e-02,1.239090251392067910e-02,3.519778998611035326e-02,2.076143181585458387e-03,-1.546240512589889572e-02,-2.130311233292895082e-02,6.718146504775254768e-02,-2.151932883077097361e-02,1.159043359447854971e-01,-6.823462618650466549e-02,5.736472521795061424e-02,3.384320608379860068e-02,6.117869712192206683e-02,1.157900726550842654e-02,1.722880157716659422e-02,8.314236708525207797e-02,7.176518276537731389e-02,-3.007113397610914826e-02,-6.895599360360826768e-03,-3.152788633018101250e-02,2.880466457246924281e-02,9.428696049720394767e-02,2.500067335219720299e-02,3.860659504085226651e-02,-9.291311777043503861e-02,-7.090431695700770767e-03,-2.483329756962774845e-02,-7.121283988924675956e-02,1.004710546140202138e-01,2.186476975601660787e-03,1.950880068501993983e-02,-1.062340749964652414e-01,2.475687605548196335e-02,9.457330752531697970e-02,-2.300856179203218665e-02,-1.011971447038839123e-01,1.398722940783675775e-02,1.281694549300461949e-02,3.201281096390896919e-02,-4.424943723425028147e-02,-7.929845369740583239e-02,-3.209123385513164284e-02,-2.568606439228716953e-02,2.798464423321108882e-02,3.302241802363367618e-03,-8.601796477410304786e-02,-6.350356518456624910e-02,-4.942692746787256258e-02,1.185249368003865253e-02,1.278181973896692636e-01,-6.341282540290772360e-03,-8.664912776692368368e-02,1.258749401836559839e-02,6.302483423525061246e-02,-3.167828664436583841e-02,7.103057301753955277e-02,8.367024362939104454e-02,2.075711514755152615e-03,-8.792954840729909349e-02,-2.097567358074876182e-02,-1.957625388289449195e-02,1.029496876857512178e-01,-5.671995886268957343e-02,8.414359596318418677e-02,3.323025943988442560e-02,7.466568384319470508e-02,-4.349078878087666959e-03,-3.711773196777913564e-02,-8.901784849160211677e-02,-2.776047372511973241e-02,-7.440914037778535717e-02,2.652566305576188610e-02,1.483652935317029270e-02,2.797634314090048646e-02,5.224739729161418517e-02,4.811846364724359920e-02,6.660074057191193997e-02,1.323313451695350140e-02,3.002365721025319553e-02,5.154572612092848255e-02,-1.015970773814001832e-01,-8.485917630547198942e-02,3.832093506087082269e-02,-1.389893658356237266e-02,-1.106029651712189810e-01,1.714938269260700421e-02,9.650812799123324737e-02,-9.593723984017607509e-02,9.861748798099166657e-03,4.911208537240326077e-02,1.717457990418994607e-01,3.189817444792833467e-02,-5.126500619373854789e-02,4.392772747710628478e-02,8.777910000296922088e-02,2.614193288674096505e-02,4.876706497740643936e-02,-3.511094975173422589e-02,7.588267191234383813e-02,-8.031108378526963243e-02,9.658122275062960838e-02,7.894077878455437944e-03,4.130904570678242682e-02,-4.796824676001547327e-02,4.502018778049281245e-03,1.017215676568527724e-02,6.971905298260404338e-02,-1.249927096463558304e-02,8.828953681790073504e-02,1.685846439063341459e-02,-3.313585447959900912e-03,1.141845175279357014e-02,-4.041099526002154413e-02,-8.930177721473678276e-02,8.312078796935770675e-02,-3.494786455918960644e-02,-9.754211454580390128e-02,1.551837423781000577e-02,-7.667931398336030879e-02,-9.745707410096648982e-02,7.730540314070209751e-02,3.106233403973593724e-02,-3.760099670144921580e-02,-5.015470501806563314e-02,5.175401530383530457e-02,4.996948828261552067e-02,-4.479835852258589809e-04,-4.998915253257898983e-02,-9.837572438333183622e-02,-4.394328713514703000e-02,9.484048470388403707e-02,-7.329383532227788656e-02,5.348821194114242172e-02,7.513586919959978905e-02,1.545676868155541894e-02,-6.665720275096753689e-02,8.599652008941463532e-02,6.187476815915223666e-02,1.249795411923883548e-01,1.710353496683247598e-01,-7.469425232046196517e-02,3.815638566780719942e-02,1.478863169866060923e-01,5.193821063609059541e-02,7.978192900189913506e-02,1.144545249749237964e-01,-5.952445010981239487e-02,-1.691000247162724544e-03,-2.929297226824019440e-02,-1.433192755171107391e-02,5.469610782438418667e-02,-1.071295976042789533e-01,-1.608255611079636646e-02,5.911674506087213898e-02,-1.297210497677583896e-02,-6.297383526398514508e-02,-6.661211834163432988e-02,1.754022896714288113e-02,-2.291302690293626695e-02,-1.678615920085869267e-02,4.692195983188188202e-02,-1.626375846555048327e-02,9.587306914563978322e-02,-2.779134699865368208e-02,-1.812369658578151521e-03,2.225743061216126778e-02,3.885020699996187543e-02,1.702282884190619780e-02,7.522925421480981811e-02,-1.416875710580861591e-02,-1.149688956848137628e-02,1.385823080172368382e-01,7.650001186107575901e-02,2.333719265666545778e-02,-5.581201417012901511e-02,-2.852462832239292811e-02,-7.424368231624071945e-02,-3.129239219683870032e-02,3.968443130951407327e-02,-1.615251960292841266e-02,-7.384075094847389331e-02,4.802006729062111012e-02,4.150789214641714686e-02,5.083063660754658097e-02,1.314496291946475048e-01,-1.303323995555330690e-02,1.179389167621644890e-01,1.346810380719451472e-01,5.527752972312215476e-03,7.098854886624354765e-02,5.473741654369337212e-03 --1.192139001383924271e-02,7.272891549392516801e-03,-2.608416292711403933e-02,-7.616958937057698165e-02,4.639915416362187101e-02,-1.260366213705579053e-01,-1.088436059527731065e-01,-6.080590767803703556e-02,7.600562596876289656e-02,3.549243090049197802e-02,-1.068595021054443983e-01,-1.721096077954813869e-02,-9.757361081149334980e-02,-2.787796110748835821e-02,-3.158749894656653900e-02,-9.783803636886598265e-02,4.063888627991964209e-02,1.337594582969521897e-02,8.056656915388775431e-02,8.170456305146117648e-02,1.029707747357637010e-01,1.516610693758103645e-01,1.617052884238827024e-02,1.694284793127547040e-02,-1.273426658731614813e-02,9.237393062079171657e-02,-4.711333988710472709e-02,-3.701476304473395873e-02,-5.598441364001896514e-02,3.000258255093363885e-02,1.829691787399360253e-02,7.242227318593821850e-02,4.304990743380634338e-02,9.766476827315656584e-03,8.179413536357285630e-02,-1.154838420165060775e-02,4.203726307554375707e-02,1.793528875345781412e-02,-3.418625273771729034e-02,2.032838153599344708e-02,-9.299030314830364352e-02,-1.255587102710510328e-01,1.350057010555559198e-02,1.603161726544597571e-03,-6.597462626023183063e-03,-1.449653910016893837e-03,7.815653185538148917e-02,6.715640163936227591e-02,4.297417661666227962e-02,1.463968547096470285e-01,-1.661740417892446087e-02,-3.183725387647162036e-02,-5.050007291330906034e-02,-2.455827982820397645e-02,-1.028658589577198940e-01,6.182511086750652751e-02,-5.698097558920562494e-02,7.181357736241497647e-02,1.231706004965337636e-01,-2.751374153472539372e-02,-6.152525196911787231e-02,6.030873301118841456e-02,-2.594838345861906134e-02,5.956335774298427560e-03,-1.022806797952955926e-01,4.651778647010385015e-02,-7.301620608590840555e-02,1.835351000278921674e-02,4.914542712163235544e-03,-7.789464670475816643e-02,3.592616446266566621e-02,-3.083229583243134175e-02,-9.008152077874204133e-02,-1.396382293555276377e-01,-2.733531692445918815e-02,-1.976204089865206751e-02,6.642335511962592365e-02,-1.136539300235511613e-01,-1.392190674630858216e-02,-1.382221043535398347e-02,-1.072076109670035970e-01,5.720645609810021820e-02,5.656874289588124938e-02,1.129988004256827008e-02,-1.069052084849152345e-01,-1.709092313115740530e-02,4.323408281600726877e-02,-1.688116654235366221e-01,-1.074299632738921484e-01,4.140222127574713640e-02,1.144387686475515642e-01,-4.980177958201909005e-04,8.428199179144485464e-02,-9.578242527502263057e-03,3.868705055734203424e-02,5.509140538898113504e-02,2.912012003067910443e-02,6.683466026940115823e-02,8.994507342691289084e-03,-9.821040855338888931e-02,-4.858075070501679388e-02,-4.065482924927928121e-02,7.121078169175298900e-02,9.635262456383233975e-02,3.586962379664450362e-02,1.795162244905582072e-02,3.441068692897081976e-02,8.236822031298568492e-02,-1.902940600206842227e-03,2.251495038440522178e-02,1.429648863499210359e-02,-8.039936852303626591e-02,-3.062745910464369226e-02,7.844161012937962696e-02,-3.304790311729609992e-02,8.097028581809226355e-02,8.044844697453294724e-02,-1.133242597819252018e-02,-8.993099367547162382e-02,-7.413739584627222767e-02,-1.616866178503219981e-02,6.157352254676329295e-02,3.536645623199276278e-02,5.522758332770308365e-02,-5.457331014957719079e-02,1.231857129454128064e-01,9.645554651346464892e-02,-2.921987869784143169e-02,-7.487696044784852845e-02,-3.643217920535007925e-02,-4.441062464353125183e-02,-3.078935572260412673e-02,4.229695696578684994e-02,-2.403307776094575771e-02,1.069965717790207632e-02,-3.506846348709122957e-02,-1.059865727808414759e-01,2.859345302348622123e-02,4.716807889647307850e-02,-4.383844878870310491e-02,-7.570256203580152943e-02,7.947255130076803786e-03,-1.408977710553874739e-02,-6.922709856978612186e-02,3.049779152001185628e-02,4.042502628463619241e-02,-1.039698262434386039e-01,-4.769235917102623151e-03,5.141156831249807380e-02,2.577968583083101173e-02,2.718883729355659265e-02,-7.890806077995404788e-02,8.616370185370561541e-02,-3.493611146361100261e-02,-7.293939349351526158e-03,-1.894165034809555670e-02,2.128491482522022749e-02,-7.270850668545930651e-02,4.039505142556111539e-03,9.574556323774335853e-02,7.858427044662538719e-02,1.284831366693893195e-01,3.044815415271761097e-02,-7.741454161534962186e-02,1.069437756841302295e-01,-4.196906530706800281e-02,-7.214558698136519510e-02,-2.759829108006212756e-02,7.126956945484816397e-02,4.959793494207834913e-03,-1.142917042473456241e-02,-8.154408733769786394e-03,-9.567879257298499140e-02,2.350708402749263071e-02,4.249305995232897926e-02,-2.016179751665528319e-02,-2.837080715622540550e-02,4.249322339373980822e-02,3.748065008707294338e-02,2.012772028125588836e-02,3.972302806332626141e-02,3.897668673531497208e-02,6.212109106834610289e-02,6.051105417567680805e-02,-4.799266429141937212e-02,1.646571862039329093e-01,-7.826061641079434805e-03,5.717424589250265365e-03,-1.539999040356228588e-01,8.288267810015577386e-03,4.622653478573596209e-02,6.864444804607457151e-02,-1.921989401831722746e-02,6.863457704649944535e-02,-1.324268199972336024e-01,3.183874524170664244e-02,-4.288666196482102816e-02,-4.100264435372572014e-02,2.135403931298596528e-02,1.589949260310285217e-01,2.664490524885536338e-02,5.188266357655874073e-02,4.151349474869885092e-02,-1.982662757933627631e-02,-2.105825614638402732e-02,9.528861317186016616e-03,-8.326622588980195361e-02,-1.706961165094994168e-02,5.999472686036620706e-02,5.735795905186621968e-02,8.015038219159563437e-02,-1.896512711384591016e-02,4.204247160333999395e-02,-2.218762994268822647e-02,1.037042048795519533e-02,2.109115179317392480e-02,-5.399707863678122888e-02,-1.372904898415015958e-02,-3.704905663804577282e-02,6.843194017285665620e-02,8.345214502067982779e-03,-7.773105163618027058e-02,-4.027925818751755538e-02,9.216487988506968931e-02,-3.221143569254421996e-02,-3.066530137143725493e-02,2.805765211377527282e-02,-2.309912549465569995e-02,-2.306739112804991401e-02,-7.579178726281332601e-02,-1.112289007731479332e-02,2.243070683620396535e-02,5.772560511070943395e-02,-1.256708898154530740e-02,-2.866358582018004725e-02,8.173141477234146385e-04,-7.277883492466183379e-02,7.200932181970494395e-03,6.073488813808182452e-02,-2.686731445340764107e-02,-6.955764282903389994e-02,-7.724501023760993457e-02,-8.897973914684426111e-02,8.728837126059606211e-02,1.300142107801118552e-01,-7.856565593235599609e-02,-1.776541930072520620e-02,-2.842464920236011452e-02,-8.255303673622557681e-02,-9.960885936974890609e-02,-1.677046921136036289e-02,2.423257785847430026e-02,-4.974339732457588870e-03,-7.708248689836279255e-04,-3.120619783127033184e-02,5.338312740625002217e-02 -4.718595301784608148e-02,-2.591571265609485947e-02,2.485160300098064254e-02,4.360933744526986392e-04,-5.121742941544621397e-02,-5.475947913373454407e-02,-1.106799261550036884e-01,3.754881228028431051e-02,-5.085136300514560802e-03,1.138670226167447924e-01,-3.950845164081209088e-02,-5.652466183423209001e-02,6.295172971997507749e-02,-6.619415037695028370e-02,6.958184944417851459e-02,-1.219788555520303491e-02,1.436893275084992760e-01,2.539216396899219436e-02,-2.204080813681317530e-01,-1.371376787249785865e-02,1.872106791341280468e-02,-2.143424609418154159e-02,-4.528559998781384044e-02,5.116391252520691157e-02,-1.286014707651104705e-01,8.946347846099804435e-02,-2.397820413060401321e-02,-3.563558118244351900e-04,1.643969643787214843e-02,7.366120998071032377e-02,4.242140809025608111e-02,3.620191519713870931e-02,-1.038717375208845078e-01,-6.497279613423678768e-02,1.216934367158975169e-02,-5.879156320108956668e-02,1.097056618348287993e-01,-8.946470956000183300e-03,-1.934745739515851895e-02,-8.665415508857169172e-03,-4.812062558411917684e-02,6.183269692676545554e-02,1.640577267775901807e-02,-8.419319879033927007e-03,3.261089329938537951e-02,-7.081189559934888966e-02,1.434171088623617182e-02,-6.594834573289770630e-02,2.758423260691905687e-02,-3.899167761149976985e-02,3.601991552322231227e-02,1.165305453175710104e-01,-9.002966895272926373e-03,5.811314052577976064e-02,-5.810893204917989829e-03,-3.740487280414887816e-02,-3.530766107826442679e-02,-3.796006711109367604e-02,1.431959629677677370e-02,1.648009004666048036e-02,-3.411707688038571884e-02,-6.434607228788952193e-02,8.434775249013529153e-02,-3.205751441515629352e-02,-8.346257239685869744e-02,7.309026478895869205e-02,-6.651187325220754565e-02,-3.588877935729293395e-02,-4.480800503118788358e-02,4.280376539533793656e-02,-6.114633213870530881e-02,-5.073090292833434822e-02,1.755924965372273466e-02,9.167422986879009139e-02,3.648136329812138451e-02,-3.846198256538595900e-02,-1.606685937583440110e-02,3.642648708370917199e-02,-3.462539739520886090e-04,1.702663689006782832e-01,1.140223719862556474e-02,5.302891656019463607e-02,-3.936323808231207561e-02,-7.685899849976318005e-03,-1.108430338577759411e-01,8.482006448612020022e-02,-3.300783726324401507e-02,7.358450236314911796e-02,-3.605320258440713327e-02,1.488371442913282404e-02,-1.032140446915711871e-02,1.963951807685524603e-02,-1.072887061309366480e-01,-1.951098719463631448e-02,1.783971572616926959e-02,1.051329631674898235e-01,5.438290376657031744e-02,-2.958443863340180432e-03,-4.532936508435032180e-02,2.590743017791888314e-02,-6.665545962503757749e-02,3.236226098971515391e-02,5.854471471508702896e-02,6.717431897337597380e-02,2.420367697940507476e-02,-3.626171902989480667e-02,-9.701385991880051574e-02,3.065773117257477448e-02,-1.459914438427105041e-02,2.066171556336813750e-02,-2.278038034420833829e-03,2.063556820921262289e-02,3.839316197048032314e-02,-6.277384738607989789e-02,5.620406838208300698e-02,8.643514170242375463e-03,4.636152814333646677e-02,-1.035202028387176842e-01,-1.966949195840294110e-02,1.327757858571563254e-01,-1.664831895535148765e-02,-4.599312203601135796e-02,1.768522049947008137e-02,6.494524719460274820e-02,5.759002921318212564e-02,1.379678967261778119e-04,6.935185950136137090e-02,-1.440721985356028523e-01,-1.552235733754679312e-02,-1.367729706049438942e-01,4.726833163393695872e-03,1.282849106732899744e-02,-3.984859748413496078e-02,-6.223914296299845432e-03,-2.342581367666110581e-02,2.999464037272409414e-03,1.065026256278002875e-01,-4.856380953816501894e-02,5.566580366176908032e-03,3.212350282220255947e-03,-7.246602357435776715e-03,-1.003509982621026975e-01,1.459290072327983911e-01,2.133877646009116683e-02,-3.841236310548541699e-02,-2.028996082541664053e-02,-6.567554724809440692e-02,3.539961084020089993e-02,-3.235508099831498063e-02,-4.488133767860424256e-02,4.117398890732669459e-02,-7.522311382646848510e-02,1.665461147095629105e-02,-2.327478286168741667e-02,1.638104233105248594e-02,8.214379724928215498e-02,-1.700032487094413347e-02,1.367262442919397192e-01,5.632085476295900506e-02,5.709078339005832503e-02,7.573789326973696945e-02,-4.807691243358534067e-02,1.007230718366669033e-02,-1.659246275337410703e-01,1.346409134047819589e-01,1.905305774079498263e-02,1.461230500069375660e-01,-6.892572499753482629e-02,-1.115969585080376636e-01,6.324476871015918822e-02,-9.515731674915942073e-04,1.438351900613844651e-02,4.500061906753903551e-02,-5.481711623186363846e-02,1.518564932936901646e-01,-3.999726340184078799e-02,-5.025391156993964936e-02,3.927883298141462108e-03,-1.357110440346931748e-02,-9.582712131581693632e-03,3.721534618071859618e-03,1.044339572040902248e-01,-4.773085834828291046e-02,-7.268604684426602236e-02,-1.912849164708235739e-01,3.294395149172598541e-02,-3.387797156773315888e-02,2.670686624628496389e-02,1.353752543269118314e-02,9.211253783423854410e-03,-8.115660330214109763e-02,1.716510972032260496e-02,1.909903850929149313e-02,2.806395357145743605e-02,-3.784336006593067014e-02,1.603907387243790755e-02,-6.046006511269512440e-02,-1.859580984175813329e-02,-9.679895053963865487e-03,-9.683589397918903816e-03,9.445809756383785616e-02,-1.101822262366440874e-01,-1.288876229920958398e-02,6.287762381248314381e-02,1.557519302672306233e-02,2.919399586913227693e-02,-1.417192981004086923e-02,8.517622671119934319e-02,2.216704441453724167e-02,-1.066702108770465618e-01,-3.538100003949440353e-02,-6.139372298046172349e-02,-4.118331966436118707e-02,-7.568779302234016071e-02,9.741119016135809175e-03,-6.223155208955221535e-02,2.883928686780767858e-02,-2.371739655211654516e-02,7.005162807176469608e-02,9.069262642766995342e-02,8.075028542010909138e-02,4.669412121883474709e-02,-1.240473852830889512e-02,-5.277440447357058945e-02,-7.678956870515851740e-02,-3.767338873335614180e-03,-5.615224136533759902e-02,5.455537232578767937e-02,-4.413889683595469821e-02,-1.288193838060702756e-02,6.299901080543743315e-02,6.375016739958849765e-02,-4.837882618983463390e-02,-5.377701932363056908e-03,-2.591660275793492701e-02,1.257004513848338778e-01,-2.942772085420091756e-03,-1.283782152577606361e-01,1.914064764830863616e-02,-5.133021378219218556e-02,-9.217487508026723023e-03,-6.381569691378974341e-02,-2.175670006069226059e-02,-3.435762438713959449e-02,9.862519507584448231e-02,8.442273085667589827e-02,-5.394425020262964485e-03,1.241329016647833572e-01,5.750664305891513284e-02,3.429800908315624314e-03,-6.858109137360275664e-02,-1.010960554647897386e-02,1.086484164295148033e-02,-2.274841546569460540e-02,1.534325970023434939e-02,-1.863551786956666942e-02 --4.880698411486125016e-02,3.350911108069211386e-02,-3.393129211222661173e-02,7.735895629799727571e-03,3.236579973814956296e-02,5.254120990491101262e-02,2.801701423403060720e-02,-6.577819247633531763e-02,2.551512929258226148e-02,-7.761091383282717193e-02,-7.668554224939450048e-02,7.623787974460144445e-03,8.176181847475472164e-02,1.195335612375990481e-01,-2.434674824104166024e-02,7.443722374569172739e-02,-4.968812737270576069e-02,1.780434895272882054e-02,-2.306731158936373843e-03,-3.094597008877923716e-03,3.887811357496200049e-02,-6.246221904938850655e-02,5.160962640386480144e-02,3.270960036453522679e-02,6.210144779633280238e-02,7.135994752577826117e-02,-8.035229569738873423e-02,-5.484995944523542166e-02,-1.402562964265461620e-01,-1.033548738265429104e-02,-4.722930139610705125e-03,3.845322571986322518e-02,-1.337911136814253811e-02,-1.218096022337994133e-01,-6.312852026810744643e-02,3.030665019646073752e-02,7.959576359796072254e-02,4.684776647666342303e-03,-7.466788947435493151e-03,1.602773320524506287e-02,4.349340950282668677e-02,7.507634379878693615e-03,-3.626988705555994619e-02,1.110474796683099882e-02,1.037900117080184093e-01,1.776901044441185629e-02,3.890361406306640235e-02,-3.989033705086796056e-02,3.744272638609308368e-02,2.085547607435715711e-01,1.390375409372188523e-02,3.295355693050487644e-02,-4.651075127461995994e-03,2.604942940269262819e-02,2.909335683036157194e-02,-1.602746819810594858e-01,-3.310450072335294897e-02,-6.162280539536070523e-02,-2.229095106621086300e-02,5.976038294825458674e-02,-3.766427370913801032e-02,-1.225338064203016636e-02,7.629100495005686211e-02,3.490921186225869055e-03,-1.115992954426572176e-03,-1.364720099121028984e-01,-4.670994093244111578e-02,3.445063196567706154e-02,7.620484324522749930e-02,-1.847712816632630234e-02,-5.776898313835622523e-02,-3.844720662390476579e-02,-1.558034378362895267e-01,1.634644995818056445e-01,4.931895117232110159e-02,-3.528984652599501115e-02,-1.338086556422665371e-02,5.069981798833868292e-02,-4.385040247065173646e-02,-8.585208141340003318e-02,-8.927959818561247152e-02,-9.743357164838548079e-02,-4.295651101001278505e-02,-5.258880873761715563e-02,-6.224751220635246773e-03,-3.172778940800881353e-02,7.091430973194789988e-02,3.906235974116661208e-02,-1.542723291467057965e-02,7.078061702205727668e-03,3.058689528810685718e-02,-9.276079995986048765e-02,9.098365077315936911e-02,1.172267312532769418e-01,-3.825694202301141433e-02,4.205355298365136968e-02,4.552886249395128593e-03,-4.623356996241343581e-02,-1.793550709943315946e-02,8.006053685081730015e-03,-7.737738256024986883e-03,-1.723513376622490267e-02,-3.978814194260639481e-03,-1.053215041895421977e-01,-1.491928271081428004e-01,-5.082453353815292313e-02,6.498724761305206543e-02,-4.609551307820951294e-02,-4.256254752292925464e-02,2.636142827815740608e-02,-1.961791250909168671e-02,-8.306274741725649946e-02,8.019904287662368847e-02,-9.105461796459306950e-03,-6.234732694868282479e-03,7.475706795806313920e-02,1.488010128759404554e-02,-3.580061974870322322e-02,4.482892251539599693e-02,1.583341955036666748e-01,5.958976653718157240e-03,5.965510180593854711e-02,-8.836847670280842393e-02,9.913739926422887594e-02,1.007290634865034146e-01,3.013195277132665884e-02,-6.595840267444645633e-02,-7.807031014762697008e-02,3.414650572806336021e-02,-3.217258091069311043e-02,-3.114715720189884657e-02,-9.369419856467911534e-02,-9.855414128116270905e-03,6.952048624362666684e-02,9.617980159412778396e-02,1.850565176301572148e-02,-9.084936851401459457e-02,2.232015620628302957e-02,1.017166507635736405e-01,7.835820402270400609e-02,-1.049187922052620126e-01,-5.118729282408326225e-02,4.443432104156721379e-02,2.615102308002350759e-02,-6.732210105117800159e-02,-6.379170681460097359e-02,-2.012400257540620033e-02,1.573322362254403126e-02,5.483206103425129763e-02,1.028798660282883998e-01,-7.879416424135474162e-02,-1.271334756229124119e-02,-1.019525445209261832e-01,-7.963612033038564020e-02,-4.756672874401104284e-02,9.622801595216683157e-02,9.767684248309522685e-03,1.621374771748122084e-02,5.954073065637978390e-02,-5.694327496711872744e-02,-6.582520451571560738e-02,-6.951108200678937643e-02,-6.161499008380463827e-03,-8.983511710983018567e-03,4.445415829383875250e-03,-1.231957113101491558e-02,-5.160864860612320026e-02,-4.029445365417029523e-03,5.363933546002169761e-02,7.149013114958990833e-02,-7.991977744113358539e-02,-4.233688175297697920e-02,1.928088549691251222e-02,1.741501822079534467e-02,3.216435227083815074e-02,8.976863588995701027e-03,1.130888265080220723e-01,-3.047604514036122633e-02,4.700259024146656583e-02,-5.548679038491907689e-02,4.157453436267465052e-03,1.373627750370613465e-01,-5.768452128367375364e-02,2.009992437038739255e-02,1.515464857969378056e-02,-2.277462088237092175e-04,6.873219698776535103e-02,5.762795778704213334e-03,1.379789923549244388e-02,-1.058707431230006824e-01,-1.510614860491039761e-02,-2.604690120102108250e-02,7.472264482769558347e-02,6.799950702592638674e-02,9.946841730774289014e-02,1.199981760542839920e-01,-2.520863449646666005e-02,-3.326841692317371313e-02,9.222924707855389981e-02,1.732821960007854822e-02,-4.546637587607500058e-02,1.225094209816863719e-02,5.334751102809624779e-02,-1.669402037389192128e-02,1.517042494572462819e-01,-3.353490343568788640e-02,-1.322937980214321518e-01,-1.266378385219720792e-01,2.944084824131389111e-02,-8.287292028567533678e-03,-2.696291658163689486e-02,2.455523873408292646e-02,8.342789194393435137e-03,6.003841426674145360e-02,9.527083227722873970e-03,8.229022391851307378e-03,-1.525920423325698855e-02,-1.891707449310182226e-03,-4.229575325134765640e-02,6.349947907381489620e-02,-2.640342142419194876e-02,-5.300941943180811217e-02,2.178126140511110059e-02,1.133696664556707590e-02,1.712583515067201204e-02,5.419788246584007763e-02,3.170947519841892104e-02,-5.487966560736912136e-02,-6.151008046264366813e-02,-7.850784117022208131e-02,-4.681206832925904893e-02,-1.704261261556271403e-02,-9.321335197023643965e-03,-1.306406265637287401e-01,-8.378238279388006138e-02,-3.216658358045297594e-02,3.354089492261853184e-02,-9.958101078156185448e-02,9.756997484786474051e-02,1.716107452648941295e-02,-3.090988948247324480e-02,4.713628191142649193e-02,5.128790396956674214e-02,-9.954617823807331012e-02,-1.580899722224723275e-02,-8.529002516049510874e-02,7.070064890295549298e-02,-5.689691781628689049e-04,-7.553454763882429257e-02,7.187239152266619233e-03,-1.022176313692277350e-01,3.385243013696038247e-03,-2.869670039064858846e-02,2.166597357597236165e-02,2.117159286971312740e-02,1.684516116086411802e-02 --1.566420208786626885e-03,-1.704040136839696587e-02,4.473681114638140138e-02,-8.324925154562035079e-02,-4.903077949444784728e-02,3.525906563483127698e-02,2.373604275519849982e-02,8.236775179371305655e-02,-9.976251879496972341e-02,-1.050211438339449521e-01,-4.428174782445435526e-02,-1.155136490758662537e-02,-3.884935846052460506e-02,-1.385567134285626001e-01,1.403966494613894528e-03,-5.809065979513658440e-02,-3.771839660856091819e-02,-1.074189145326395894e-01,4.428328554352575080e-02,-2.285396391625945517e-02,8.458677213232504433e-02,-5.023622716343250499e-02,-4.434289848521197341e-02,-4.302591035979963507e-02,2.952223375374552275e-02,-6.206712758350846348e-02,5.368400580875638123e-02,-2.443501214835411967e-02,-1.735280571508768591e-02,9.921766493802755549e-02,7.245558064287299649e-02,9.453739209299474866e-02,6.955371770679970311e-02,-3.315765439637727297e-03,-4.733438171683477733e-03,1.496608755653379697e-01,8.441254040606666065e-03,-8.534009462021618919e-02,-1.169498835918690756e-03,5.732473635940825256e-03,-1.689076459139201292e-01,-1.783393526090627096e-03,2.382980245285092405e-02,2.547082831540280895e-02,-1.096804504550484005e-01,1.013891498808558271e-02,-3.355909683393167031e-02,-7.870298997915498029e-03,-3.115057620873574670e-02,-6.534920338715843879e-02,-4.446861667229098924e-02,1.151247840537511363e-01,-4.973000737013232031e-02,5.420759785990961199e-02,6.004039599732832050e-03,5.376856369692774307e-02,5.213449507936630811e-02,3.437272609776611393e-02,-7.830543715552833184e-02,5.420804859011342514e-03,-7.231653795387840700e-02,-7.673233753298897876e-03,8.080325498757652547e-03,1.029300285060902320e-02,-3.113223610614997833e-02,-5.585331258459390325e-02,-7.297514732074900437e-03,-1.242953640035949353e-02,2.062816432420029947e-02,-4.568015222035375772e-02,3.024070613745189623e-02,1.041307077696092126e-01,-5.038140166550071630e-02,-1.224903798023844054e-01,-4.284225429769980553e-02,1.118914613730868923e-02,-1.204638829321813404e-01,5.538844511794113051e-02,-7.502137444292866775e-02,1.850080058665309196e-03,7.326039223880766171e-02,-1.854011989136606850e-02,1.163635070175261521e-01,-1.221627821710608991e-01,-1.071265201586530935e-01,9.140979199022235269e-02,-1.757851629218339790e-02,-9.394290521755326029e-02,1.608104738668394290e-02,-9.140948093699044485e-02,1.079884525278279883e-01,2.048003715967917221e-02,-6.726691515505986430e-02,-3.314679661797177712e-02,-4.596978961637770977e-03,-1.021547228757034749e-02,-1.469507249845592237e-02,-4.765109900912604945e-02,1.718329137075944729e-02,-3.201627285139906770e-02,5.319707686814229008e-02,3.816074956876830404e-02,2.424880780777254746e-02,-1.340445702898948321e-01,2.882655958958831727e-02,-3.824789024944368065e-03,-4.788574554762060898e-02,1.266306142692573064e-02,-3.466888064582976381e-02,4.849093869988305161e-02,-2.858733199423062199e-02,3.159875862700747828e-02,-6.276498072734984501e-02,1.030200719347140192e-01,-8.288820596820126174e-02,-8.128490994720295737e-02,-5.393465162673564961e-02,-5.842626160483039427e-02,-6.030833615770245237e-02,6.264609760265618665e-02,1.383232396040324011e-02,-7.721145929500136673e-02,2.938662303136231266e-02,6.824976232453004721e-02,1.246073184591155597e-01,-3.966736266665178007e-02,-2.987546328273895865e-02,-1.107416489715945390e-01,-3.868819725624986217e-02,7.252670705638548931e-02,-2.798175442549391545e-02,4.387817208683314485e-02,-9.140916392837584309e-03,-3.809886067699963519e-02,-7.178914131220189010e-03,-7.678881493526314161e-02,-6.376689659122940224e-02,1.661317372157733799e-02,-3.201777589685741998e-02,-4.935135101858673101e-02,-2.085200617314159760e-02,-5.219459865203186083e-02,-9.311675490000538569e-02,-4.391907838098567385e-02,-3.273039570932126385e-02,-5.398235772319630310e-02,3.744076153999893741e-02,-4.085638444752396486e-02,-2.682593805052057093e-02,7.501203119876412306e-02,-8.183033592532318323e-02,6.256331580856278052e-02,-9.914925936766241787e-02,6.683196459966041225e-02,9.410791410665103607e-02,1.116561131833082032e-01,-2.239277542194665141e-02,1.425117838649600510e-02,4.162017239160614723e-02,-3.718914593997050405e-02,1.148352642357420086e-02,3.338984637731436805e-02,-1.399797506755662974e-03,3.044179870053362325e-02,-1.034594434003669322e-01,2.820408937040658551e-02,-3.788566930433205464e-03,-3.081995357380820991e-02,-6.370285790408396209e-02,1.620624039765536373e-01,4.208535649190837946e-02,-2.070557323671518707e-02,4.002121261572418043e-02,-5.121928155810805822e-02,4.766869489806131732e-02,2.903953137837533838e-02,-2.002233567513440546e-02,-9.901039589110864625e-02,-9.652409149359036933e-02,-9.197084377659238419e-02,-5.179496939036312697e-03,5.880056066802096665e-03,-9.014663506711555196e-02,2.235335000845514730e-02,9.352240503758244006e-02,1.582447536610059879e-01,-6.632601981008090508e-02,-1.039196573059260886e-01,-1.139020449270598534e-01,-6.775244937673512158e-02,4.699453156737793541e-02,-8.172474289976543529e-03,8.500721447876753711e-02,-1.650717958803283800e-02,-2.198721987253954291e-02,7.560137575620830275e-03,2.363255381371396485e-02,2.141209749351593097e-02,-1.951001558915208337e-03,6.356162929652877519e-03,9.374088738805545426e-03,3.852596196779339843e-02,2.704273912222790455e-02,6.863079488322942900e-02,-6.519139489344966121e-02,-4.432465762026442657e-02,5.841268489581821419e-03,-1.918535346636009858e-02,6.228082404637882985e-02,3.497630694672985985e-02,3.007167812486890093e-02,4.484149944804161070e-02,-4.615976282963087790e-02,-9.776040458040555214e-02,-5.997267227452711749e-03,-1.259685921484950677e-01,2.302212987738999628e-02,-1.057694421493198256e-01,6.798462865743153116e-02,-2.807433357138744423e-02,-7.780032814641513195e-02,-5.750475244655940676e-02,5.407082964473002018e-02,-4.852413406530420997e-02,6.373749330116858236e-02,-1.523965640970535371e-02,-1.736646755457445235e-02,2.905917464114836368e-02,-2.021656373731959416e-02,1.585588158905330836e-02,4.905824452199535751e-02,-6.178657765643962713e-02,-1.417948461961616446e-01,-6.768135173746976652e-02,-9.648688793767845606e-02,4.369463707244649470e-02,2.702569928286559231e-02,-2.685408086627476959e-02,9.369218218966120226e-02,-2.256052028692993178e-02,3.611520869848441895e-02,-4.361653946013152594e-02,-4.387926483011170276e-02,2.284727592282441780e-02,-6.005354397458567695e-02,4.056655230130318951e-02,-2.044775480643434493e-02,-1.049189488062861608e-01,2.566854221970849177e-03,-6.927952318324485936e-02,-9.295493381189964821e-02,3.791557371690568168e-03,3.163911660050165719e-02,2.091996905410235114e-03,1.714198281491676745e-01,4.289347232874617111e-02 --3.801395890055792037e-02,6.433684891384574070e-03,-5.602538444871234952e-02,2.906728877462652627e-02,8.121608296527886225e-03,-4.316682541085412050e-02,-5.963649100404547904e-02,8.545165622058528698e-02,7.533307571001121583e-02,-8.847817453101901319e-02,4.563625592916828455e-02,2.924329609167281019e-02,6.425521627375757605e-02,2.880058091188201358e-02,-4.881599902507956223e-02,1.188247995995753148e-02,-5.674793510629803917e-02,-2.518245530586813927e-02,-1.110775509933395883e-01,-1.615464799401094415e-02,-7.179227119496359166e-02,3.267193153696438396e-02,6.782414562302664651e-02,4.538734189459832369e-02,6.153236649521581086e-02,-2.463180421690497682e-02,-1.504192174878495741e-02,-2.392489118865503972e-02,4.383383002485483765e-02,-1.489275102364281955e-01,-1.100294694827867736e-02,-4.643440696856949990e-02,6.453989344120579508e-02,-6.201808215549834552e-02,5.057823924096391255e-02,-7.733118086495354912e-03,-7.403000834309551859e-03,4.777456700916924565e-02,-5.580297264057136608e-02,-7.061623380451915143e-03,3.059818238393912510e-02,2.656866872850063149e-02,-1.550135650056433390e-03,-3.113656030865456781e-02,3.670251095949804421e-02,-3.098004881418058021e-02,3.997107579297271701e-02,9.004123379194300947e-02,-8.520625200518032460e-02,1.638868966586054521e-02,-6.367334003478467008e-02,-8.274990584826790591e-03,4.863363174911132514e-02,1.244144012396698057e-02,8.220969567940292211e-02,2.686500645408870791e-02,2.842330320064348492e-02,-4.825920494029066476e-02,4.806337551125415553e-02,-1.205389373485018234e-01,3.810901954054036339e-02,1.235688952712314138e-02,9.297742536006625702e-03,-4.750925463510567021e-02,8.648194714267290473e-02,7.040974437066929714e-02,-5.796072157979843820e-02,6.696281724036605085e-03,-3.097081471835494773e-02,-2.659895748010475749e-02,5.525300288390569414e-02,1.660562585912369465e-02,-1.634780763163902473e-03,-8.060947461794476521e-03,1.019212960302124694e-02,6.132002088209531632e-02,-1.056375387837759326e-02,-6.839461838222198053e-02,2.305021518448121393e-02,2.529977865852530219e-02,7.990463605142900105e-02,-5.140901999469031647e-03,-1.032366770675064299e-01,-2.164865784477372490e-02,1.152824516781940506e-01,5.427955162373765408e-02,7.231326275692430416e-02,-5.679475258818847405e-02,2.527533877343901211e-02,-3.696049571378823284e-02,8.060840371201272170e-02,-2.424967113544854169e-03,-7.675831857204103159e-02,9.281094716152375398e-03,-2.107538485756814406e-02,-3.601902063460016984e-02,1.009551873117844195e-02,1.309192324241478245e-01,8.889771394052142783e-02,1.347856631960692719e-01,8.945560306995187227e-02,3.878799494594109554e-02,-8.236688112673887230e-02,7.102792235876699556e-02,3.107950395745115446e-03,-1.479884969618950330e-01,1.480509200384623469e-02,5.740784678604494984e-02,1.076774995707795030e-01,3.366455257442578508e-03,-4.767481643037900507e-02,-9.726508774464921042e-03,-4.333397500350850279e-02,-2.560447815270446709e-02,-5.927964295877074553e-04,6.471195428365172919e-02,7.598165473447594798e-02,-3.734608678175573016e-02,1.198449475998242947e-02,-1.177392550780712865e-01,-1.551004716386738744e-02,3.424794456286110039e-02,3.213431920806365111e-02,-2.568679687985158180e-02,2.670105965173635854e-02,2.905374866596511021e-02,2.623144688039789202e-02,-8.764423137129183361e-03,-1.364287732558825961e-01,-9.507863564749838292e-02,-6.247511415383016908e-02,-4.661778371757068634e-02,-1.262886497606783842e-01,-3.858065313135534968e-02,-4.963102469058287142e-02,-1.417419835190351397e-03,-1.525600976613362691e-01,2.331506623285111574e-05,-9.316086550017423085e-02,5.302298101411805076e-02,-9.100941231417448984e-03,1.579107215291294247e-02,3.657189064866776806e-02,5.118590101621267596e-02,5.035698606278266043e-02,-5.715895910906995669e-02,8.479086611853409172e-02,-5.039126923175833717e-02,-5.487489029869115076e-02,5.559136166209485697e-02,5.230285182467963451e-02,-3.586079213706200308e-02,-1.244425104801091253e-01,-8.354087301875451343e-04,8.601493066342559904e-03,6.707415628676773045e-02,-9.656576881681115665e-02,-2.996028211772787039e-02,-2.216072448293311681e-01,8.855682136466520860e-02,8.905178048571571470e-02,-1.141840280003236507e-01,1.260147721999194288e-02,4.287251545400543908e-03,-6.317559630525569625e-03,3.171641946977193927e-03,2.352107080221340143e-02,-8.323131957788545976e-02,8.846663744515867456e-02,-4.233575775359197518e-02,-4.868817990731279244e-02,1.696525039574368898e-03,3.760159641661084406e-04,-5.595054714075731184e-02,-3.100705468211467988e-02,2.278795596673240109e-02,5.527490664285131250e-02,-2.299727735271675511e-02,8.874957263244605959e-03,-1.589080137859613384e-01,-9.233206369553259574e-02,-1.003873683180664111e-01,-1.152914060009105068e-01,6.610067751894313504e-02,2.466505035413328259e-02,1.596566209142971093e-01,-1.302033446025315511e-01,6.167367896987091536e-02,-7.752101131288444136e-02,2.210367433333039477e-02,-1.285135993542044913e-02,9.093729445814940915e-02,1.011222464202756466e-01,3.756595926903176508e-02,-1.850658099137798482e-03,5.432891057799484058e-02,-9.806680353294076891e-02,-5.334934348914847912e-02,5.556214634440930639e-02,-1.509320179111763027e-02,9.229819722258089665e-02,3.359546210076261308e-02,7.488251173156384999e-02,3.170997675039875785e-02,8.963990256713796900e-02,3.584599079957740547e-02,6.075955855155274615e-02,-6.351890378299104856e-02,-1.174744431967959107e-02,-5.262863212626974624e-02,-1.669574057132107714e-01,-6.849457711700088214e-02,1.488817965400724558e-03,-3.786792732617209889e-02,5.876483293882900361e-02,-1.729109720382782794e-02,8.647141777624453574e-02,-2.274837475512948501e-02,-2.840915871796595785e-02,1.534732660875573126e-02,1.695097116104538407e-02,-8.561219621372964661e-02,-4.756080026039030684e-02,-6.553415271505023953e-02,4.455259636387176592e-02,2.549713964613380818e-03,4.054017699555882703e-02,-4.777962330108627420e-02,-1.101165250011997138e-01,3.650884924451406471e-02,-8.001873134757399308e-03,1.571693760371218815e-02,-4.210217944947697993e-02,-2.815052025254355383e-02,3.709924811779257564e-02,1.232255004249638020e-01,9.799341466522785288e-03,4.611450453700052798e-02,-1.804236842313034980e-02,7.098345671137649970e-02,-4.714252689672422669e-02,6.543616560682319339e-02,2.171197686237437072e-02,-2.226418022776387867e-02,1.981957707700165300e-02,1.959904040318391494e-02,-1.010017357251575681e-01,-1.306099735822468383e-02,2.200887678843972683e-02,-6.626472315873299743e-05,-4.039092498464069703e-02,-5.623876692271904260e-02,1.298044812410437121e-01,6.109948720663921362e-03,-3.058970818277666898e-02,-2.303488364111357101e-02 --4.800486238031775876e-02,4.960493356044959855e-03,-6.645820797222850435e-03,6.190184143369151931e-02,3.719152159475452857e-02,-4.509446566453750860e-02,-2.897847550498049499e-02,9.049301676211486678e-02,1.508604688852559564e-02,-7.304931434209760432e-02,-2.202087425840145926e-02,-1.146297344959611303e-01,1.588159301694627018e-02,1.811664153136597899e-01,-5.002320777154010967e-02,-1.622796548883322088e-02,1.045032314471683495e-02,-1.768537792144191101e-03,-5.033924981188028086e-02,1.541212709124931439e-03,-9.761984718840146721e-03,3.389139362661714588e-02,3.916262419042354503e-02,1.128079330708445238e-01,-1.340688318972223236e-04,-9.011789560186157094e-04,-7.019210665944286553e-02,1.942183419509219133e-04,-5.580604329661206786e-02,5.136401676340199632e-03,-2.553886916877492336e-02,-3.601702511291116071e-02,-4.888964957530374245e-02,5.544726089119311058e-02,3.444555028887560932e-02,-7.685012611057001619e-03,-2.022543884600564237e-02,-3.536200619217674812e-02,5.987567582345918599e-02,1.034458786378069338e-02,-5.606978633086205643e-02,4.403911640563844265e-02,8.836654005215381635e-02,8.782417045452986515e-02,-1.669780408290380547e-01,-4.015573072467673588e-02,7.733648178865841494e-02,-9.866379353679312347e-02,-3.360248063801659496e-02,7.168225177088703715e-02,-1.456397299350090102e-01,-6.790401978826203500e-02,-1.076745022476692099e-01,9.725851924912033014e-04,2.705038240039724790e-02,-2.693894069429415838e-02,-3.297186009386535166e-02,1.560006591995575370e-02,7.238723898603545426e-02,4.722274301399518454e-04,3.537035336662804758e-02,-2.983168758085530739e-02,-5.727747186477546992e-02,-6.362021507449187874e-02,2.246910138109598984e-02,-1.053336505124137651e-01,-5.042567410334011790e-02,-2.633686137332143934e-02,-5.975774425112569815e-02,1.904326550787146383e-02,-8.015254604419684570e-02,6.699582073276502681e-02,3.916503417703331991e-02,-2.952797866858244275e-02,4.948236906194339313e-02,3.320341178032636120e-02,4.123758847370053576e-02,-3.701416533751730742e-02,7.605882962202162312e-02,3.560337230707263934e-02,-1.552146551145389131e-02,-3.954480329718193521e-04,8.108722858082145768e-02,5.872077736690595531e-04,3.969029261048333257e-02,7.014185212480042453e-02,-2.266883888137792569e-02,-3.784973950607615351e-02,4.843616198061276301e-02,2.206965295956263978e-02,-3.595882063694933600e-02,-1.255650608402935076e-01,-5.617496919717102771e-02,-8.025229175468454046e-02,-9.386802526112861256e-02,-6.182981636964401950e-02,5.466901562434678397e-03,3.000237561165228217e-03,4.808336149981314728e-02,-2.388344605035239848e-02,1.292976235809421837e-01,-5.971272491994306791e-02,-4.905608805902665626e-02,-1.608228210246768000e-02,1.088666924210774378e-01,-7.725311578321136796e-02,-8.057502244088272925e-02,-3.203806307576043122e-02,5.775007318765033176e-02,5.249173358665789463e-02,-2.272347747474257051e-03,4.354348547164722805e-02,6.818823855955731539e-03,-1.125777565373069049e-02,2.600611992610498552e-02,1.440166159197415335e-03,-4.173633623305416512e-02,2.880997509121271949e-02,-6.203505242527467683e-02,-9.083031354690171422e-02,-6.247063429042937904e-02,6.197949591991314816e-02,-5.357002528835810157e-02,-1.415318228224505398e-02,3.283642783761792083e-02,-4.545438871383541968e-02,3.021104116675146778e-02,-2.170837673006661278e-02,-2.254419408061645105e-02,1.229616201043061928e-02,-1.795829148669740929e-02,-1.373060207779039921e-01,-4.438786015753903857e-02,-1.098438648731903111e-02,-9.561860194612874578e-03,-4.556106009614176167e-02,-5.048797153424992856e-02,-5.529640735992526895e-02,1.445976107319920656e-01,-7.128599136082741650e-02,-2.401448513103827606e-02,7.371654441563296398e-03,-6.080707419544687659e-02,-7.326898946028564605e-03,-1.247923409587892163e-01,-2.731781819512694304e-02,-8.077623452418952743e-02,-8.504876470415527467e-02,-1.068763729755278641e-01,1.432859984710938839e-01,-3.112403025945917681e-02,-6.121578351731646328e-02,-4.723637805596345857e-03,2.157828199161477764e-02,9.055090956532031721e-02,2.205002293305690503e-02,-3.613167853467597057e-02,1.207108281037395925e-02,4.118027803724305658e-02,-5.224694074815942890e-02,2.634278870503646564e-02,5.527675536284375191e-02,-5.769169406312764370e-02,1.426846008987825778e-02,6.003872343048540644e-02,3.863824965208365048e-02,-3.609880450680822556e-02,-2.943376072623817008e-02,-6.930413783431688124e-02,-5.697842256843898417e-02,-1.824494921135485365e-02,1.680044307924453140e-01,-1.654638737087893441e-01,-1.444780528962401334e-02,8.840411902733366634e-03,-5.279286527873437024e-02,1.529325417214773275e-02,-6.608697237725512375e-02,-1.873824445394230154e-02,1.465240456032263217e-01,-9.404816094000426063e-02,7.756624855083152359e-02,6.214325998253530936e-02,-6.789881308177640740e-02,-2.496497168394744628e-02,-2.132371441445847504e-02,3.755171064965208416e-02,-3.073888980615896438e-02,-7.139157382033087251e-02,-5.109507078108099787e-02,-1.031049773757981292e-01,-6.573543614381585631e-02,8.788258481987873405e-02,1.462176689486899619e-02,5.407230147642424350e-02,4.764515188192151168e-02,7.614570852406941925e-02,8.386313332618783312e-02,-4.793342618646884504e-03,-3.169470606675849067e-03,-6.185751980245653495e-03,8.995902612293394870e-03,9.516637971749564162e-03,-1.715125294935987421e-02,-1.668195590775707615e-02,-8.342713581389876731e-02,7.472049180440640259e-02,1.505587239648528142e-01,3.828093672510142487e-02,1.182060531313476187e-01,-9.974542281728071558e-02,1.325367809725950530e-01,-6.690113556520171922e-02,2.712710719162380116e-02,2.458648650825713891e-02,-6.025031539225871730e-02,3.057084440891711524e-02,9.310213036574337131e-02,1.074130205135208904e-01,3.892372170120150798e-02,5.297035884890776390e-04,-6.556587506494315765e-02,3.666321817826583007e-02,-3.475922921347375516e-02,-3.410287717515523426e-02,-3.026267094541850583e-02,2.179733838676746471e-02,6.430909744898827274e-02,-3.202549779348701914e-02,1.015998067268022720e-01,3.676417060409871429e-02,-7.994345448278084021e-02,5.290332491317945030e-02,-2.910569577039248679e-02,4.163785470949661616e-02,-9.891518471602014917e-02,5.434696945451724000e-03,4.912962657665451149e-02,-2.950429757980094908e-02,-2.001244334946323741e-02,-2.342916242310238636e-02,-3.623242736200141256e-03,6.320133108349473838e-02,-5.544296940750670366e-02,4.373245812632216289e-02,1.280006988093520225e-02,2.306941840246125655e-02,1.121449919744504781e-01,4.912051122079245935e-02,2.300237435243053519e-02,-3.555375961463296680e-02,6.294180801845208201e-03,-1.917144919395651625e-01,-5.579708483177995421e-02,-1.037926163938778512e-01,1.744071169380541700e-02 -2.541167584899185067e-03,1.170127283921126686e-02,1.320889645080876827e-02,4.302869283509822007e-02,-4.226022717014752544e-02,2.166671175049068171e-02,-1.249112948225266501e-02,3.473161016465943496e-02,1.254384760885003669e-02,5.430798351423304915e-02,-4.081541959088342414e-02,-1.000112331164822177e-01,-5.795281256732355557e-02,-2.201513062052207204e-02,-7.873879754887948279e-02,9.241509515857437851e-02,-5.414021426541793663e-02,6.176314722046293004e-02,-1.843596874141144315e-03,8.857129114163862782e-02,-4.522612599372047987e-02,-8.987310545813435858e-02,-4.338408394370664445e-02,-2.191888609963318565e-02,4.686361346389102817e-02,-2.661857917356099529e-02,-1.352756874056575409e-01,-8.474484298772884394e-02,1.029951066218262046e-01,-6.525556437846712077e-02,-2.165412878855773552e-03,7.559101759536651644e-02,-3.292487606020203655e-02,-1.776901231634247791e-01,8.363287230840302233e-02,1.568784539016784135e-02,1.310666836954779424e-01,1.798229797376183631e-02,-1.582949477894503645e-02,4.109006217274015910e-02,-5.357294505856616262e-02,6.183455967389816718e-03,7.199407169457210966e-03,-1.072141151225001685e-02,1.362610075186398984e-03,1.289364189744610090e-02,1.870378340787862137e-04,8.963402800760142397e-02,2.139014671465301609e-02,4.065235698481099724e-02,3.102546225753387982e-02,5.097802057510839729e-02,6.929297319080128525e-02,1.275103973537859009e-02,-6.834521278923752496e-02,4.437396369499778392e-02,1.099193075584418422e-01,-4.321714207538635000e-02,-4.307412452293062077e-02,8.577108789536358469e-02,-1.074119648650657816e-01,-1.575763647977961035e-02,-6.934082786981300706e-02,1.980101683913174093e-03,-8.416155768784716606e-02,8.305710042876800203e-03,1.445985622431230321e-02,2.181284576443594941e-02,3.825818178800971525e-02,8.325026550624442523e-02,-9.098208450362501622e-04,6.561614798362248174e-03,7.305185399579783301e-02,-8.557938502740730646e-02,-1.036643490336499815e-02,5.593516246155713634e-02,-1.233730012505205101e-02,1.503985863374826711e-02,3.853302360925063563e-03,1.498655028206267179e-03,4.402117727983682980e-04,-1.850351358513850630e-02,8.114799921572833641e-02,1.190426977979264778e-01,-8.173372295594290293e-02,-4.084072608283650929e-02,3.954785060582698131e-02,1.208916149384582465e-02,-6.186357805871673526e-02,-7.496080107068031628e-02,-7.012028569721680260e-02,-1.868837496507505525e-02,-1.533555474213678035e-02,-7.326122604507026570e-02,1.424168720879010787e-02,-2.213908136960060155e-02,3.146190158547624779e-03,-5.447752134628045667e-02,4.744553729923561836e-02,-2.220223929456553724e-02,-2.646154398706958288e-02,-9.668450186012436665e-02,-4.927767174874515821e-02,-5.483889444789034201e-03,8.801703006539002283e-02,-1.488158450353125051e-01,-3.740461849820921753e-02,3.065025317977111696e-02,-5.077861306304972538e-02,4.510423428548774455e-02,1.015438587011076449e-01,1.635245902692709224e-02,1.570129397269676430e-01,5.049165830325557519e-02,8.695032282012760474e-03,-2.634750087168452257e-02,4.739013673647831520e-02,-2.807059216583681835e-02,-3.588422177466239760e-02,-8.409429934903324411e-02,-5.347196362588427232e-02,6.839425612328950410e-02,-6.923928599091620784e-02,-1.953687479858918358e-02,-4.759527226030495911e-02,-2.388922327333245499e-02,-5.029985304095137277e-02,1.112209930127288499e-01,1.643738777527252448e-02,8.378513892071011437e-02,-1.221394684111253570e-01,-4.168148368969801232e-02,-7.789551475654822588e-02,1.293165364926433802e-01,1.482437718874796487e-02,1.915351471767522823e-02,-8.931185937316106493e-02,-3.247820221582102540e-02,-8.659229808372395643e-02,-2.184294664071693634e-02,-1.496936808602645705e-02,2.188020784732359544e-02,6.172429126999685878e-02,-5.069969076629698979e-02,3.926996769046176006e-02,-5.567749592724158259e-02,4.436075574086298379e-02,-1.584741144769211824e-02,3.208347050958700042e-02,-4.909520185690373351e-02,-7.786633662235378783e-02,5.521741083030905911e-02,5.990738916445581541e-03,-4.901670571634164641e-02,2.694570623934328807e-02,1.020388333116555579e-01,3.352708856049810282e-02,-4.000396688101567899e-02,-3.528467611836128004e-02,1.039623192245901356e-01,5.681579262809317681e-03,-2.269642743296105408e-02,3.744096923192410248e-04,1.196140582353674431e-01,2.224815037724424405e-02,8.348589734638714455e-03,-4.317559656818097114e-02,1.187931942501219314e-02,-6.231374940981118660e-02,-9.241983734988337695e-02,-4.440512510889972619e-02,7.495205591666831424e-02,3.313679705380397772e-02,6.139799874607992924e-02,9.465572166534201581e-02,9.730559132309278547e-02,6.231594505989861837e-02,2.414082566146007192e-02,-6.036828985244503329e-03,-9.415521792238579907e-03,4.509417527283525462e-02,1.649633074145670508e-01,-2.233825195562934574e-01,5.560490543832026511e-02,-1.911397103466525027e-02,-1.327465705340668545e-01,5.814289805656321780e-02,-8.350289954413867705e-03,7.680726230994684711e-02,1.374735300341118072e-01,-6.043095984894060468e-02,1.089493256850965902e-01,-8.768455776300762908e-03,3.151082264213649065e-02,-6.303009825462439764e-02,3.826697402403565762e-03,7.681387437520961758e-02,1.267236288284747693e-02,3.244585448452589477e-02,1.214817907873555307e-02,-3.209377730399240458e-02,-6.066048609797947827e-02,8.401832440687399817e-02,9.073024126433457126e-02,2.538709190644196367e-02,5.770088501607785469e-02,6.144441263519774543e-02,-4.705836323343359773e-02,5.026706264433594001e-02,1.348144333330199111e-02,6.055669260223299488e-02,-3.998657451452515554e-02,-3.554226159225420262e-02,3.779519389914277883e-02,-4.519670313101650100e-02,-4.501897141821390586e-02,-3.903318982589086550e-02,-1.211381183236924855e-03,2.297567190778832327e-02,1.753919000176689183e-02,3.439661654161656201e-02,-5.948093866129610524e-02,7.743063580110452759e-02,-1.438440521270742400e-01,1.026470876778567964e-01,-5.903399805473016904e-02,-6.697548805778980041e-02,1.577740971467567546e-02,2.318415843868425891e-02,-1.679880734367346015e-02,1.965813909366648932e-02,-4.136890673834035803e-02,1.085557350120113923e-01,2.074590237139979920e-01,2.080302239002265047e-02,2.174281698377728694e-02,-8.455956882123707996e-02,-1.751715968748509059e-02,3.582641459904926245e-02,-9.481489647984850735e-03,-2.695397097324353855e-02,-1.045727236368558868e-02,-5.796279295199120024e-03,3.208431491473871452e-03,-7.878195097395830626e-02,5.878794395287557234e-02,3.172539388465147464e-03,-3.020591806148818087e-02,-2.200187137367367457e-02,-2.889636336902066949e-02,5.869022083034872855e-02,-1.288071912859410986e-02,-7.004750688531026503e-02,-2.064795454788015058e-02,1.802621506555807801e-02,9.685106448854367833e-02 -4.029234689859576696e-02,-5.706695002621504226e-02,-2.569634083674681921e-02,-9.519226229290451979e-02,8.107024937447857016e-02,-8.420177622428030861e-02,-5.055128228206383356e-02,6.153802175577392786e-02,-1.604918927050272281e-02,5.750296621388623075e-03,4.670193429902115556e-02,6.150521548138071504e-02,6.071022850284624184e-02,4.484418157773226166e-02,-9.142086187485749804e-03,5.075383512382773687e-02,4.274897669625600527e-02,-4.328905265505141042e-02,-3.408309617848699624e-02,5.240562754285341823e-02,1.247972221213808908e-01,1.257140378668780423e-01,8.166661717214183935e-02,4.547854078546086087e-02,4.205628152967898215e-02,-5.227970729522851689e-02,-8.493891143771170604e-02,4.026948522603951419e-02,2.931051257933737011e-02,-1.858956938520429231e-02,6.396857487302194667e-02,-1.235370115384802348e-01,1.147228310497207110e-01,-1.009043999514538893e-03,-4.331356181192012184e-02,-1.104793533139943745e-02,-8.061350040363084515e-03,3.760922928366085272e-03,4.833439714323130298e-02,-2.572067057284983804e-02,-1.035453096513663240e-01,-3.268915891336071633e-03,6.848555338296730133e-02,5.927193120722408504e-03,2.675126391643736085e-02,7.272266679361445718e-02,-3.880330680579362201e-02,8.854218824098404605e-05,6.935594011968608183e-03,-2.800661311914242821e-02,-3.158383964705155367e-02,-3.909947296427745272e-02,-5.370205531144589539e-03,-3.511506226066092584e-02,-1.758587677003582428e-01,1.019560970701934577e-01,1.822503851494568228e-02,-8.376978625547040758e-02,-2.447540441294925040e-02,9.084784116381403041e-02,8.137493374768019205e-02,5.305334894453019728e-02,1.248508967734633934e-01,1.414254149973624142e-01,2.196062912533174039e-02,-7.306856009414039455e-02,-1.043446626714217840e-01,7.871813679866718205e-02,1.067723611214988211e-01,4.503671676912454624e-02,2.303121916708209330e-02,3.165326289768437967e-02,7.445625540327530101e-02,4.256924078283396556e-02,-1.258927446204574814e-02,1.748053285608566418e-02,-6.239818939160868272e-03,-3.174628009126597566e-02,6.447907581795425322e-03,-4.688539741471051819e-02,-2.402294036249539091e-02,-1.139375145866093203e-01,5.174459841842488778e-02,-7.490154022840805892e-02,3.283589693944864718e-02,5.305239777604033036e-03,2.398285070189803275e-02,2.664089077937684819e-02,2.486588965691050035e-02,-2.165480818267131180e-01,2.848213970853162580e-02,-9.593894007415566805e-02,-3.036356463351975316e-03,1.101724567057922460e-01,9.264307429211607325e-02,-6.074942968915084118e-02,-6.299335963643377945e-02,-2.009794285904002026e-02,8.743465304892237069e-03,-6.674300214332420933e-03,-1.357979223026309357e-01,-3.630650687071425992e-02,-4.839718760895686533e-04,9.742698904692630169e-02,-3.290295104687049332e-02,-3.073410267170600257e-03,-6.635994952739647512e-02,5.386406347251902066e-02,-2.439968624106703468e-02,-6.647157002190184905e-02,3.267277074673140269e-02,-9.974824853122597063e-03,-4.371686807474080283e-04,8.914647332331465668e-03,-2.755023526563613046e-02,-5.781718575600659771e-02,-4.108521621378365130e-02,5.602132151883022076e-02,3.658449087820973139e-02,1.130816153880147279e-01,-1.632489033940701453e-02,4.368654869753707537e-02,5.693153397210769240e-02,-7.008142309621799571e-02,5.798162661913788471e-03,-2.623877272813140982e-02,1.213262931794737917e-02,-6.386046300598990921e-02,-1.112646328654752492e-02,5.598122618403630901e-02,-4.655079856074731404e-02,-2.917924353227871470e-02,1.781159955273475121e-02,2.343066369088087422e-02,-3.064999947427216043e-02,7.868082851992255766e-02,-1.409557029626865021e-01,-8.329446371546068983e-03,-1.851098866348867278e-02,-2.680344315751244377e-02,-8.589938836403764044e-02,-3.466282853029004768e-02,-3.333868594328394386e-02,-5.344790982666377394e-03,5.486240506839876879e-02,3.808877977111869984e-02,7.467577629923569105e-02,-5.322575231952921676e-02,-2.552686248633892485e-02,3.550434298528862970e-02,2.221262174489885277e-02,3.630320231438621120e-02,4.727845686787354729e-02,9.648697732875205491e-02,-2.369917865313420080e-02,-6.235150786744934198e-02,-1.030035485283377766e-01,-1.235396725277583541e-02,-3.322717655294014222e-02,-9.129783449649543983e-03,-1.121756857504280780e-01,1.046034015571992992e-01,-5.532586405835895893e-02,-8.340248276993075804e-02,6.150706926815335124e-02,-4.522871383615751500e-03,-1.286109653782489814e-01,-5.743746538526237067e-02,2.355265890884487845e-02,1.102757890979648020e-01,-3.454629908311812603e-02,2.866253266533732094e-02,6.178583191044438594e-02,-2.881944513440892752e-02,1.218630927925243856e-01,1.218534276879784384e-01,6.537628422249663895e-02,3.123751286146176850e-02,-1.043414403102622089e-02,1.902417842974666456e-02,-1.355892993453429451e-02,-4.702074674907574803e-02,6.443985653705923411e-02,-1.696625686326670018e-02,-4.682994401327236766e-02,-1.477472800760571414e-01,5.331046056565099528e-02,-2.884764941292078388e-02,-4.281839609364725130e-02,6.620664815990073837e-02,7.816181928855815697e-02,-2.058566333226696798e-02,-5.082505444743055933e-02,-1.353196837824913352e-02,1.269151519625853763e-02,-1.293297603854469402e-02,-5.053907211286710888e-02,-2.003573412650478530e-02,-2.681039676806336314e-02,-1.141230938178911247e-01,7.560247292260122154e-02,-1.474180183891664331e-01,-1.275910317722733500e-01,-1.284749535943713883e-01,8.114415694059427808e-02,8.397009908007435630e-02,3.759765266732227607e-02,-1.631792224170000236e-02,1.202049424712484972e-02,-9.163266310077643195e-02,-9.016331711695610429e-02,-3.442599424929998486e-02,-9.381859377222490082e-02,-5.107481890133144797e-02,3.609917765238322696e-02,-4.209782646502754910e-02,-1.191920877292924376e-02,-6.081528767620669740e-02,4.795156861218825839e-02,6.846768074958495265e-02,2.586603585223348073e-02,3.912874988565438056e-02,-5.444086059758687646e-02,-3.373523765603311103e-02,-6.656871905069016229e-03,-9.329285186766518578e-02,4.414978997976187758e-03,1.007227895273726664e-01,3.974924553668533755e-02,3.844747279919267374e-03,2.817005004873376905e-03,-1.349462531073373703e-02,-3.711772130354391158e-02,-5.484040388865028814e-02,-1.531584055841467805e-02,4.816468784993954699e-02,1.050697082468433907e-02,8.889850061790265101e-03,2.743209686061980068e-02,8.505126285979333622e-02,2.894495262636839067e-02,-8.300754744998961598e-02,-9.957111240169902455e-03,-5.904406295245792635e-02,1.871763375706479368e-02,2.594030532046990439e-02,1.357793094082602320e-02,-6.257385313182385611e-02,1.823285589846228294e-03,-5.039883136460595464e-02,9.091046184560284960e-02,-2.185298474669927771e-02,-8.320667747333791719e-02,-5.655487012236008748e-02,-1.076922738716745015e-01,-5.961825806860165944e-03 -1.913525810443518840e-02,-9.408477109435088581e-02,1.233065980954415747e-01,1.771016785395189419e-02,-1.254272972782821335e-02,5.195202588935624727e-02,-1.133624519074008935e-01,2.627755156100949202e-02,4.727678740015345965e-02,5.142850567342417689e-02,2.183530326276468536e-02,5.433940101082099239e-02,-4.646412466755542314e-02,8.442801387082798925e-02,5.544835780865477876e-02,-1.016386672150103448e-01,1.756195437377355839e-02,1.829774683867564644e-02,-4.475304979968089403e-02,7.005787651446830633e-02,-7.011102410580331845e-03,-3.901808457692631377e-02,-5.024908131241979936e-02,-1.316728489878760122e-01,1.073045576391106204e-01,6.531819844104480577e-02,-4.321429292571127290e-02,-8.044739311499402024e-02,-2.765441797010961128e-02,-8.621793599111929329e-02,6.665986981735989858e-03,-1.033568091137379019e-01,-1.141559409308947892e-01,-1.270821191754523405e-01,1.622282592731785489e-01,-1.006716829043309003e-01,1.081930508048790790e-01,3.364031937005675099e-02,-1.149675018532306525e-01,-5.513838099409425209e-03,-6.241230437287220711e-03,-5.023416903338193129e-02,3.788151990873955821e-03,7.203306581225463190e-03,-1.817600450028654738e-02,1.093034344759087617e-01,-2.416623661019335484e-02,7.300796589203784742e-03,7.360181415788733666e-03,-3.638630133723562421e-02,-7.333028138559505305e-02,-3.930939558631189878e-02,8.277227532998727988e-02,-5.321604660308509871e-02,-1.753305465630907584e-02,5.610634798123789735e-02,-1.335200789664128113e-02,8.752197694290077268e-02,-3.562292931733292145e-02,-1.022610130299362458e-01,7.688276775214059056e-02,3.278400904718098419e-02,1.872700284969099072e-02,4.221557322879967988e-02,-5.471963824269960447e-02,-5.866208521958952288e-02,-8.072341452732906264e-02,5.071428764272703504e-02,-2.496495001883640777e-02,-7.537549452032964170e-02,-2.124529683629541865e-02,-3.947740451108589649e-02,-1.377861586406507743e-03,8.015816460386557263e-02,3.812703800194272574e-02,-1.911577522642943922e-03,-4.701623260717445463e-02,4.453135786739582935e-02,-1.297059419985794049e-01,-6.220504740191673543e-03,1.503145991804616735e-02,-5.857126283328489835e-03,5.500686297041332273e-02,4.479330709031154045e-02,7.295339177303222788e-02,-4.395292424049213054e-02,-6.867064581036218252e-02,-4.281939433310240350e-03,9.234358779040911791e-02,1.091611615567156246e-02,5.436496996124932229e-02,5.894664493105465641e-02,-1.216495343442916890e-02,-1.050855269932841549e-02,2.604546753608984536e-02,-1.128101217033647234e-02,5.667628102744423635e-02,4.177690224581908501e-02,3.059397004962905592e-02,-2.847397213912169003e-03,-6.987656818757603805e-02,5.603850490450067341e-03,-2.150520963530528490e-02,4.589000046058808874e-03,5.155638445019960700e-03,-6.315681320228964946e-03,4.950290925542182030e-02,-3.035735389643393067e-02,4.066060924064320015e-02,9.381487066179401701e-02,-1.457882941440018509e-01,-3.256385061048642365e-03,-7.529014091369932993e-03,-9.304608519048140902e-02,3.039639009948857939e-02,-3.754056654276057003e-02,-6.152006091332722429e-03,9.446119727679599953e-03,6.765865772499648698e-03,-7.032924403970937377e-02,-5.246797253430687119e-02,-1.595243380556344293e-01,7.119543368513524262e-03,-4.889126610054748273e-02,-3.346384826215867414e-03,-1.518411225221153252e-02,-5.223890217585465401e-02,8.251876192998119225e-03,1.613914991120467962e-03,1.539439936713869472e-01,6.396556768425744333e-02,3.031811470217578297e-02,1.022225030951776775e-01,4.364529631983218166e-02,2.971140041644068641e-02,-3.865725757595148051e-03,-9.790911190069848113e-02,3.736435492673333830e-02,1.353086216969075772e-01,3.524272907127856680e-02,-3.068033195607981073e-02,-1.247112183276816444e-01,4.815760706341051106e-02,-1.101922408746593279e-01,2.901897992903564039e-02,3.263925040995242777e-02,-9.356872980308167387e-02,5.116799925125907478e-02,-9.065805131067826428e-02,-3.705840714951669224e-02,-1.111341426044588909e-01,-5.174873399799718998e-02,3.465523340969221822e-02,8.181200826858553432e-02,-2.793598547679401745e-02,1.499821027199187418e-01,4.188912449105498814e-03,1.008481224071203775e-01,-7.476090192489888431e-02,-3.742494486165618556e-02,-2.831727692759362230e-02,-4.219268855816860064e-02,3.203645428287315439e-02,1.240644992823815122e-02,-7.754492477368857273e-02,-1.778594335208212932e-02,1.120240381552806974e-01,5.170577490100711215e-02,9.406246412387771205e-02,-1.553156300224831088e-02,-1.360211990761861500e-04,7.755927108052634855e-02,-6.927741681079653546e-02,6.185795553608335051e-02,2.041922670804021539e-02,-8.125463428373648489e-03,3.668038144410069173e-02,-1.470864224364347786e-02,-7.751508538443671603e-02,3.277131253434144298e-03,-5.900999278547050531e-02,4.432968173205910356e-02,9.953199974461729438e-02,4.255617554749255294e-02,2.868447486596161639e-02,1.335928678011769066e-02,8.696251855887675297e-03,-7.064751792276455222e-02,9.784445117226778232e-02,-1.562105338211283029e-02,8.198517023812193494e-02,3.885518029952249319e-02,-2.016348478493843258e-02,-8.432180382513015937e-02,9.913095884139885849e-02,2.600746644206275091e-02,-9.479215554316042236e-02,1.527039327738925409e-03,-2.633245102461862983e-02,-1.639638746834657518e-02,5.820399170223791718e-02,9.334625044919359071e-02,-1.605334438489254556e-02,4.684602268625281257e-02,-4.705535852720865719e-02,-5.240620057862421799e-02,-6.627225539896397510e-02,-7.094159007616859802e-02,-1.458471491164039374e-02,5.551333204554845940e-02,1.003152555993034288e-02,5.694807889899890924e-02,1.274340098404648340e-02,-6.597234725948056266e-02,-9.703133288335598350e-02,-9.665056544845673780e-02,6.958611612596213303e-03,1.536458909772911267e-02,2.326296120534933343e-02,-1.176290653818549553e-01,-7.417629733759006916e-04,3.523827473687624057e-02,3.402736061129481987e-02,-6.529749251260336118e-02,-7.199981515354504646e-02,2.790943938906284560e-02,-4.390239001515154615e-02,3.263312486443867289e-02,2.030525062910079614e-02,-3.764285870588833516e-02,-8.758830273303504221e-02,9.957579987610566674e-02,-1.527722795551491979e-02,-2.320406335654244534e-02,1.164547975855804302e-02,6.962610381785919872e-03,-1.003087823283146107e-01,4.524569581468853435e-02,9.571631480744943743e-02,4.743280368103362787e-02,-3.714109056463676950e-02,-9.190242660167004252e-02,-3.497118616972449967e-02,1.624444889047846341e-02,3.685520539399142481e-02,1.474446676045185478e-01,2.201535854909178469e-02,-6.527631910419445793e-02,-1.011896230071397326e-02,-8.101203656220926086e-03,-3.571221383771901181e-02,-1.011422348808424521e-03,-2.981260331546514014e-02,1.655551699859494907e-03,-1.405197386466290888e-01,-7.495264254107270041e-02 --1.622543890320663970e-02,-1.286101312748167032e-02,-6.622061098725208483e-02,5.604699294190135933e-02,3.579679711398565251e-02,-1.217407936612490901e-02,-9.332106484907504163e-02,-6.150131680823387598e-02,6.019109386875981516e-02,-4.725765030635152575e-02,-2.428415966612957125e-02,9.772059004338526367e-02,4.139011010643700583e-02,3.997833842133152737e-02,1.340135306068586038e-02,-1.364344587840150488e-02,-1.857291163043905957e-01,-2.948393972758451415e-02,-1.460400987365015057e-01,4.649671698506051970e-02,-1.293881790124116679e-02,6.839564374885158393e-02,1.518502875836331110e-02,-2.544144748208045786e-02,2.716200659981944654e-02,6.683725306753277029e-02,1.374368238316290847e-01,-6.909946751061958603e-02,7.795743264047427790e-02,-5.114581425249036667e-02,-5.884982437334745908e-02,6.285753097797998346e-02,2.446255982851981356e-02,1.336007836470510385e-02,9.795981648423587873e-03,5.917513363749534600e-02,1.272718511970724435e-02,1.003667425922617207e-01,-1.294586689259117074e-01,-1.414766399922432090e-02,-5.213612717717103912e-03,1.227185471581031945e-02,-2.108348468063449971e-02,3.370624927379189950e-04,1.263004103371053732e-01,7.018196385518599145e-02,-7.398502716404212198e-02,1.035835531862880354e-01,-4.170074608934862953e-02,9.557443818834335247e-02,4.835739270880063528e-03,5.334947957301519605e-02,-9.068555675839145777e-03,5.209424277222224342e-02,1.804222184030047568e-02,3.505932512544827906e-02,-2.730977382631898445e-02,-5.084723745244632692e-02,1.652396461781664369e-02,-6.001404585221634114e-02,2.310772655350198587e-02,4.661606533268234515e-02,-4.184258281179287220e-02,2.391395069013011951e-02,1.487644562619667640e-02,6.696231563508907303e-02,-3.379068809030612297e-02,-1.677803294041120899e-02,-7.990880464929560681e-03,2.595344230205441638e-02,-9.088117561804241740e-02,-1.816712627846472222e-02,3.513932361546912714e-02,-5.455918789160570104e-02,1.466265992417597683e-02,-2.408891875518488247e-03,9.751945116978345124e-03,1.061874391761051389e-01,7.606396816324027810e-02,3.944867981321314304e-02,3.692007006261451008e-02,-5.669220978494888402e-02,9.991685813903437852e-03,-6.719877706864350875e-03,-1.526985359299667944e-01,4.072230820292139369e-02,4.393575773197178763e-02,-1.165810213330112088e-01,2.685131876833790621e-02,6.454245372129704950e-02,5.252721053942430163e-02,5.697724673594653511e-03,-8.763454458991962875e-03,9.836406657038714030e-03,2.481484032744574186e-01,-5.409266677393707867e-02,-3.854367051251596232e-02,7.619121205621171011e-02,-8.894269323949709205e-02,1.814444511293168347e-02,1.157267869789872783e-01,-4.737275043082627562e-02,-3.533548451393313250e-02,1.253399766815158634e-01,-1.252219043723900443e-02,5.411415307649483641e-02,-7.737332767820652955e-03,-3.758846924791722777e-02,6.398740376521128148e-02,5.871413358471631525e-03,-1.892118252909451986e-02,1.876119668837655102e-02,-9.219336966058468141e-02,-9.292441960417655022e-02,-1.821783518463590545e-02,3.089406601329235807e-03,8.284279612570506768e-03,-3.767591837438978575e-02,-5.873661949336211191e-02,2.364315036624091426e-02,-5.030304890973437415e-02,-1.327098804547901800e-01,-9.734021987776486640e-02,-2.263104827712644762e-02,-4.969868982929151469e-02,-3.464440037480234075e-02,2.918706422020272279e-02,-2.111935656718976970e-02,1.134739642639551321e-01,8.676967522314099623e-02,-3.097817506376157257e-03,-4.670437336772292197e-02,3.552044131367321222e-02,2.949444858304037242e-02,-2.280242295340522557e-02,1.433274361684834346e-02,3.600676611820593023e-03,-4.366229108369319711e-03,1.456299703450565468e-02,5.449331107711486571e-02,7.611178539862491210e-02,1.509499244719135491e-02,9.671690686315263796e-02,4.656200406456151297e-02,1.044058143813999737e-02,-4.103480270086715120e-02,8.567315336594921726e-02,3.051975587359018888e-02,8.757256483187761398e-02,-1.536217204857479382e-02,3.920184475141406943e-02,-4.045622966144466620e-02,-8.158574927789565001e-02,-3.569820852395413219e-02,2.549572385787760020e-02,-1.438016148716070031e-01,1.087514015322178784e-02,1.112417015125681807e-01,4.736794694573592118e-03,-1.291006676146266019e-01,-1.592270678557557856e-02,-6.934209083229831219e-03,8.458594652964519095e-03,9.844460318043914138e-02,6.214096505723037261e-02,2.424262507313807921e-02,-5.220652528412141058e-02,1.026126183727962479e-02,-3.194659008175822906e-02,6.564172459564587836e-02,-1.728835223212281114e-01,2.624258779070558212e-02,8.101624887686040277e-02,2.759551725365098213e-02,4.235150227019085500e-02,5.377945114984328256e-02,-3.094808060159350005e-02,3.593170706303641659e-02,1.056906093988967121e-02,-2.097761425605992880e-02,-4.530489117064028654e-02,-3.209933962797548168e-03,1.229296212276901994e-02,-5.118749004727991131e-03,-4.939338726945276259e-02,-5.511097899253097204e-02,4.994436618884617873e-02,-7.775310321633982502e-02,1.111136361036101257e-02,-8.779572317714839924e-02,2.521903486829490254e-02,-1.199519516146457798e-02,-9.992230118835299102e-02,-3.916661513751285018e-02,-4.731441045070296225e-02,-3.045405430115166734e-03,6.069028360531270733e-02,-6.093861987742681119e-02,2.726002875771783157e-02,4.061745657111044683e-02,-5.928761362370668642e-02,-2.848866050178062298e-02,-4.733616499848083276e-02,1.714323051253930155e-02,-7.687699075157566897e-02,1.337546031443664733e-02,8.486365977938811755e-02,9.384546403606643106e-02,1.314575896859840887e-01,1.298766661397933175e-01,-8.073792298985525695e-02,7.664959333106684258e-02,-5.593986956191868143e-02,1.009292545506179119e-02,-5.399912300764731637e-02,-6.811529242148632590e-02,6.988535789552655086e-02,-7.521315255039157099e-02,-4.039994424947258855e-02,-3.663852430713696040e-02,-1.356159909221519438e-02,1.681627645484942354e-02,7.884506668644682137e-02,1.672545855060484365e-01,2.363971003235527252e-02,-3.853310829411393107e-02,4.228437173111644631e-02,2.825791877333953990e-03,-1.298330569199581015e-02,-5.938501412405795821e-03,-2.007098945694008735e-02,-4.523422422108312746e-02,-5.842561602587146435e-03,7.545006022549381741e-02,1.942543069691265653e-02,1.174010867014347974e-03,5.947573418786299831e-02,6.129011912416576502e-03,1.544717537235839599e-01,3.329003857671904501e-04,6.269761555387146079e-02,-1.078407686903986792e-02,4.296197032197043159e-02,3.981138987334719576e-03,8.226076984488214783e-02,-4.850866084812928414e-02,-3.926923737171264528e-02,4.068817213777817243e-02,3.799233540705709150e-02,7.350619760384201340e-02,-5.836697829469945226e-02,3.435273859772055433e-02,-1.313709867751169935e-01,9.905267662018588104e-03,1.168502690947641337e-01,8.878210499430822314e-03 -1.035783640675229933e-01,-2.320392312797056253e-02,1.181168372929588817e-01,7.631691342180946247e-02,-7.316039633065969661e-02,3.140213388287592389e-03,-1.390969709643871954e-01,-2.736033770535110932e-02,-5.423433682249426124e-02,-4.036133790435333241e-02,4.703058831826863184e-02,-6.808045216758484841e-02,5.822593833953081111e-03,-3.565453324474882058e-02,1.368180951986118499e-03,-1.153132115791333512e-01,-2.768935543969821417e-02,-5.658396170733689945e-02,-3.609402668652801627e-02,4.089188210710397592e-02,-1.167318524105402655e-02,1.177543384815541716e-01,-2.235875776558329114e-02,2.713078371812694539e-02,-6.973715365710758674e-02,1.215199267125664090e-02,5.053330368173483689e-02,3.286968392950706530e-02,-6.560282610368206244e-03,4.549229979826717452e-02,-1.945800921567122921e-02,7.181809130894961646e-02,-1.007659638369301225e-01,-8.059707746356069985e-03,4.774563406582151698e-02,-2.102753409602216642e-02,-5.206717509963419699e-02,9.987317971554082741e-02,-7.500607032954709208e-02,6.160060318769588171e-03,-4.436426622176156687e-02,4.747997137555071134e-02,1.393998856804206377e-01,-1.589748208839923335e-02,6.810748477815978252e-02,1.187462661096145983e-01,2.566401224737352438e-02,1.055835558247253253e-01,-2.877711639275170805e-02,9.239584325883598570e-02,4.554915340502163623e-02,-1.274151747986242145e-02,-1.251479445508065294e-02,-5.886845835621572903e-02,-4.151214989517141435e-02,-3.085486147118696870e-02,2.526685617441939893e-03,6.156948674726332726e-02,8.763466132558334065e-02,-9.216033725745562816e-03,-1.506135275967817461e-02,-9.723605081151939777e-02,1.174783888366587141e-02,6.952662438636278419e-03,1.086591344668663695e-01,-1.394240369256787293e-01,9.304984522254725055e-02,-4.645072475101391651e-02,-6.877835823052648456e-02,8.725612672626607835e-02,4.552194201841182342e-02,1.165391548730192357e-02,-1.137871721523925905e-01,5.713138807021751059e-02,3.121530685761705198e-03,4.158655879252586701e-02,6.854024978266360507e-03,-4.476362273529511937e-02,1.076983429765787370e-01,-9.628099534494050094e-02,7.631910035210912047e-02,-4.706589265388356341e-02,3.529938268446781990e-02,-5.207527845900758456e-02,4.322981069065030824e-02,-2.329625734123254993e-02,-7.939010698521742349e-02,-1.979020181119638122e-02,1.025293343002137703e-01,6.042101646755285160e-02,2.823048798299646056e-02,-2.250719969115529129e-02,1.027957037072847329e-01,-4.772033921422214053e-02,6.720297705170696217e-03,-1.656341008366796902e-02,1.784225949572915138e-02,4.149406300073388287e-02,8.008143039365291094e-03,-1.717600872553148861e-02,-3.314418237125117683e-03,3.176500537992751200e-02,5.849911705216587210e-02,-2.526775059001658622e-02,3.970031173420039694e-02,4.593453187668000703e-02,-8.325830821508718727e-02,4.735670731729830357e-03,4.316024240402573098e-02,1.395765325634134614e-02,7.511303373480719692e-02,1.320007133221472451e-01,4.433853077183129432e-02,7.071788670745526861e-02,-5.283296009458328391e-02,-7.404110603621408271e-02,-1.129844064566334766e-01,-4.214237260891008641e-02,-7.894011808852502421e-02,3.542696899838213703e-02,-3.573452208328752044e-02,2.089249869016767194e-02,-3.401468538986400503e-02,-4.152778031896853239e-02,4.999769502200697435e-02,-4.617962357565452702e-02,-2.031492679819946884e-02,7.331639708095998020e-02,8.129297257867279525e-02,-5.199872506896525970e-04,-7.128154398444917772e-02,8.655853308441853466e-02,-3.373780977217914062e-02,9.108664139637710766e-02,8.535166306257059576e-02,-1.873699218374912448e-02,-9.439477235660756471e-02,-7.063906836372994824e-02,-3.671039799961355043e-02,-1.930334670745578740e-02,1.133890370259774544e-01,5.726901076972660404e-02,5.113693848938040409e-02,2.178963426271247200e-02,5.516390972324825004e-02,6.010894562138487801e-03,5.745252929148236126e-02,7.156011976761127158e-03,6.485558335086630960e-03,-6.654226118958281311e-02,-3.436622513423041175e-02,-2.257745815315882754e-02,-6.314404434672904465e-02,-2.872887460468801640e-02,-5.600604052593788351e-02,3.878520680726277314e-02,2.383831371296529777e-02,-1.313699414324994152e-01,1.067796743362004787e-02,-9.427107594540408766e-03,2.604109269000078053e-02,-3.649650083433053638e-02,-5.552070058781040374e-02,-8.752420971228588864e-02,1.257302837780320537e-01,1.060737632920811524e-01,8.806534945672059300e-03,6.464980225777330891e-02,8.370503383220925797e-02,7.543410065355893312e-02,-9.784778383806251589e-02,-1.144508426280732225e-02,2.881691469479600795e-02,4.027103070193668866e-02,-1.221232459620813643e-01,-4.635452527790013058e-03,-7.406790820717643054e-02,-1.588338882069177671e-02,-2.012812408497022948e-02,-2.582736032380635038e-03,-4.515861059776322961e-02,3.310545189164239288e-02,-3.591374792289638712e-02,-1.634920276936137618e-01,1.157509901727395568e-01,2.739114266893609081e-02,-6.190610597723658626e-02,-8.078539279171619014e-02,-2.117506516018852550e-03,5.871034025773529119e-02,-6.728974228110141953e-02,8.814906535024452650e-02,-1.499216889575919615e-02,1.415262352192336870e-02,-2.126374518181312762e-02,-1.371712062857717226e-02,-3.265130892269985791e-02,-2.119302931701873283e-02,-3.462433333147653391e-02,-6.715968322020834935e-02,-4.624004342704501891e-02,-6.416156505622920558e-02,-5.418296692009535141e-04,-3.572372892002521200e-02,3.608979340521519669e-02,9.022192386112228013e-02,-8.453180486792055692e-02,-3.491228682184977644e-02,-4.877173077168275167e-02,3.713997874928643556e-02,3.839055171741038996e-03,-3.177358754497725674e-02,9.298656858353959720e-02,-1.410326212335412908e-02,2.194620789916494966e-02,1.017947650214028610e-01,3.871970128465308802e-02,-3.265236143436459071e-02,1.975955000470226985e-01,9.246814162857183717e-02,-3.338323750464995687e-02,2.389527002189313978e-02,8.999625736479274773e-02,6.578183300084960820e-03,-8.312905440382928335e-02,3.165997613283669243e-02,-1.871915969676399977e-03,1.306046229958919280e-01,1.537173160032313801e-02,-4.716793143120438098e-02,-7.021685311892936210e-02,-8.297645917021166662e-03,5.219751859200290334e-02,-9.775897312171301368e-02,-3.368097916304109429e-04,-4.857650875127565715e-03,-2.888948949039186337e-03,-1.258587130335828974e-01,-7.688553960146890520e-02,3.085352574756382998e-02,-1.689587922593695571e-02,5.572779241448083676e-02,-4.896782674486999803e-02,4.668247835541719515e-02,-1.387464948358212678e-02,-1.982322927735208351e-02,1.311026279005003334e-02,-8.034814849842253359e-02,4.585935396795914193e-02,2.854729350647284072e-02,-7.615277228669736775e-02,4.225850604214850814e-02,9.930452090759607497e-02,-7.442524115015611286e-02,-1.005100402123670633e-01,4.312356414201812954e-02 --6.108499876501286008e-02,1.092140200751778867e-01,4.473399812589645176e-03,3.356683453125761107e-02,5.498631751772894982e-02,-1.500050712465036355e-03,-2.946386775083398302e-02,1.004013354432110317e-01,-4.457453827133309177e-02,-1.371930362456029451e-02,9.103084436704482474e-03,-6.008343298421099654e-02,4.086215945895693008e-02,-1.653813557592527517e-02,-4.369551681884786126e-02,1.097947371958180041e-02,4.716125798342885034e-02,-3.070147613856148011e-02,4.413261357956210079e-02,9.455760210486494066e-02,-3.627292081513588951e-02,5.015876708535001172e-02,-4.866928746240379439e-02,1.337195754190986519e-01,-9.876947295055013912e-03,3.838358989115682463e-02,-3.452506960254669854e-02,-2.587170335922167955e-02,3.398819992827464831e-02,-1.197382924441637020e-01,-9.023281867595798339e-03,-2.271674670845724373e-02,-4.109557139536754861e-02,1.014681432521169144e-02,1.607643767475030461e-01,3.146356154086268203e-02,2.541985420125963263e-02,-4.914635833390786773e-02,4.277756604401623981e-03,4.321456840128675564e-02,-1.470784255006684083e-01,1.182137315524156018e-01,-8.202195441976295465e-02,4.776386747382982412e-02,-2.232986654718702862e-02,-4.796328439213454198e-02,1.755859359900125219e-02,-1.072840716503348640e-01,1.646649966949493005e-01,-9.025138614319088912e-03,2.079579570745323028e-02,2.231223215629720910e-02,-4.216394122196683025e-02,-6.659549210894260239e-03,-2.036978275080304074e-03,2.524788534963981479e-02,-6.285199208141975524e-02,4.700402517281183912e-02,1.734434237610752577e-02,3.057011971152016894e-02,9.357938773656705733e-02,1.397508361943784402e-02,-5.579046357155258151e-02,1.202882876829088760e-02,-4.105071076741183961e-02,3.669519574852717764e-02,-1.517505811846272049e-02,8.450441553341327128e-02,-6.944533789913462374e-02,6.128394032757986977e-03,-5.447187367726447071e-02,-1.541831196737530711e-02,-4.883067772462034578e-02,-6.050443764109594141e-02,2.904229217768256044e-02,-2.026965602006176176e-02,4.036656043529252003e-02,1.111765490185943894e-02,1.612364528988184317e-02,-3.278838559576321282e-02,3.668573119575174735e-02,-5.587414939779508302e-03,5.540883065253533052e-02,6.452786719172080854e-02,6.226395137451848361e-02,-3.734248133503210304e-02,2.920719529402990983e-02,-7.312148186033060049e-02,-7.361526355613555450e-02,1.488437561595331790e-02,-4.054588106611609066e-02,6.021481067381312391e-02,-4.974017924849066102e-02,7.639485177671698213e-02,1.193892264346200355e-02,-3.050009151940496349e-02,-1.477520969344632909e-02,-1.090159692982469591e-01,6.001106654907854138e-02,-2.710423024960963156e-02,-9.472758194885559646e-02,-3.216430520278167060e-02,4.899679851785856272e-02,-6.233554206708371431e-03,-1.087685843874720447e-03,-4.875235982781868371e-03,-7.080630179035110605e-03,3.539164939483831901e-02,1.390812988196783208e-02,-4.581078545827683979e-04,4.584969661518388528e-02,-8.060633130325640938e-02,-8.239725045641403678e-03,6.489490923125189326e-02,7.751715759535166006e-02,4.884760220337230202e-02,-1.043206076346193667e-01,7.339018735736810295e-02,2.748112805668542769e-02,8.142724001292579727e-02,1.940181251307398352e-02,5.475074694042832840e-02,9.395987515114813393e-04,-4.859169495678828526e-02,-1.120367151112190707e-02,8.030196330456849940e-02,-6.089744296146114982e-02,5.803929645561581308e-02,-5.512945005313275859e-03,-1.683402571203410775e-02,-5.066748626848932568e-02,5.882250179319192113e-02,5.822126305909654936e-02,1.560915739917553488e-02,1.702853149615197667e-02,-6.509801246408765396e-02,-3.703823816559505960e-02,3.361231714177156937e-02,-1.279405107655474383e-01,-6.376735527194504760e-02,7.364280200622641548e-02,3.146777386767234813e-02,5.378538479990679910e-02,-3.895298645638374524e-03,-3.188323314269487022e-03,-3.650987507161963797e-02,-6.951889036718977843e-02,1.090405145755333344e-01,4.546248972317885190e-02,-1.355056147989033843e-01,2.996334281292502549e-03,1.719996985178449400e-01,-1.684718967669899259e-01,1.092543797064340905e-02,-6.795324935056260007e-02,-1.848630619649942186e-02,-1.217856497937001153e-02,-9.789157596328827617e-04,9.776497006462875339e-02,-1.123379840212220260e-01,-4.479840567018764791e-02,-5.132391121040225102e-02,1.360747188035840542e-02,-7.801467975294414103e-03,6.213467441976750633e-02,-4.676498540121057368e-02,1.393945162942478687e-03,-2.576263322414607676e-02,-7.296175262760049018e-02,1.089200524831171191e-01,1.642730477493531405e-02,-9.158166933432977663e-02,-1.329423915698748626e-01,1.295344821826888149e-03,-3.252573771392452961e-02,-2.374609523501648028e-02,9.542168075827235640e-02,-1.571993676572621540e-02,1.048717337754247469e-01,-1.043518891602106952e-01,-1.662897884474660859e-01,1.523058049930160057e-02,1.936230730748314879e-02,9.993837524477348910e-02,-3.348869052025039983e-02,-1.069028937034376299e-02,-9.104679407615870468e-02,-9.323383011071505189e-02,1.719430808661333993e-02,-4.135380564391558100e-02,-6.370147413819804427e-03,-3.553094996364564856e-02,1.316580531096025186e-01,-4.177173096207865977e-02,-1.295990875377997786e-01,2.544497921375712610e-02,-1.780465589040472474e-02,-9.752039869935641425e-03,3.227197341785781326e-02,-9.070424567040619168e-02,7.375015538386188452e-02,3.984358263456652671e-02,-5.666086084359814312e-02,4.372625051180994288e-02,-1.067782732523089029e-01,-6.609607401425796058e-02,-1.506047603649361515e-02,5.228101073697282580e-02,7.224966362961277033e-02,3.655853478595305628e-02,-2.194848321080751230e-01,-8.290308084811733730e-03,6.004030925051274253e-02,3.035230184576434564e-02,-4.904197985286224554e-02,-5.173832467965244070e-02,2.289781730665574333e-02,-6.891980770568503778e-03,-6.215490301940765350e-02,-5.373134903354435943e-02,-4.996082018860684149e-02,8.797107184443606065e-02,-3.161593665052858659e-02,6.120415318202865329e-02,-9.731222311618301091e-02,-7.231434821954221825e-03,-6.511121042619934240e-02,-1.085865769888060828e-01,7.427534788311737635e-02,-2.878011542112376975e-02,-7.655002900651260156e-02,-1.009171186299820736e-03,6.164558551162382938e-02,5.838151783650152166e-02,7.123453533708240873e-03,1.318709270435483930e-02,7.133412829200128091e-02,-1.125172515911486287e-01,-6.508917775843900411e-02,1.131708877619718306e-01,-2.086805456887194654e-02,-3.509928899330859615e-03,2.227850926493936054e-02,-9.779524745779429040e-03,-5.595192023222227140e-02,-3.795400766421916372e-02,2.765019325134624242e-02,-1.651987335440706015e-02,9.755636596784361680e-02,-4.486973503963580939e-02,4.676326904185107436e-02,3.790494973021087122e-02,5.140703012560212531e-02,5.957850316965034032e-02,-2.233075545620404057e-02,-6.404969102398143377e-02 -2.410995128460119256e-03,8.127005227084800609e-02,9.858182327770349807e-02,-6.364165432803008116e-02,-9.023873217792673096e-02,2.817449272284681602e-02,-1.150434570269272261e-01,5.733559363683528309e-02,-1.828724131486574728e-02,-1.296974245304209628e-01,8.018035643983371930e-02,9.612077756462648892e-02,1.121378859711688325e-01,-3.268782290691336911e-03,-6.277276132486601934e-02,-5.979196085405016903e-02,9.896358735835632942e-03,-4.515003259903529326e-02,-1.540995154633426935e-02,-1.878368211648178548e-02,-1.328015165490097904e-01,8.765528393019019968e-03,-5.793619572248525501e-02,-4.562037846743588997e-03,3.196756109596537304e-02,8.427190032260160890e-03,5.102877022932042483e-02,2.743921316894606943e-02,5.170824092039918302e-02,-4.434705657714791555e-02,-9.141516790745543575e-02,-8.868023574273012355e-02,-1.265138285442076599e-02,-4.577969014441633094e-02,-4.377989993914539052e-02,9.952893876146105823e-02,-7.388137546846135217e-02,-6.591267183547061537e-02,7.072555850607700756e-02,-1.379836932631278157e-01,5.941730012701734037e-02,-8.046756253330529518e-02,3.596743709156915991e-02,-1.894469736034140595e-02,-9.881081834730882918e-02,2.630968239872841230e-02,1.356391061281151944e-01,-1.048155143601533640e-01,2.423685813457279736e-01,2.528574802803735394e-02,1.387298817990459399e-01,3.594476211359012852e-02,-1.241158920360280714e-02,-1.877895683889349923e-02,-6.666656518903735229e-02,4.131874649639901559e-03,-2.884966698969757340e-02,-1.060493980013299947e-01,-2.976415176531573004e-02,-2.577165157527910519e-02,-1.082562568017483157e-01,-3.735449713549470613e-02,-1.761470169849830014e-01,3.034852046609624876e-02,2.569503691994583228e-02,-7.605091179165393567e-02,6.837511016600343605e-02,-6.107683611543834185e-02,-3.167619864028611842e-02,-6.451163162013874575e-02,-3.957524889079894193e-02,3.820453884472529982e-02,2.064693700436587981e-02,-1.288102516289479221e-03,-5.190891842718795696e-02,2.084981754482868530e-02,7.120748381065755694e-02,-7.694039884591803713e-02,-4.890162164031276043e-02,1.005166777035245013e-01,-1.103599064154030251e-02,-3.305972422528235394e-02,-9.385322301887273622e-02,-4.423135735252278766e-02,-4.171968583376329739e-03,-1.358835850835670156e-01,5.969851298266138501e-02,-6.448381542178126258e-02,1.339570447599186878e-02,-9.232799354801367320e-02,-1.054522882199316614e-01,1.049963418212965585e-02,7.502597482573010457e-02,-1.185595574325857410e-02,5.893748767568156299e-02,2.970630043412131674e-02,-3.399502069539665772e-02,8.504867674530082466e-02,-5.541883405666782336e-02,-7.053665368222948595e-02,-2.337020053964441665e-02,6.927383376875724530e-02,-1.067642163412155787e-01,-4.426101192490695946e-02,-6.287559965220286540e-02,2.103543478374041154e-02,-3.338862403650194599e-02,2.649089638109822925e-02,2.874429266658361931e-02,-1.562897013696407100e-02,-2.158813122257863260e-02,1.065195574224259145e-02,3.614290362155379160e-02,-6.073258535345990583e-02,-2.853571464591473511e-02,-8.055627249400866552e-02,-2.064680838563812820e-02,1.296504954516830566e-02,1.850323255782772985e-02,1.761456047576388448e-02,-3.821967853549683089e-03,-1.217012783359996096e-02,-1.301351606554934925e-01,1.062576173328554918e-01,-4.742512643538538547e-02,-9.877802460087857506e-02,3.896893240729475821e-02,1.471246072443894992e-02,-2.186561155410743362e-04,-7.030387626134387835e-02,3.432085235867463058e-02,-4.541013942108416968e-02,-5.902757341652811740e-02,-1.036998033584552170e-01,5.230469068582586922e-02,-1.408154171475944558e-02,-2.043376959352360966e-02,-3.733563767073659612e-02,-5.569986414176031353e-02,-1.929047379740952625e-02,-2.727014017113760583e-02,-1.193225880000068906e-01,2.291164824735698186e-03,-8.212187352312065591e-02,-5.140176763400687576e-02,-7.007256859198776611e-02,-2.614569269920792235e-02,-4.667205221262338277e-02,1.705194242518208447e-02,1.439502769337288983e-02,3.157306009932735191e-02,2.416989296369205509e-02,3.363450956224748462e-02,-5.165469578233201908e-02,-4.563982637214358817e-03,7.257667449190960407e-02,6.154532816667406675e-02,-2.457161715962100090e-02,-8.156993671582965511e-02,-2.139575758310980312e-03,-4.846884878165924615e-02,-6.993103964413882601e-04,-1.533924546512836252e-02,7.128063405662549146e-02,4.565470028637481914e-02,-1.534806592724717068e-02,-1.051084606268190037e-02,-2.471422306489076334e-02,1.335614761534693190e-02,2.717657270621476329e-02,3.451754994561301304e-02,-3.923749085785210172e-02,1.565038681554522734e-02,2.080763941938505029e-02,8.495718969878916782e-02,8.329795528305074248e-02,3.070689149700660953e-02,3.016073076047603996e-02,-1.604749349560703406e-02,6.623487470053254778e-02,-1.209712486839959605e-01,5.785347138411658929e-02,-9.602207390222942257e-03,1.947604298874881096e-02,7.830472072199155809e-02,4.980947485608401559e-02,4.202647973268508474e-02,1.152491413774573148e-01,-2.707232193801939468e-02,-4.683283151155809287e-02,-7.542256123112482669e-02,2.794450679094473075e-02,-5.744629927583020171e-03,-7.473506621908210246e-02,-3.033599990054612616e-02,-9.610805402403700681e-03,-1.235598175534760174e-01,7.212442154653801729e-02,-4.035698000643039851e-02,6.972197073756727925e-02,3.301863699768956689e-03,-2.140099399568606495e-02,-1.998328888692187458e-02,5.987698145189510046e-02,-1.140923811760348183e-01,9.761670442269335807e-03,2.284848338379474181e-02,-6.243032730803323160e-04,7.293536453555266030e-02,-6.773395902724324742e-03,2.736604333030018960e-02,-9.790884595495967513e-02,-3.427197165492405079e-02,-2.584579731452008727e-02,2.826103082919929552e-02,6.003258821477273910e-02,-6.696611841752929395e-03,3.616640184138496250e-02,7.665265198250875711e-02,-7.010771179405760056e-02,-2.611195771308189376e-02,-1.370277261326832002e-02,1.774678413777708644e-02,7.750915590499687924e-02,-8.112795311910016294e-02,1.124835911869296289e-02,5.301802767443589137e-02,8.022996629608833419e-02,5.694104734691630976e-02,-2.020030758108660313e-02,9.917046992622589874e-02,8.152434214558047787e-02,3.838949610959951908e-02,2.024910416112891270e-02,-3.894080043530453311e-02,-1.111115283355986437e-03,-4.268637471860564636e-02,3.697553906616363878e-02,1.482307768024901384e-01,2.541753319047013529e-02,-1.747575503201360264e-02,-7.912643974978464737e-03,-3.685446353512836920e-02,-9.125214552066224227e-02,8.282492191435618378e-02,-8.523446358485510521e-02,1.149780637062155358e-02,-9.185790332394930258e-03,2.264865540012005185e-02,2.709916311706533909e-02,1.493476652573434149e-01,-4.240017649668324784e-02,5.873240440195461043e-02,-1.541823974668735976e-02,-3.472298886346354349e-03,-9.058913516786341880e-02 -2.700867474978008165e-02,-3.763200354220109373e-02,2.905289684625548749e-02,-7.697956145133534600e-02,4.119956337924319742e-03,7.942990614819820522e-03,5.698305406889764974e-02,2.164493766474243769e-02,8.707755122446769747e-02,1.698824026026712897e-02,-7.069331196374148418e-02,-6.591362318796711861e-02,9.774316765820412201e-03,-1.265785759205810823e-01,-5.697352074602540783e-02,1.060673895487948548e-02,-6.118690821112086897e-02,8.485899603303223571e-02,6.187048111655287647e-02,4.155684822735246017e-03,6.333763474273054750e-02,-2.489248172435042789e-02,-9.466013876816745110e-02,2.015627934414288003e-01,2.019106821632666790e-02,4.768540511799895798e-02,-4.957253376678029400e-02,1.562404578614896497e-02,-6.750396891991929105e-02,5.675864551519513301e-02,-1.220567563806125300e-01,-3.251658750255345676e-02,-6.529668663006270302e-03,-2.960180263185165778e-02,-7.532236839179978771e-02,-3.870056554394345899e-02,2.876730536953833392e-02,-2.666148087456084170e-03,-1.397303657427245438e-01,1.364560374103147566e-01,5.740672025964212166e-02,-4.958281813238868407e-02,3.131907986840248154e-02,-2.027599798300683959e-02,-4.511058494893317727e-02,-2.491762584721348567e-02,-4.478859760904149029e-02,-3.412921702823785985e-02,3.841338236005224782e-02,2.446607525782714537e-02,-4.226977520858309773e-02,5.794107772433410020e-02,-1.082453855534917450e-02,-3.075178708385137641e-02,-9.145761979719306545e-02,-3.668239007768039905e-02,-1.994734605782102349e-02,-2.304556389981680248e-02,-2.628885610040997828e-02,6.878579320654377993e-02,3.200541464479057796e-02,-3.413228838885487559e-02,-4.335899775382481836e-02,4.867094398122415871e-02,1.118932587991248451e-01,-7.996369759132399002e-03,-1.002953697543506884e-01,-4.021420417511989420e-02,-1.598217846804110276e-01,-2.381514695908470633e-02,4.791448202274847801e-02,-1.241349925864186979e-02,9.147834849460818180e-03,-2.523183626842918220e-02,-1.534147678198683196e-01,2.518200707914004799e-02,-4.641048077161978591e-02,-1.091917636083978432e-02,-8.636735455337279932e-02,-4.519982525966350848e-03,-8.181600343264804065e-02,1.104016967211986222e-02,-9.594187057147107733e-02,6.199480579870007141e-02,9.128110620837440181e-02,-1.441288016909625935e-01,-2.015943842969413619e-02,-5.108892048070994008e-02,8.872455397089237805e-03,-7.974841680178430814e-03,3.041923467176089330e-02,-7.829643321086668384e-02,9.684841576247719269e-03,-1.864619118909885409e-01,4.072304194053403409e-02,1.750636468879100682e-02,9.377149696421500924e-02,4.392557715727080092e-02,2.710914351829720945e-02,1.927232659009268587e-02,5.066447877530317473e-02,-9.732236433618209059e-02,4.224913606958505263e-02,-6.495606135053927366e-02,-1.195830370245432733e-02,-3.330286424905584686e-03,-3.476598975018898967e-03,2.375275655724927784e-02,-2.837694845689351514e-02,-9.089956574353726115e-02,8.646480868504766160e-03,4.883229435949256597e-02,-8.143254473169722618e-02,-1.563921536093287323e-01,-1.895562470753737669e-02,-9.686125620690849702e-03,-2.714780938684112702e-02,9.508621978619916093e-03,7.622027786983405495e-03,1.116055118853052919e-02,6.552346932305538885e-02,-2.777877693116334937e-02,6.683948554941476772e-02,2.775025249756481183e-02,-1.008509085399332261e-01,2.422975751050068446e-02,3.746509921104525243e-02,-1.209551786986053784e-01,-1.094518250291378822e-03,-2.264124810057275422e-02,-6.033760801932445267e-02,-1.129983953936565116e-01,5.444502632962282956e-02,7.083753872507853044e-02,3.919396232765953725e-02,2.990205522753478645e-03,5.879457955026248794e-02,-1.037392427645714155e-02,-4.591964309619300799e-02,6.034308982568968582e-02,6.118117673120099242e-02,-2.063612694859068286e-02,-4.898278239640747878e-02,-7.554815775336141959e-03,8.293365488302339994e-02,9.660589593659930918e-03,-1.572829776106683952e-01,-6.189122321209505895e-02,-1.285353881542729976e-02,-2.144462088735216154e-02,1.402912627220295749e-01,3.290593503811248960e-02,4.943399921275632575e-02,1.874146131990930136e-02,-9.458418550093329602e-02,2.962731928460495684e-02,1.771173738307000450e-02,-7.742671920300744914e-02,-1.983079659844162387e-02,-2.199283964101248628e-02,4.512760801481964512e-02,-1.294092894032443097e-01,-2.972660789151365321e-02,1.081135325774736367e-01,-2.853760728408733308e-02,-8.462633410023230607e-02,5.846696622684341427e-02,-4.354217021447126523e-02,4.975394917964635205e-02,2.537558646642613588e-02,-6.876508080371478149e-02,7.151210456405873717e-02,9.586333258646792532e-02,-8.457961971902433485e-03,-3.133382573006741195e-02,4.421708569662686356e-02,4.304858690728967374e-02,-6.579089085534656212e-02,4.322194955763064561e-02,-7.043450189158179109e-02,-7.970334610328909733e-03,6.410927369843542889e-02,8.020943915097999977e-03,-1.168271136983522335e-02,-4.729473653102832936e-02,-3.796565765554150984e-02,-8.530503843914954976e-02,-6.032094277723957371e-02,1.482986424682256749e-03,-7.629235863845454380e-02,-4.081183664152609331e-02,-9.235600483217126094e-03,-3.639270078730087038e-02,1.555221075207565556e-02,-1.724460928704266496e-02,1.434465787784517873e-02,1.321095168185392466e-01,-3.551276751036362700e-02,9.248895776763475796e-02,-1.022959899842371401e-01,3.078672155029257840e-02,-1.907200012094156794e-03,1.901520275000516055e-02,-2.870888491824250682e-02,1.754151163341905253e-02,1.895835878261680624e-02,-3.272339966969461944e-02,3.270817477103088455e-02,-3.441353062967959092e-02,1.638083839990586710e-02,3.809820161848227749e-02,-1.879371194484686161e-02,-7.317238364765187020e-03,5.779224353200294872e-02,7.791960910294958020e-02,-3.100615041888850343e-02,-3.505769041552510984e-02,-3.923203069934996096e-02,2.258377183078439515e-02,-4.816917219603653044e-02,2.568782426336874147e-02,6.516635626683041360e-02,7.904665451966871870e-02,-3.208571378641683625e-02,-4.784475435582880021e-02,-1.197358696125698091e-01,-4.769688868735434684e-02,8.084742712426007394e-02,1.264806416408642831e-01,-1.859811950185299989e-02,4.502822924627745971e-02,-1.073937186354528145e-02,-1.616459361579041520e-01,-3.162421410882798600e-02,-4.419396774830978980e-02,-9.731788472440241339e-03,4.917994090708661253e-02,1.517383945946675361e-01,-5.786800331688798232e-02,4.323107250912062993e-02,-4.386259758564027555e-02,-4.783575178815027001e-02,1.312801264752879726e-01,8.189152290879259805e-03,5.441269844762218477e-02,-3.674455069833964388e-02,-6.750074309958174756e-02,-1.058773459583281368e-01,7.162759138832988648e-02,-2.280777199900616697e-02,-5.626038183750520227e-02,6.506487452420982309e-02,-1.664696661162989172e-02,-2.113343567962670810e-02,5.039873848796640188e-02,6.120530352802941269e-03 --6.042679778333808448e-03,-1.232103398269865230e-01,-4.669053895832128709e-02,2.158758385576969516e-02,4.403298263042838834e-02,-9.790220304234927701e-03,5.848989266825499467e-02,-8.907364791909608281e-03,2.689645866212606040e-02,-8.956985750319218509e-02,-7.645288309172061647e-02,1.210933474132191989e-02,6.064948130037800533e-02,1.389541255666404407e-02,2.186326602899718585e-03,-1.409444902839043778e-02,2.372992296403129975e-02,-1.268359041369230555e-01,1.143845779518340780e-02,2.115674968911533671e-02,1.245767321063161043e-01,6.749872060175340915e-03,-1.525642285347797444e-02,-1.707901855149262516e-01,-1.088671160570286664e-02,-2.861331840614791608e-02,3.582779061346422211e-03,1.287811446631027638e-02,5.648633411654246927e-02,-3.702639057373878495e-02,3.369517980943632918e-02,4.543634328359900942e-02,-1.628002089999902036e-01,6.643736880191106545e-03,7.938795254358617381e-02,-7.334644312045297820e-02,5.301996510420027564e-02,8.483561986841570146e-02,-2.825716455232734486e-02,-4.248714193367404690e-02,1.174572657311578477e-01,7.792696959246026012e-02,2.050943633892863757e-02,9.088181219496248298e-02,4.615914742208496180e-02,-9.869849376053367596e-02,4.387212909376356218e-02,-8.305729540108934550e-02,4.765245091183771542e-02,-8.211786345566388090e-02,1.257203847384895656e-01,5.446168919525390222e-03,1.185457293544725371e-01,1.029603314405582760e-02,7.288714246210373227e-02,6.185232662732846609e-03,-3.374635400589488798e-02,-5.426000629724056501e-02,2.229736595057580095e-01,-2.576061844551606750e-03,7.216972333117677163e-02,-6.265173638835173919e-02,3.519224708716621464e-02,-9.025721228152931708e-02,1.368110744304769366e-02,-2.825110251057404848e-02,-4.874225029604453696e-02,-6.618442892781063042e-03,3.997898033118278682e-02,4.727604674613059693e-03,-5.632646673807997773e-02,2.529970421756734533e-02,-4.936362083032283521e-02,-2.682252143505442499e-02,-3.264470390110409820e-02,1.338366448546570581e-02,-5.619219996776218806e-03,2.848521225735006768e-03,1.442146622736068494e-02,2.714737570263742877e-02,3.418104001373104689e-02,5.604004976946177358e-02,6.758546643033488954e-02,-1.247364034796881460e-01,-7.256536802563054445e-02,-1.041725674137154896e-02,1.933084919081561048e-02,-5.194164044999566993e-02,-5.319906008005551024e-02,1.653605690101776771e-02,-8.779511947259015237e-02,-6.368775843478757803e-02,2.748687244667005758e-02,-1.551108514480901979e-01,6.351821382659397774e-03,-6.206987096342547749e-02,-1.887890670764451329e-02,-2.867134698999241650e-03,-3.687454058930644818e-02,-1.218335365201862752e-02,-4.195627982639349440e-02,-1.079865719273935681e-01,-4.839840876312642703e-02,6.046474637793129540e-03,-6.854259158775136834e-03,7.847308104330538714e-02,2.866404930153007916e-02,9.421868746282119345e-03,-5.435282068697305791e-02,1.691192189442464536e-03,1.478206818857601412e-01,-5.946870691740092918e-02,-5.336123841535343748e-02,6.360485211799721261e-02,3.291636091495636629e-02,6.163742804789220514e-02,-3.293127881267569196e-03,-6.491448723444530333e-03,-2.294450885521517533e-02,-6.427166019877807168e-02,-4.399103077712240850e-02,9.668763856321896388e-02,7.925846303310968288e-02,-4.296569592762933959e-02,-1.420522435610169221e-01,6.967497027844181401e-03,-6.860253612013972671e-03,-1.515609893642439789e-02,-3.973905625750336068e-02,1.191731855498412082e-02,-9.205255090932090711e-02,4.216347577626376047e-02,-9.011330959684777514e-03,-5.425688567324808698e-02,1.401403989226866938e-02,7.220650797382335839e-02,1.672781845203840864e-02,4.699101986077611724e-03,2.188757291179264236e-02,1.936811402137883151e-02,5.014741089331265118e-03,-1.331787027097182038e-02,-2.315823875555539368e-02,3.150946669525615113e-02,-7.983514369524737087e-02,1.817300104562081797e-01,3.290839508142912040e-02,1.826481113398049222e-02,-1.113243932513989509e-02,2.551602870147838195e-02,-3.180693704327774290e-02,1.898499125288651695e-02,-4.118281556238784918e-02,-4.036823430938220325e-02,1.000682365748237213e-02,7.573251994400568426e-03,8.216723997586131145e-02,3.852209226709152334e-02,5.544060101307368010e-02,-5.515202041279419765e-02,-8.471949519179995322e-03,-2.497371097113516847e-02,-4.739936132636652505e-02,2.845450568607160724e-02,-3.470696805635740956e-03,-2.104509975003956979e-04,-1.858604596459374411e-02,6.164177220248460987e-02,8.642969583197573913e-02,-4.226969546671210942e-02,1.003376788900411654e-01,-4.231314214480122460e-02,7.332511808957940802e-02,-1.313284822338989244e-01,7.911699042051305308e-02,1.018753738190216439e-02,8.032624133775707886e-03,-1.339606339466538343e-01,-2.654817517237196417e-02,-9.144385478053972810e-03,1.378612058896772595e-02,7.060461743986877481e-02,-8.884258944339470943e-02,-1.286486577585013724e-02,4.781134169572612280e-02,-9.841590016727515960e-03,3.444568182365840420e-02,1.547242046000953802e-03,2.974255182934814126e-02,2.862316198952528229e-02,2.255898906177399715e-02,-7.555181223031721960e-02,-1.359283263088375063e-02,-7.316523146686525203e-02,2.440713939590043874e-02,6.395946976074050327e-02,7.312844823185597753e-02,3.351764115777599445e-02,1.145785530559483983e-01,1.283783627346863909e-01,-2.139046439409132694e-02,-7.714093144009576825e-03,2.444571513606072499e-02,-5.709103642671572998e-02,-9.469673072218054433e-02,1.564900032323906023e-02,2.234203956195397051e-02,-2.454377836371234717e-02,9.536758206096003498e-02,-4.198132525357194111e-02,-5.625365145397652478e-02,-1.151090910816044734e-02,-1.023135101700413663e-01,-5.589774685079357958e-02,9.308650989771216633e-02,5.684259461952279047e-03,-3.744339876718968257e-02,5.997256825222778420e-03,-1.051030008246790076e-02,-9.047327890373700610e-02,4.536234491349529219e-02,7.239751100587331865e-03,-1.198328359302829743e-02,-8.184154651927755264e-02,-9.567746993274749145e-02,-1.565726622448422589e-02,7.024760034021079735e-02,4.462683176044383859e-02,7.091955903478290579e-02,-6.470680950936583131e-02,-1.270831424499505036e-03,-5.474583522922241152e-02,-5.391023761454245139e-02,-9.089218117451783319e-02,1.773440812282481641e-02,6.136978629195097984e-02,8.475042865456156038e-02,5.922627211373340805e-02,-1.642546083987227873e-02,1.972110717672835578e-01,-7.736957450201144629e-03,-1.350169767483223177e-02,-9.228549119660197503e-03,2.868341255047130930e-02,7.200866495792981881e-02,8.310967897378845892e-02,2.279366260825590323e-02,-7.575225658997378209e-02,-7.778861021832716993e-02,1.467077416762693765e-01,-1.196816910456163438e-02,-9.215452951800374992e-02,1.191276391445259819e-01,9.435763634364323013e-03,-4.860791770723425081e-02,4.451907953853206229e-02 -8.729639897169229334e-02,-5.903338888254412026e-02,-7.907268871814247813e-02,-1.160034018895889302e-04,-1.862907324622710067e-02,-4.030612344847806644e-02,3.047645931170880895e-02,5.800735154543232297e-02,-8.149510355599399319e-03,-2.947037098642368153e-02,-9.856798189191123827e-02,8.013999554895240296e-02,-6.527444977538408161e-03,2.938159709941876377e-02,-1.145416894861468748e-02,4.424911144448905737e-02,-9.893075227632035229e-02,-1.073466815178207440e-01,-8.345658730807405112e-02,1.010082177225293809e-01,6.849131799096244526e-03,6.676038973980373548e-02,-3.695717895602554370e-02,-6.325706395601247978e-02,3.900744369670176848e-02,7.683916121766637408e-04,-7.059536968417895464e-02,8.505103927718875301e-02,-7.045049092801407287e-02,1.900799655920203668e-01,-4.230293043795563951e-02,1.164363090020593128e-01,-7.846690320440621869e-02,5.895434560844155236e-02,3.219697268443443006e-02,1.768180144143131211e-02,-1.770543944496226485e-02,-1.023141662860411506e-02,-7.557257133651540393e-02,2.189210214560769613e-02,-2.352825384658370142e-02,6.639141371014965765e-02,-6.025516627297857614e-03,-1.091469144480296805e-01,-7.279726451909510933e-03,-1.741555672707692515e-01,-1.266264670421135899e-02,-2.058951940963830302e-02,5.292292052825361276e-02,-3.422324695780636084e-02,8.818449721520435491e-02,-1.163584063804118524e-01,7.006480171354803610e-02,5.276334498429254699e-02,-2.208990805160115228e-02,-6.886911472521566047e-02,1.209252908327710166e-02,-1.309500488848942650e-02,5.728359145139738651e-02,-1.010590281549697267e-01,8.031347703505133107e-02,6.936297200261271056e-02,-4.342299210649461227e-02,8.543923222978599963e-02,-9.118612557668509533e-02,-2.939440297606479499e-02,1.222055789412734850e-02,4.947554739530996004e-02,9.060885761858726573e-04,-6.318463356356089633e-02,-1.108589954234058678e-01,2.154143247113918941e-02,1.447612014902570632e-01,-7.478610083977852568e-03,9.781796751638875687e-02,-1.753608862743382540e-02,-9.103021799034957673e-02,-1.216364959880985863e-01,5.848408604155999613e-02,5.568348186463342037e-02,-4.753479299117404494e-02,5.620161858046916759e-02,-1.148212714233108958e-02,-3.707362284957681914e-02,5.374676809388862636e-02,3.696324963112763984e-02,-2.419463101405527200e-02,7.225341870967677896e-03,5.464356963890334984e-02,-2.528147163122491589e-02,-2.009914548826687020e-02,-9.688425225885005654e-03,9.793593162430759558e-03,-3.039525121973167196e-02,-3.389108310124100409e-02,-6.034876423062621176e-02,9.207998389156110686e-02,-5.939991738369895108e-02,4.116370408109290557e-02,5.553246264728175369e-02,1.947008938834215649e-02,3.384519749846684467e-02,1.122357204497879957e-02,-6.044695987839536488e-02,-4.900546366594166203e-02,1.008026516566361275e-01,-1.715168117313407015e-01,3.541703942668542049e-02,4.143641914552954192e-02,-4.902859559208269831e-03,-1.114649615684179529e-03,-4.564955948445272027e-02,-5.810275503156085708e-02,2.997257991547303685e-02,-1.738494433399395689e-02,-3.920858930285579802e-02,5.705614392286909031e-02,-1.530090680600328275e-02,3.372892442721382722e-05,8.261710003153879822e-02,-4.291266903036184549e-02,-4.311361674919553244e-02,-4.283921189410257180e-02,-1.038794165002867101e-02,-3.583801267229822646e-02,8.754737186581304509e-02,9.929293275436433530e-02,9.412713316607296266e-02,1.080250037306040023e-01,4.094961575504295348e-02,8.261140614359260836e-02,2.971589555709256217e-02,1.009679097648655266e-02,4.755686006277522193e-02,-5.608857151134877261e-02,5.919693666160776024e-03,-1.142708101707502755e-03,3.800709800104110775e-02,-6.587796080279580135e-02,-9.304832201902417388e-03,-2.871234176677579797e-02,-2.041579384524474725e-03,4.244816430444092042e-02,-1.582222754457641989e-01,-2.991546630335985973e-03,-4.931468082449031592e-04,5.359480418979978289e-02,9.471673781774644918e-02,9.110820825500227926e-02,4.541380288976382129e-02,1.851141073241013846e-02,-6.740709326843447036e-02,1.293664841351629402e-01,7.639768008278963185e-02,1.332767735800955214e-03,9.929248496125966938e-02,-8.462694597860050127e-02,-3.628572874332347542e-02,-3.735477071288448303e-02,6.091162344775684356e-02,-1.615373480955623053e-02,-9.726745408921411151e-02,-1.314690032257679296e-02,4.232290710330528782e-02,3.503176011560237696e-02,-8.862673493111201950e-02,-9.356874628388536785e-02,2.346595989619805478e-02,5.021043040129112567e-02,4.217878322931557095e-02,4.764467943158914170e-02,-2.363374925263680634e-02,-1.973076957754934385e-02,4.067935786430751377e-02,3.567150435258262427e-03,6.697450846765272414e-02,7.816260430962496120e-02,-5.778153765268754030e-03,-1.866290606162826580e-03,1.626684992115021156e-02,-4.490440117572137846e-03,-8.383878782969160504e-02,-6.690989712406791856e-02,4.518825597924688853e-02,1.263516484220782166e-02,-2.344482873238653078e-02,1.022468484536809274e-02,-8.018031365324636978e-03,-1.424432560977999598e-02,-8.328684111142571622e-02,-6.654661514767801211e-02,1.160185321069658465e-01,-3.571914714247913236e-02,7.008967417987534143e-02,2.444926534997109158e-02,2.353282138392612938e-02,2.384165334389009260e-03,1.406169495023563420e-01,8.509284300065469986e-02,-4.625771836528029535e-02,-2.333710763832419488e-02,4.532780163022952225e-02,-1.637869445168112573e-02,5.485449758713929758e-02,5.180600061579918036e-02,7.780783489358414351e-03,-3.030259174183502074e-02,6.745552762246620482e-02,4.785055724108596559e-02,1.114535254545941274e-02,-1.953301314839646363e-02,-1.202014293329914385e-02,-1.465584925227515593e-02,-8.384964407130192365e-03,-1.003234524605803313e-02,1.129491910579955016e-01,-1.602272103198714576e-02,1.276714662227332597e-01,-1.456689673346401107e-02,1.303470764593156472e-01,-1.235494146289871920e-01,-3.014550239371219936e-02,1.836042821905841185e-02,9.851608124324808674e-03,8.765852975583163231e-02,8.734106043850450862e-02,-8.976284056188792160e-02,-1.474923442845127095e-02,-1.269025785330931896e-02,1.793736275485571102e-02,-1.515106475792800216e-02,5.329130671160733912e-02,-7.433216003112606829e-02,3.314799240116445000e-02,1.214491153537944519e-02,-4.997392981939530188e-02,7.535946300854171054e-02,-2.399887323321131874e-02,-1.322115347562505608e-02,-2.407358296170926046e-02,9.164854902066853215e-02,7.256617684596167184e-02,1.420321306496050429e-01,3.869011673912912952e-02,8.394528885902062532e-02,1.045554004810669707e-01,5.547705121953062923e-02,-3.574150378518953231e-02,-9.869540939161371498e-03,-1.391037296604560325e-01,-1.187720658840252935e-02,7.493541689611843415e-02,2.903264376119150253e-02,2.037153868382152871e-02,-1.297825165247734613e-03,-5.205953567869775478e-02 --1.460654254456752454e-01,-5.704528875020848516e-02,-5.376492899490695893e-02,-8.795216002212946638e-02,-3.443361822467759514e-02,7.320344548017858333e-02,4.009015489209059868e-02,5.252410031187177558e-02,1.842308761220694258e-02,3.447244344123963355e-02,1.505968725900873328e-02,4.256437484910926411e-02,-5.760287008601290559e-02,2.343308723880965089e-02,1.103913635817835026e-01,8.043955438559895199e-03,-7.406939785104000445e-03,-1.249600211747135987e-02,3.167086262609668246e-02,-5.697484566016929736e-02,-6.971636623652456566e-02,-4.296909589349170999e-02,-8.340969337537103290e-02,-3.459858072097103665e-02,2.703544390450031767e-02,-2.774579715963990112e-02,1.110777348230086309e-01,6.605002337132778734e-02,1.777175440642669163e-02,8.039007020477788312e-02,-3.751205898646880338e-02,-1.824110234923072560e-01,3.420156077642752535e-02,-8.123750941385468605e-02,7.740443513253912611e-02,1.700853804336657088e-02,1.089907441917806023e-02,-3.841883093669583304e-02,6.945207288889047870e-02,-1.471054976570872717e-02,-6.035172116314751339e-03,3.182668006095150381e-02,7.525016298082182198e-02,-9.603165793133413425e-04,6.114928179858433732e-02,-2.296973759132163451e-01,-1.110949510384379585e-01,4.395361140618553319e-02,-6.893667889383905956e-02,8.105675798060717890e-03,5.910793698460477252e-02,6.077049702579796764e-02,1.196269200563545160e-01,-1.761335402254149765e-02,3.457846488822868825e-02,6.331732927686136748e-03,2.376857580560841343e-02,-3.462093400968605829e-03,-2.512439188535698503e-02,-9.264955328991877209e-03,-4.088899527825493041e-02,-2.979447448950731450e-02,6.191728920920634704e-02,-4.528074655164017470e-02,-9.554074846599945603e-02,2.147520807120262276e-02,-6.486727022531658948e-02,-5.474959669841389376e-02,-1.234104209449060491e-01,-1.739905939042186711e-01,-8.186947475269688163e-03,-6.851978593048504984e-02,-8.315654393994312277e-02,-3.413288582421275530e-02,-5.000199030153758967e-02,2.222515887441228050e-02,-1.507410462563925357e-02,-4.753521605175026546e-03,5.281394391666310861e-02,2.750282630840807443e-02,-6.071108024129645564e-02,2.920063422387349922e-02,5.387829951113155130e-02,-5.590891274089503094e-02,-3.593913840382248481e-02,-9.823606263824712082e-02,2.762668596926123876e-02,2.854936703946404225e-02,-2.592954201254840316e-02,-4.267378464620245199e-02,-1.443190497512238449e-02,-3.850984119994819987e-02,9.921437715151644465e-02,8.472574828688987647e-02,4.406553038026533170e-02,-1.050463358980841533e-01,-2.270094255225493102e-02,2.530368199339365007e-02,-3.595155755715206958e-02,-6.392652724146717647e-02,7.390316927189059110e-04,9.710804765818425999e-03,2.172125586753176352e-02,-4.177559682934855328e-03,-1.537680680369948524e-02,-8.327017474555473142e-02,2.378387800218370943e-02,-1.045644624659665400e-01,5.196280688348958038e-02,-7.166624392809402277e-02,1.260065387894528655e-01,6.593465074220747346e-03,2.446400865814544676e-02,3.124379249389941035e-03,4.155975414746766738e-02,-9.557627211035710224e-02,2.233856793897404103e-02,1.199760583518961685e-02,-6.802633911868151417e-02,-1.844521121838333527e-02,-5.787181729617583981e-02,7.586654020061367465e-02,1.118074048121061731e-01,-6.416396301821535031e-02,-8.117638890582776212e-02,-5.804571891800419658e-02,3.359880999920986100e-02,4.057207637887479124e-02,2.379059662820677545e-02,-3.402617323333301247e-02,-4.234302582628854394e-02,-1.537913644160920912e-01,-2.584594347144707205e-02,7.344254623276658123e-02,5.964754004667182502e-02,4.297969949836652481e-02,-1.341354026071461814e-01,-1.962321404318021975e-02,-1.422633384732480133e-02,-8.049457003715632109e-02,5.961981394542657797e-02,-6.137888704952763202e-02,-5.001611364061435416e-02,1.235115365180782313e-02,-2.244713618443611863e-02,-2.886772829743295549e-02,7.938127529282050510e-02,3.939991221666832438e-02,-1.672118398738233619e-02,-1.014648607979248482e-02,3.995508524390585514e-02,-7.767094847904578575e-03,-6.698403063572878902e-02,9.603538451054107264e-03,1.173602745068296066e-01,-4.032409066670929187e-02,3.729534366800091316e-02,-8.951724503624901644e-03,-3.900814595771866233e-02,-4.987496579653145912e-02,1.062840097417458024e-01,1.352396368836806043e-02,-5.673106621873995337e-02,-5.096586598431342152e-02,5.448833229222901919e-02,-2.870152303374199146e-02,4.021443322754447020e-02,1.835571580957992902e-02,5.224086726802823488e-02,9.864300613090140613e-02,-1.093066349557215849e-01,-3.731998003192481261e-03,-3.331214786595558530e-02,7.474611578348655527e-02,-2.113862982035233720e-02,8.190386940793072806e-03,1.763964792653492653e-02,-4.729238003617901309e-02,-9.955942385608696626e-03,7.619374623893979126e-02,4.247892102836882400e-02,2.009807725920973245e-02,5.427663945282200364e-02,-4.963272584098481210e-02,-6.966232742343447804e-02,-3.156755743463444952e-02,1.969638996148168758e-02,-3.994095996260067089e-02,9.085495360552790778e-02,-1.818138802859332048e-01,6.545898567737000462e-02,-9.806293856696973732e-02,1.054867790592849963e-01,2.296601466624140914e-02,2.153760272611629500e-02,-6.637306746744627006e-02,-1.120602699531921681e-01,1.998791229037402348e-02,1.272639162695072060e-02,-1.023522764808358909e-01,-6.663734559144184055e-02,1.101438017817048531e-01,3.670611367818669979e-02,4.670954153024705485e-02,4.669072449609533437e-02,3.321757805285320900e-03,-6.432346649978371023e-02,1.694548854736700749e-02,-4.014391383571423721e-03,5.012164429397971549e-03,1.871987705418677594e-02,3.606036815523347427e-02,1.899059192329973639e-02,-4.115118150874023939e-02,-3.835750168438042951e-02,-3.905944204353902632e-02,-4.693638673062347499e-02,-1.645634456142109139e-02,5.328604189835924043e-02,6.896814625538161270e-02,6.560442586733690462e-02,4.187336895987710944e-02,-7.195957215044426064e-03,-2.630525680689566048e-02,-2.638911677125510905e-02,1.002166209625491716e-01,2.574050373860052240e-02,-1.813002365322490678e-02,-7.218915264124235731e-03,2.308346117000542597e-02,5.356855667247306663e-03,1.279766420902630486e-01,1.830444309526994828e-02,6.369229277728832850e-02,7.383209294985213292e-03,-1.288981368024727514e-02,-2.428448243513936858e-02,-8.787517752885684250e-03,-1.007179290937890576e-01,4.590294495738879432e-02,6.976958886360271062e-02,3.845382040470123053e-02,-5.850957663695335875e-02,1.092538750257822378e-01,5.340997451209433811e-02,-4.151241993957386495e-02,-5.815109225315712865e-02,5.959047034071815463e-02,6.592765034996946744e-03,-1.474599803327760750e-01,3.938785324966007778e-02,-1.216520855082282582e-01,-2.123291716241179888e-03,-1.072882875920053575e-01,9.553858062620425307e-02,4.801408236630053117e-02 -4.586607307192350907e-03,5.800848403161504123e-03,-1.029083716662137632e-01,-7.003804951132171913e-03,-6.666736893647569329e-02,-3.869386943781639998e-02,4.794824458568237163e-02,3.480660088820645287e-02,9.083517139092575632e-02,-7.949562106499119540e-02,-7.162818659726413739e-02,-1.110064953318293246e-03,8.373954732193700234e-02,1.307134679678763900e-01,1.970232579339042944e-02,-3.717011411248632990e-02,2.085352576531546848e-03,-1.250906409545318454e-01,-4.620616123953474874e-02,-8.144965321957159388e-02,-2.606298990288390127e-02,-1.331495500853714375e-01,-1.264974251297991403e-04,4.188768148924751356e-02,3.184476943670026816e-02,2.298885478934012405e-02,2.532404957919725738e-02,-7.831812778609496817e-02,-1.159124411097223611e-01,7.532585203796871021e-02,2.346609717180611274e-02,1.437694512733754537e-02,-1.299793875409396143e-02,4.244518806820673368e-02,5.345544442754606579e-02,7.676006967126426661e-02,-1.793337233227241395e-02,-1.185882784597425410e-01,7.445296343137329520e-02,-4.568093837060675788e-02,-4.446002460335993223e-02,-1.119970160020703487e-01,2.919699182380028346e-02,9.237768175713903518e-02,1.591914631599948560e-02,1.336719521666509813e-01,-4.036525377821587046e-02,-6.016342038512634965e-02,1.621189532876893963e-02,3.175755647778319424e-02,-5.925429224895584213e-04,-4.933837131405510029e-02,2.404792728701068971e-02,-1.412232872311015158e-02,-4.188234548832566601e-02,8.126374275666734676e-03,1.224091848940819099e-02,4.294222448557296484e-02,-7.504054767402680570e-02,-8.261431347965331184e-02,1.258985457423923877e-02,-1.144000323946067071e-02,-3.683195014293468017e-02,9.687561376989994866e-02,-6.736661065281727978e-02,1.280980717314685879e-01,1.744774866313292955e-02,4.172573817247712813e-02,9.621430066503028344e-02,2.347818233851207276e-02,-5.849250001127392906e-02,-7.058274036913889105e-02,1.564856296926595697e-02,6.744085404245736548e-02,-3.780946859304610413e-03,1.794054708250110430e-02,-5.233418753861452527e-02,1.695002928023160021e-02,3.910187192888159996e-02,3.870392599707840608e-02,1.284249329069553358e-01,-6.348012884069893780e-03,-2.072649472946385893e-02,2.316466548433813305e-02,2.687711949739092498e-02,1.036944144955603136e-02,4.121024578063749166e-02,8.573317224816831561e-02,1.496840636880528153e-02,-7.244394857761383977e-02,6.263209121622473341e-02,-1.033277384582522232e-02,1.555880924029194937e-02,-8.081226014481554532e-02,-5.301808077997135354e-02,-8.719135758057983132e-02,-9.614527769508765531e-02,1.004121161901918580e-01,-5.778040721127324086e-02,-6.117038606912110488e-02,-2.337727100890189813e-02,-2.687335174357260992e-02,-6.944997391104149043e-02,7.307960373699751155e-02,-5.753201549455660979e-03,2.177985608746906693e-02,8.764246653258180031e-02,-3.913331159423819733e-02,-1.960014284990335004e-02,5.236966399582330262e-02,7.858647448703290350e-02,-1.048900029719726096e-01,3.514244931858864079e-02,-7.983834434333036967e-04,1.504094486713190328e-02,8.206150172252742181e-02,-6.580062725679064473e-02,1.448456716216254755e-02,8.218019074303280214e-02,-8.760798287820487085e-02,1.655979310211520492e-02,-7.579455074785608482e-02,5.642758712680746858e-02,-8.191575509984277970e-02,4.314718871670664840e-04,5.006474490640996228e-02,3.750962395713703518e-02,5.551751367483435951e-02,-1.367615692403045466e-03,7.067103717110596872e-02,-2.332203750346326099e-02,8.182343536399436235e-02,4.703969297153777485e-02,3.319694359502388392e-02,6.784704198321488133e-02,-3.608566880344021843e-02,1.380431471975917257e-01,-1.683035652646865804e-02,-1.076234726752287041e-01,-1.139985573310812739e-01,8.696514139215814898e-02,8.051472272190109136e-02,5.153200798845539166e-02,3.760542208481440496e-02,-4.669930460179128051e-02,1.005415069334996553e-01,-8.940441047738972857e-02,1.627999418169742002e-02,-7.774917019904521720e-02,1.352904137845719300e-02,2.099900242697875369e-02,-1.259937421259937629e-01,1.937713584520441359e-02,-8.406715587369088805e-02,-4.055722149032483723e-03,7.958244654648582994e-02,5.714350869952177642e-02,-8.576528877811349927e-02,-4.692063240697261911e-02,7.540761600269790810e-02,2.857538509534897353e-02,-4.184034943556669967e-03,-1.786482668597837858e-02,4.088028261183536405e-02,7.917526746059794229e-02,-2.102420584421143407e-02,-1.141403230366696275e-01,-1.607057412641794622e-02,-9.351114108658337143e-02,-4.404650844240274449e-03,-2.067965941049181577e-01,7.401745461453907576e-04,5.131394461594161738e-02,1.642796789749009778e-02,4.245125038940859158e-02,-4.302605752410218476e-02,9.138998567972493603e-02,-4.432560534478423991e-02,3.976779228951266704e-02,5.245873949147890347e-03,-1.337055548926025482e-02,9.323552132696876632e-03,4.850492539571295975e-02,-2.306234214994735618e-02,5.417809838573940662e-02,-6.366076705821262227e-02,-6.457369972161128291e-02,5.798011711191716766e-02,2.186774474080683053e-02,-3.802299166563957028e-02,1.314797060858603588e-02,-7.086345892233680138e-03,7.867918207484383974e-02,6.656430031672031823e-03,4.611266136297191565e-02,-2.996258468425006330e-02,5.070226697713164227e-02,-5.191485136795614963e-02,6.572788758293415801e-02,-4.408046129751733849e-02,3.491197515831877263e-02,-1.151576997797690155e-01,3.103241267750245225e-02,-6.743447344375826102e-02,-6.063562694264772873e-02,-7.680687811810685273e-02,7.975541887989130718e-02,-8.506419106445196887e-02,4.550109599064761437e-03,1.463730210299534623e-02,1.034727868879182638e-01,-5.358940007490019253e-02,6.782744975194039683e-02,-1.186575209969434747e-01,3.423105366024720847e-02,-1.115930712854610457e-01,-3.705246526803557261e-02,-7.426864677623372379e-02,-2.725418559337929730e-02,7.232854442913483339e-02,-1.397596931987789837e-01,-3.709181751346560252e-02,4.304378253648191710e-02,1.592943609427405899e-02,-1.664750135533416553e-01,-2.823702184894305578e-02,-6.780896438446180202e-03,3.213292078768845150e-03,-4.883095668657463917e-02,1.799957548376910493e-02,-2.065627598194250472e-02,6.977706245941700525e-02,-2.250517214673146069e-02,-7.105277641528379151e-02,-1.046942680978904180e-02,8.719434483148311155e-02,-3.374057014489242196e-02,1.770276314148389041e-02,-8.067808448544733224e-02,-9.350476835525226016e-02,-2.836220852560750530e-02,2.333828899600271303e-02,-7.312306163341136811e-03,7.352038144612452197e-03,-7.986446142912891066e-04,2.087612582488285531e-02,7.652113446738789682e-02,1.203462999411772372e-02,-5.863375414572873090e-03,-5.370818299005430951e-02,4.371732608729810704e-02,1.003427290524981919e-01,7.531649322326376683e-02,-5.502733608465156662e-02,7.759247429946240782e-02,6.159753786294161482e-02 --5.200949002895684931e-02,4.036255914348379353e-02,1.815018562691195492e-01,9.875602395253282872e-02,1.705716312883989033e-02,1.253990720491139077e-01,3.642529598998707485e-02,-3.917815645118571249e-02,-1.501946874789813341e-02,3.060531007373483436e-02,2.212192427554527177e-02,7.348978069693664836e-03,-2.362509429098112387e-02,-5.203185945489013225e-02,-5.658546431315568392e-02,-5.170439196394688991e-02,3.238906716256655105e-02,2.691339315114969072e-02,-1.104592202799875994e-01,3.962870229653199516e-03,3.883787499552149997e-02,-9.617142488460699024e-02,1.117833363509081884e-01,3.602169251304018077e-02,-8.546969283910698517e-03,3.782190717750653908e-02,4.278123130746378129e-03,-4.134363531104467970e-02,9.201636331231453914e-02,-9.357444132263360859e-02,-4.949201697695566893e-02,1.532389800818358995e-02,3.421879235723804258e-02,1.734334668343520403e-01,3.680416571820305932e-02,-1.878790367060425778e-02,-1.345705220007994800e-02,9.692037901897283814e-02,7.941512432006767042e-02,5.133505178363143712e-03,-9.383127887899585240e-03,-3.545568444681525266e-02,-4.553113523546019337e-02,-7.189585492877939576e-02,8.231757109718121357e-02,-3.382429123445915981e-03,-1.066517547728635801e-01,-8.821042248056867874e-02,5.000528085916442628e-02,-8.635029550192509229e-02,2.747743516119077745e-02,8.323940602834033198e-02,8.857502548412395094e-04,-2.115844363393649258e-02,-5.990499337480162390e-02,-2.103993660718396497e-02,6.117682259782999954e-02,-1.089912786851126303e-01,-4.692291345041977452e-02,-2.175908081388674281e-02,-4.567460933678731821e-02,-9.056850112453040946e-02,-1.437903493389873326e-02,-1.130844216033386612e-02,6.341198945059971226e-02,-6.588173762746007966e-02,-4.384270541564742885e-02,-5.073576303066144877e-02,-4.243194332787083439e-02,-4.148746111954585564e-02,1.148983832358369261e-01,2.130846672316309351e-02,1.507119767519367673e-02,8.700351526929341051e-02,1.818178009218525751e-02,-4.166860776159590080e-02,1.936455362210558542e-02,-3.708516355447512985e-02,6.142908227547598521e-02,4.227064875159344121e-02,2.549141462354896290e-02,-6.547527638536802808e-02,3.175016475223392878e-03,1.910190947203137604e-02,-8.743937344431733372e-03,1.638032308170026680e-02,9.716269252080196595e-02,6.950104628727049594e-02,-2.434624506673458560e-02,-4.529312002443462715e-02,9.079421127804225555e-02,-8.875529460962297557e-02,2.031861543942267700e-02,3.867627845831241723e-02,-1.152493864955258934e-01,-7.912053658662115962e-02,1.592492976913040953e-02,2.488028855538382961e-03,-1.392013903791204099e-01,8.851673020892311672e-02,2.584393510713903913e-02,-4.752327377399367947e-02,1.658917843501378220e-01,5.493201779311632921e-02,1.324635984416329126e-01,5.582892014763674721e-02,1.331149524124183858e-01,6.172260913722165537e-02,8.096706750664957319e-02,8.216707947967592796e-02,3.797437238781422447e-03,-1.606437548651387659e-02,-6.235060256072204321e-02,-7.828600947587599501e-02,1.184547263308731169e-01,-2.330253702675220973e-03,-7.732033598345407210e-04,-5.025719827963258390e-02,-6.780853475585515988e-02,1.446935311649161920e-02,7.095895740787397377e-02,-3.535330596540570136e-02,-1.665013320388071538e-01,-4.939105250942626285e-02,-6.793871420223998592e-02,1.338326449719529770e-02,4.429527713468049688e-02,3.614062337959533799e-02,-6.328265145069227049e-03,2.506652977586126471e-03,-6.615324317409904120e-02,1.442890096094337320e-02,5.010274105462364513e-02,-1.873212511670546351e-02,3.635197692895426186e-02,-5.766347059898153937e-02,2.973379107773386088e-03,6.292636497632415893e-02,2.003308006200237371e-02,-1.057927724402667330e-01,2.147069410348743934e-02,-2.370964008948715437e-02,-1.173305596548634838e-01,-4.748035037753693677e-02,5.048350606335896135e-02,2.883517502644058692e-02,6.619517189580256811e-02,-2.899254554548853433e-03,-3.701997389103013625e-02,7.858124154937631833e-02,3.701895391859268136e-02,2.449719007171417828e-02,-3.045660569887276986e-02,9.909248884943344660e-02,5.743393075817175564e-02,-4.983750470160272505e-02,8.322696103849556645e-03,3.836306755102490001e-02,5.231236904363470441e-02,9.079530059503114481e-02,6.846603539320722520e-02,2.074632211100759308e-02,3.787829532484558726e-02,-1.226959946856924638e-01,1.064416322257489764e-02,3.675145373248736580e-02,-9.780910878267072561e-02,7.141618410693763297e-02,2.863105325346874952e-02,-3.616408674180809218e-02,5.273520995403704198e-03,-1.385608968394281409e-03,2.358463833835439810e-02,-2.140320945266340272e-03,1.758779256620370685e-02,-1.328488159660184617e-02,8.807037464208435085e-02,3.307345962269660211e-02,-1.002620580439570752e-01,8.862089599159449216e-02,-1.997067070722090276e-02,9.949631703579779607e-02,2.368454910186702095e-02,1.591131946593131730e-01,3.041108794709153004e-02,-3.975690332336195842e-02,-5.868827110705032019e-04,-6.350505836994985165e-02,-7.138559799850488843e-03,-4.078970898736994366e-02,-7.324136339737608026e-02,5.197495638176204830e-02,1.023573733601137840e-02,3.334106196930872179e-02,-2.969834626297935057e-02,1.493429707462859968e-01,-1.075627836501881068e-01,4.235213143792591201e-02,7.058893677813636580e-02,6.948108296149183016e-03,-6.394326400536921862e-02,5.472197176607729308e-02,-7.998684068621837728e-02,-3.847391399156722264e-02,8.029391503668888153e-02,-2.131909434978679005e-02,3.985476471732747605e-02,6.513298522102790866e-02,-1.159556967203948658e-02,1.085617200636879720e-01,4.494275636403755048e-02,-5.448193572390090667e-02,-5.758080076190724705e-02,2.327304337951410090e-02,-1.002233683016180363e-02,3.492650459187948003e-02,3.396862756421027002e-02,8.504984053538009250e-03,-4.256559167487117268e-02,1.339644141139713016e-02,3.850680896494020761e-02,-6.303272682117453188e-03,8.729433603081533655e-02,1.027058044197548363e-02,-9.267199279146076663e-02,1.051823739704377432e-02,-4.003927028183299625e-02,-7.384854988586886382e-02,-4.203334720638570582e-02,1.474406455998369600e-02,2.999794088191035102e-02,-9.145806062990163854e-02,-1.419366548834045394e-01,4.445122707163819131e-02,-6.932957803520312101e-02,2.455926315134062884e-02,-9.376898798456081985e-02,-4.727492206546981407e-02,-5.812118971411075335e-02,-7.324018253593545316e-03,-1.622953910157017357e-02,-5.527371520402477034e-02,5.710814699387531390e-02,2.419592704645591105e-02,1.506366304692014282e-02,2.347534756653134672e-02,5.023020891121211806e-02,-9.549177737375506847e-02,-6.859816157234943645e-02,-3.825912739401282625e-02,3.055067241656133498e-02,1.909078150600522494e-02,1.061579481323340068e-01,4.460440173910463352e-02,1.234092435263488621e-02,3.735926278300659431e-02 -1.811829513868015717e-02,-2.214676580152697208e-02,1.175050032590219257e-01,-2.304867677774194781e-02,-1.162103931747490021e-01,-4.927200015276109180e-02,-1.990302899274273753e-03,1.090162032087404684e-02,7.258609333605692271e-02,1.283837949408898338e-01,1.432405686541221926e-02,3.973315817250706872e-02,2.760149607601719915e-02,2.592108203478195316e-02,-6.532179276560376446e-02,1.004431122911902674e-01,-4.108245082335729159e-02,-3.470672707906223564e-02,9.784417889057871187e-02,-8.451497715796388310e-02,-7.093931333613098800e-02,4.940967376920692850e-02,-2.289813731978989839e-02,5.096914989427787923e-02,-1.526195605524444110e-04,5.926109953085475363e-02,1.568376705186553741e-02,2.161916663733255348e-02,-2.273806867832483397e-02,-1.507400942250358700e-02,2.200344585800925040e-02,7.267540794146699901e-02,-7.749971094597629673e-02,-4.523672227221393893e-02,9.845250759721033140e-02,-5.685150157842475127e-03,-2.431862279315560611e-03,-2.218016230134418204e-02,-4.503829265111454483e-02,-6.487213483533538216e-02,-1.863708451224613954e-02,-1.098126580636234195e-01,-4.629468774546156157e-02,-3.073370172738315587e-03,-5.785862459708543404e-02,5.443096277145701573e-02,-3.257677263405368839e-02,1.718856949296032610e-02,-3.389928217952553613e-02,-3.748374492960844517e-02,-5.141115787230077228e-02,-1.297547940299637881e-01,2.867829492975414868e-02,-5.439996040751915451e-02,-9.571770551767333490e-02,-3.864893100909357060e-02,1.591457005286736962e-02,-3.435780962085175499e-02,-4.238733125717783490e-02,1.447269972522941157e-02,-4.886451964973495543e-02,-5.584231162619536504e-02,2.144801837057006694e-02,-4.936073940888249800e-02,1.067413652904318777e-01,-5.995750577838215856e-03,-2.662609891545071952e-03,2.687120451286735719e-03,3.125792139356003280e-02,-4.213964303364410580e-04,1.763013456208792698e-02,1.548615450119659315e-02,-3.308722527323249007e-02,-5.067326394633264730e-02,3.393985026678836975e-02,-1.126769221438635310e-01,-9.461079112542911118e-02,-2.828508985072587800e-02,8.633882335543850706e-02,-2.217206090507563981e-02,-4.519901497542315461e-02,5.318136505201605246e-02,-9.099468843831714018e-03,-1.100163013316770438e-01,-5.201200364838073215e-02,4.803010732495188728e-02,-1.425842065208423881e-01,1.190613059557145761e-02,1.118755988385064792e-01,-3.560415811842138588e-03,-3.896819838189684304e-02,-3.696412506278860810e-02,3.187649884194296290e-02,-4.018934002931323635e-04,2.239977824203849555e-02,1.720689504388710211e-02,6.542397011345373048e-02,4.466133780784313501e-02,-9.313304057928879776e-02,9.853131167834534854e-02,-2.911486682144364541e-02,-1.932277751181371572e-02,5.146479400091590051e-02,8.948390199066535988e-02,5.757283252503942600e-02,9.805683596054234896e-02,-2.214762123398225646e-02,5.151849820253796147e-02,5.641720702508068452e-04,6.916916748615839494e-02,-3.723848689029544967e-02,-6.049337108519037970e-02,-7.870693907632061309e-03,8.972954787547950117e-02,7.187656143983504886e-02,8.360926933839630051e-03,7.737110066399974229e-04,1.349663977632215017e-02,4.507938217394960589e-02,-1.040972746581614837e-02,3.932934557084491467e-02,6.846820581722548593e-02,5.334167223191748491e-02,2.507675681709453652e-02,-6.871188744021362871e-02,6.045798286027829690e-03,1.157080166911246370e-02,-1.984575280103888506e-01,-5.951338495871161260e-02,-1.447844638342531409e-01,2.005514635473593446e-02,-5.706107305814360486e-02,-7.870587158227548841e-02,-4.085690754248655354e-02,-1.647490914299081302e-02,1.458267722394673144e-02,-9.667740431188436012e-02,8.591815581256963263e-02,-1.059172561378622646e-01,3.030917424613831929e-02,-5.891975212543355128e-03,4.142587688814095698e-03,1.317827431884753819e-01,3.408309123832827664e-03,-8.539088931888338951e-02,-5.415175185652143508e-02,3.838063594766280562e-02,1.583731311441879119e-01,-4.756083475570078989e-02,2.276333788944243444e-02,4.209910435648608895e-02,-2.334622188411527091e-02,-7.359371663776542727e-02,5.026041613269356667e-02,1.920639477869018896e-01,6.538415253104440383e-02,7.641672446620034753e-03,-4.372324040104066251e-02,1.115379974951513642e-01,-1.379065348443658801e-01,-1.382523885320989243e-01,2.007737792143846067e-03,6.260877915522528037e-02,3.606287924627881586e-02,-5.737087869578092608e-02,6.697816828018039736e-02,4.914871222522546046e-02,6.247295850866071282e-02,-2.008993875546297159e-02,-7.908954825399060196e-02,-6.326863604533039531e-02,2.019246095013782841e-02,1.069348591080419714e-01,2.063614232585288949e-02,7.476121596014384529e-02,2.928424387214643293e-02,5.226959279674564529e-02,-9.453260425707021852e-02,9.405055928045126523e-02,1.342483626332162117e-01,-2.064091357881587402e-02,-3.448864944163973489e-02,1.073001990044259585e-02,9.452397383282146182e-02,2.508357736084225811e-02,6.146881791307260562e-02,-4.394020928218674710e-02,-6.108107607964196467e-02,3.206884287009508644e-02,1.201125255924763262e-02,-6.188534186456000341e-02,-5.389082968686123576e-02,-4.740333045348067015e-02,-2.702092324082890074e-02,3.585159988818074905e-02,-6.997757239518691275e-02,8.800044383888157296e-02,-2.730135322459614766e-02,-5.839543384532910886e-02,-1.322898177636916243e-02,-3.532366701963546643e-03,-1.100865633549859106e-02,-5.634391724328345619e-02,1.908966949387424714e-02,-2.812707088238995332e-03,-9.980156250628299808e-03,6.703732105262838470e-02,-1.088281296292532979e-01,-2.917539626754782345e-02,1.247017793281570974e-01,6.915557557563999438e-03,-8.305618605522108872e-03,-3.342500455650695979e-02,7.219173604129333621e-02,1.388871981370469744e-01,-2.287632424733918729e-02,-2.813790660099924415e-03,1.755196223711734393e-02,-9.504024996298529593e-02,1.189016647126390769e-01,4.210792006000784599e-02,6.391133818257130339e-02,-3.991399836587651573e-02,-3.125435358427770816e-02,-2.586470212226696722e-02,-1.339801646259593595e-02,-2.467026493712921181e-02,-3.888159946899707314e-02,-5.416658467631881718e-02,1.497973244356290770e-02,3.027948982333495201e-02,1.160531929178240662e-01,2.479651863795007127e-02,4.778647694179773142e-02,6.651356114152712218e-02,-7.956658765087763763e-02,1.177906889339653401e-02,-4.166716987371853198e-02,4.411046030371106041e-02,-1.791025682071887073e-02,-8.636041410776952587e-02,6.156834759453538863e-02,-3.332281128472692339e-02,8.910855529526595653e-02,-6.839035748069459408e-02,-4.528348178513628974e-02,-1.137394045453020766e-02,-1.246324772481205156e-01,3.371089980580591755e-02,-3.384185042385434022e-02,5.304161574344374436e-02,-5.426353248686964503e-02,-5.340082191886666441e-02,3.442979499960713979e-02,9.168777798185251751e-03,-4.617832212757458366e-02 -5.357557713125735144e-02,-4.499303546612750532e-02,1.070389771543856783e-01,4.053921380644016220e-02,-5.590767235998310436e-02,-1.226860782931880889e-01,-1.412405913677246549e-01,1.053075842008268093e-01,2.701912747178624591e-02,-2.279458696184516117e-02,6.079632008348659356e-02,-2.862539684576152071e-02,-5.408793605316800090e-02,1.103698344356473793e-01,1.167999348589185693e-01,-2.941385765427767529e-02,4.916363004544253845e-05,3.526431349676036053e-02,3.164141654437167828e-02,4.676444064415303387e-02,8.967781774780096693e-02,2.725133614242560703e-03,-2.509626321434348800e-02,-6.258725493693154118e-02,-2.250286553630360953e-02,2.317653407297184942e-02,-5.610940202124896359e-02,-1.562134642449376429e-03,4.044507382379847193e-02,1.182531004046899897e-01,-2.782180520242755889e-02,-4.232850880275044331e-02,1.378435181428896809e-02,1.371823102394449656e-01,-7.942823913405619440e-02,5.496526098699673624e-02,8.336795844373773301e-03,6.688249580911610759e-03,3.509782441454952634e-02,5.751203641701816555e-03,1.220417808018861225e-02,-6.449320767099009366e-02,-7.725261296253707610e-02,7.355522762292782124e-02,-6.994566563985056551e-02,-2.458810759825839215e-02,-1.519120713955697533e-02,2.674752457040435391e-02,3.771537220815421310e-02,5.051265805768580253e-02,7.617867355136724583e-02,5.209574979555538915e-02,1.395781440483628670e-02,-4.579723496569179653e-02,5.319833151166087487e-02,6.033569725125190186e-02,3.813115504589908999e-02,-3.843140410407211438e-02,4.261057349552689610e-02,5.969030971518101553e-02,9.597631626280061362e-03,-7.093375713186571285e-02,-2.679431875900713939e-02,2.050028245902739410e-02,-1.006051239722019833e-02,-1.746047386437048210e-02,-5.938526201486675482e-02,1.135631057778045683e-02,-3.048567377072067264e-02,3.256223918851480770e-02,4.093675571059424145e-02,-2.735115178429031735e-03,1.126514242672057436e-01,7.900592137827061948e-03,-1.445607123831060305e-02,6.608863424617847371e-02,5.837622359159518048e-02,4.458736646589835095e-02,-3.084757098888265409e-02,-1.027447304797815542e-02,-7.723370239773341174e-03,-1.134814919172099368e-01,-1.374794106255073679e-02,3.126491380774733264e-02,-9.603051604913188932e-02,-3.901516064505101966e-02,-5.533645644796233998e-04,-5.567127249955820811e-02,8.045492427892368681e-03,-5.034282845073094481e-02,6.738046603599388840e-02,-2.306399613232175752e-02,6.019435117846635985e-02,-7.070977517300541804e-02,-1.324892377508087027e-02,-3.003715092988187146e-02,2.582517435747999635e-05,-7.352083697571330789e-02,7.312161292887575659e-02,1.101553269867005247e-01,3.780130506772448606e-02,2.723047788744263620e-02,-7.794224049654521758e-02,-2.132792013275080362e-02,-8.322497790509948701e-02,-1.405486242475015128e-02,-2.353781169866754966e-02,-4.757888321618581468e-02,3.241718742622849703e-02,-5.576177513526315438e-02,6.226141192805716151e-02,6.763745965401202209e-02,2.870222224782462486e-02,-4.057809023171914803e-03,1.743881137074080323e-01,-7.672910434561254489e-02,-7.014773919930751556e-03,2.113851054787239600e-02,-1.995435466528470286e-02,3.696925511613312343e-02,1.156724416154135751e-02,8.225406229469996167e-03,-2.749705573424728391e-02,-1.324937936680802841e-01,1.332899534686555337e-02,-7.449309872425947943e-02,-1.310916870268666497e-02,-7.709496899726415997e-02,-1.719144529693687751e-02,1.074217359413763388e-02,4.635548559830719728e-02,3.871796599064765421e-02,-7.505568618813095172e-03,1.141566136281436661e-01,-8.869661533483770566e-02,-7.104699524126008303e-02,-1.141995138298866480e-01,-1.263423995302306174e-02,2.009881024528594601e-02,4.785553338642127563e-02,-1.249754662716587167e-01,8.253070716864777112e-02,-4.848400728266522713e-03,-2.220061209602163660e-02,-3.492373878740330506e-02,4.258161496297475923e-02,-7.340030991112342362e-03,-4.319566042572674930e-02,7.548473282008316687e-02,-1.164156531826581464e-01,4.220603587749645336e-02,6.490923435434052535e-02,-4.500660126570846242e-02,-2.979994990448938535e-02,4.281451248223163558e-02,-3.017523557316428851e-02,-2.513895722090167539e-02,-1.416520413196905859e-01,9.859783013998778267e-02,-5.786451208779443828e-02,1.489330413524758007e-02,6.200726533489150927e-02,1.074342888942163426e-01,3.343469402312868738e-03,-1.015602963777152701e-02,-7.179859259596912957e-02,5.382319831483883821e-02,-1.016878620311222892e-01,8.509962729947030680e-02,-8.268655717100285851e-02,-6.711583622176736652e-02,-1.252016042336520918e-01,-7.085487903119729036e-02,8.684340468078005637e-02,5.857995979061841191e-02,-7.675688558891420599e-02,7.837983934934923680e-02,-7.773473675930518689e-02,-1.083919992236913182e-01,9.474637092919365941e-03,1.624772437359123800e-01,-5.312731175594830740e-02,9.531663932166516140e-02,1.883597501230572721e-02,-4.015670536298193705e-02,1.002631127676019418e-03,2.361741113254910845e-02,1.230152296376026827e-01,1.541427739801811379e-02,-3.712879418082262739e-02,-5.496517022649996531e-02,-2.908934849178779897e-02,1.778956863630555552e-02,-6.094152229212645988e-02,-1.521208402791580580e-02,2.260924208187070107e-03,1.316698536566039690e-03,-2.055508365344135141e-02,7.175540941442131934e-03,1.162952400308681217e-01,-3.760169439918448209e-02,-4.190438551380347432e-02,-1.959249276101340115e-02,1.248555566068387662e-01,-3.262685363125789745e-02,3.862292334148058337e-02,5.077272600916097473e-02,-8.177545862162383106e-02,-1.053082754207296419e-02,-3.109510309857786703e-02,2.976407587324893245e-02,-5.617698981981621847e-02,-4.471944635535232693e-02,5.449970900744444124e-02,3.103537901209586433e-02,-6.838874935395006238e-02,1.940490050730383706e-02,7.973748109557154268e-03,-7.085850797877341067e-02,-2.396897896719373672e-02,-1.599684385612501134e-02,7.265179721252941480e-02,-8.207462817246630182e-03,3.499765923806517215e-02,-6.842318367128351342e-02,4.421889807946822321e-03,-3.160344739914501544e-02,1.643632795923711393e-03,-1.546041978209297674e-01,-2.368478751959471676e-02,-5.806128653593779776e-02,-2.042073412308748059e-04,-1.804454540303501026e-02,-1.819616673763874495e-02,1.517142455805847168e-01,2.776770658550335724e-02,-6.364937990880889518e-02,1.760879420573174467e-02,-5.320936576085433245e-02,-4.119106305178087124e-02,-1.358027571626322857e-02,9.810394804913569189e-02,6.763987716755825996e-02,-1.070725218708734189e-01,-2.071159162405827175e-02,3.488926206531615254e-02,2.790466914265682732e-02,-7.537198599400241408e-03,7.584097792769087243e-02,-2.906879792882270316e-02,-2.263237799854490584e-01,2.079256345319022842e-02,1.026796510979533644e-01,9.992790708459346749e-02,5.759934433605302939e-02,-1.464464070458091835e-02 -2.733433059677707177e-03,-2.273990325965860679e-03,1.015409021719703569e-01,6.680082148747186477e-02,6.811487882875358335e-02,-2.716047504158970380e-02,3.509510435745211154e-02,3.245762050361401807e-02,-4.875036849024968338e-02,1.718352822919762095e-04,6.218100111092369348e-02,-3.292353353330602916e-02,-1.031207648772976548e-01,4.708542157258160249e-02,-1.322504825368799597e-01,-7.792470705555304411e-03,1.307537969245388051e-02,6.809848946595188635e-02,4.318415045792487611e-02,-3.116763610093337133e-02,-3.595550168559231985e-02,3.670319286417267346e-02,9.247308965595057995e-03,-3.068226248564592459e-02,-4.721832346417859094e-02,3.188128368708536414e-02,-5.115552723921841516e-02,-5.625077874502507069e-02,-6.817307209844257476e-02,6.099351418382575885e-02,4.730711533580046757e-02,-1.564221870265165848e-01,4.716022537983319274e-02,-5.031739133221312804e-02,-1.960655043589155933e-02,-3.092934023685724870e-02,6.579077321226523001e-02,-5.051702152678756025e-02,8.207141681229354613e-02,-8.146104700188887271e-03,5.047627553260477379e-02,-3.749786925044535507e-02,-6.760949879173620836e-02,1.785939162851095732e-02,5.810816355351764405e-03,1.081273846471454969e-01,-6.778322044671027113e-03,-1.003841911697916089e-01,-9.535758943341189620e-02,-6.015392151086639688e-02,-1.468479467128159383e-02,-1.873338234326728940e-02,1.265420341051186337e-01,-3.525611688606933303e-02,5.764588902958324435e-02,-2.769173450204738540e-02,-6.011374352019568240e-02,2.779188910368321844e-02,-4.027299449462188458e-02,4.396019950583150010e-02,-4.368333292958777314e-03,-2.755362935636026904e-03,3.329868004715300783e-02,1.491180765931798130e-02,4.650239227422020560e-02,-1.333659335202501683e-01,-3.249057293428822546e-02,-5.064247092931619884e-03,-1.642769102535176010e-02,9.039712470852297177e-02,-9.723010823171987504e-02,4.063552404895968972e-02,-1.454310237026906483e-01,-1.438469408326518151e-01,1.357630043141889314e-02,-3.367315466870807877e-02,5.555493458289751524e-02,-7.785789374540169616e-02,-2.168240491018356281e-02,6.631641499994593569e-02,-9.029860976727543259e-03,3.933879828155184555e-03,-7.312431943797999268e-02,-4.813782217240096528e-02,-2.762928880837494869e-02,-1.674802320551589674e-02,1.701414734384814609e-01,-8.306777712456525631e-04,6.437390273117328532e-03,-4.932612389007549891e-02,-4.884574442235497388e-05,2.158473595318692595e-02,1.386804507249386753e-04,-1.041357347598750871e-01,-2.552915794563337046e-02,9.443425248331283051e-02,8.277029692624879309e-02,1.090048623137240408e-01,-2.574598371633497404e-02,-9.308273675398481828e-02,4.752477767478501501e-03,-2.927675438767399566e-02,-3.402044057363839968e-02,5.432124186822322176e-02,1.184924413682392035e-02,5.535967428970717599e-03,-4.672040674089182610e-02,6.995560328708413933e-02,-3.124671240765755642e-02,-1.376462804361321124e-02,-4.200530559813187015e-02,1.786789678560125882e-02,7.676212418344086075e-02,5.772227586460822180e-02,1.414304404037843715e-02,6.618282315996192391e-03,-4.072065138103157095e-02,1.091127787321749532e-02,-2.052431656402064347e-02,4.797491816424180600e-02,-2.919313448383913448e-02,-8.799287489117736671e-02,-2.258032927541199497e-02,-8.396882899573880621e-02,2.201264918846218555e-02,-2.696690398397199456e-02,-5.289171628937152855e-04,1.063414191876305842e-02,4.295729884199836479e-02,2.414372073798588467e-02,1.424507163582175162e-01,-4.933612560306231687e-02,3.016754088397604824e-02,-4.657655913377364149e-02,2.689038861799470520e-02,4.740553148682273565e-02,-5.211988417303231591e-02,7.027770504070314506e-02,-1.145759301237295885e-01,-8.151259187117909644e-02,6.942156877589884811e-02,3.948527607130898620e-02,6.861925549136706215e-02,9.588368727491594634e-03,-4.806807170442112254e-02,-3.980051358050734622e-02,3.450976923228902399e-02,-5.336327642390691940e-03,-7.022765301628541845e-02,2.063969234155898017e-02,-1.513863057784539494e-02,-7.960947730640335895e-04,3.684322779453540198e-02,3.553565650481457783e-02,-5.219881250442481857e-03,-5.999623081158717702e-02,2.065058724590868033e-03,3.444050560193838428e-02,7.235054308882983565e-02,7.774082483199500071e-02,6.006506856299183295e-02,-6.949207600817558506e-02,6.615366322747165406e-02,1.006640201452828204e-01,6.628273294402113602e-02,4.711585018916937539e-02,5.094834339866140260e-02,2.984642000344734161e-02,2.655089022746230480e-03,6.524919101581397318e-02,-3.167214937857577850e-02,3.103205909582483144e-02,-7.933183592665178641e-02,-5.615977877546099450e-02,-3.089200425354919101e-03,-2.646440973623269410e-02,1.333714728551214923e-01,6.716993698492296216e-02,-8.769249181905487689e-02,-6.579124275632688978e-02,4.826023976812494898e-02,-1.806572089244829205e-01,-7.046587502212185716e-02,-9.292065649840378083e-03,4.212821080160530397e-03,-5.959277780971563437e-02,-1.795834523788605863e-03,-4.541088090178545256e-02,1.362049558035180463e-02,3.708088916491691228e-02,-3.985950197796256367e-02,-4.831670844448569813e-03,1.775017940826009721e-02,1.316613531070086884e-01,-1.869816284926685904e-02,-7.267489765849514949e-02,6.808001067821473073e-02,4.077499875305458377e-02,5.906036692713100694e-02,5.169830783205021929e-02,1.333565358086461622e-03,2.033869759397490332e-02,1.637926852915802534e-02,-4.745595879603755479e-02,6.506507186546771349e-02,3.925034466188865451e-02,-2.237476320712606173e-02,1.745196266549996211e-02,8.742494226807265290e-02,-1.226980670804864865e-01,-6.014919714470507578e-02,1.133135420861362125e-01,-7.160847121198180010e-02,-6.416557680790370422e-02,-8.274292549150175785e-03,7.212982762504806244e-02,5.045761818302386265e-02,5.758149994292863921e-02,-4.126689835249795768e-02,-9.766055377194361731e-03,-2.767113841677487640e-02,6.008140270830091001e-02,8.545632281580101131e-02,9.584478355298313434e-02,-6.003701610323174814e-02,1.747049972411365837e-01,3.004173331605717451e-03,9.234986187054371187e-02,-2.029115494380166057e-02,-2.246582624225614278e-01,-1.435316366889221457e-02,-2.968566828315397782e-03,-2.916220645368767378e-02,-4.147942507590924449e-02,7.527470617649728579e-02,1.048085963770076512e-01,-1.560753493566848643e-03,2.786046841626155235e-02,5.605449418256533770e-03,-4.592943139914899137e-02,-2.419123088229664480e-03,-1.160520224891006258e-01,4.315835425278543441e-02,1.409705331975508180e-01,-3.892783039038416604e-02,-5.020583364922825975e-02,-7.482660203700945700e-02,2.022793350691357711e-02,1.744950580323167272e-02,2.161845769830544276e-03,1.538808479772263080e-02,-5.186495752724517433e-02,-3.620467586623263290e-02,1.342145468007479303e-01,7.795995953830769931e-02,8.432112055235932435e-02 -9.336939335619866276e-02,-3.815786664627442148e-02,4.588854383545321741e-02,-8.879604774450661209e-02,-3.245621922319028713e-02,1.584958195530308811e-02,-2.217190664226461065e-04,-8.404849504100876320e-03,2.204254798158562095e-02,1.481124035191438149e-01,-6.354311538796922516e-03,-5.515642252515336880e-02,6.622026108794841492e-02,4.222910691356855650e-02,2.498519792410578907e-02,1.060176770380340597e-03,-1.232680524030665969e-01,-2.383655270667986784e-02,2.411945826199464807e-02,-2.205158922074978989e-02,-1.370857826311973338e-01,-3.324038453794250086e-02,8.103744869037069268e-03,-7.170217266740686046e-02,-6.397680612618346363e-02,-6.301386336759674800e-02,-1.223288323210846434e-01,-1.688870718135260587e-02,6.438243036985558110e-02,3.119459338963047682e-02,3.052508986118520132e-02,7.938996125843159057e-02,2.786420564355976404e-03,-2.731444224731030199e-03,-1.600769402256274621e-02,-3.953433085610578990e-02,-9.103792440455152693e-03,-1.157142896566428908e-01,3.981716997519320345e-02,1.323774397804093894e-01,-2.908069864272464465e-03,7.563144848891352467e-02,1.135360001316931475e-02,-4.965051924754020074e-02,-1.617529665136833300e-02,-1.450612660130551623e-01,-5.575976536311190446e-02,6.456591231782921221e-03,3.052191647989888415e-03,1.428967304145857195e-02,1.117134683536840506e-03,-1.782201415749986098e-02,1.164706605660683902e-01,1.369253338708202594e-02,-8.167060824511154088e-03,-4.412323509267093019e-02,-7.428955443919381396e-03,-6.040884945151257096e-02,-8.073090666541386795e-02,8.557396257245820231e-03,-8.216642018344374326e-03,-7.842305133447835530e-02,2.592991509080562126e-02,-4.875500173961275219e-02,3.392694980703853808e-02,-8.160222471358347107e-02,-7.573549065127840874e-02,2.615564461943312158e-02,6.635916874519541542e-02,-1.053044486824409876e-01,-1.332241791583530129e-02,5.966599099953871777e-02,4.667957213385295900e-02,3.519410111966254223e-02,-8.002242039601759060e-03,-4.011922991267651006e-02,1.361529707355913776e-01,1.429981071188834996e-02,9.843359214119877520e-04,-1.032735928423572924e-01,3.254251611695476581e-02,-4.133640173424566605e-02,-3.099555180926855114e-02,-2.832010515755328112e-02,2.465108487486326189e-02,2.672226613387407995e-02,-5.224550721116030805e-02,3.035990774017474636e-02,-6.594932650848843447e-02,-1.117459075666453472e-02,-1.522561503375117684e-02,-1.846030148050924538e-02,-2.249429990789440087e-02,2.440393377998249097e-02,1.544873073517194148e-02,-1.159801035070746889e-01,1.821056550954269077e-02,9.980363044913402593e-02,-3.398412511844486200e-02,-1.339423873115717090e-01,2.200146106354639031e-02,-5.354423449954787140e-02,-9.937594178318283344e-02,5.366899005269711725e-02,2.528600340898943394e-03,-4.403726403857769617e-02,-5.656677445344875099e-02,3.999362332932879238e-02,5.466696725613473529e-02,2.535643187889741190e-02,-3.657047053221453362e-03,-6.713015967268705364e-02,4.083235670314452892e-02,2.462440087928324806e-02,-3.992995125552801366e-02,-4.712329170979365212e-02,-5.203009246308111686e-03,-3.261750376274966806e-03,-6.433886816667273509e-03,1.168905988343131030e-01,4.385772737714796476e-02,-7.520811891978987362e-02,-1.965918189948496347e-02,-4.066896401098323705e-02,7.519254809799641448e-02,-1.117615426437850584e-02,-3.907598325004903866e-02,-6.646103990017911134e-02,8.194256861979538897e-02,4.869459406333818963e-02,-9.359819740193907334e-02,2.173609902459527898e-02,7.453439338450355833e-02,-6.261757243094336067e-02,-3.742103337379436984e-02,1.140128310938170464e-01,4.615274272636375741e-02,-7.747250588160269069e-02,-1.462836488359780737e-02,-4.176058380084053667e-02,-6.172267957191179821e-02,5.610016231137177067e-02,1.333178923497815860e-02,-1.763823157303345129e-02,-1.166260530483034796e-03,7.420971282885248377e-02,-1.269508107280738762e-01,1.906274271194271369e-04,-6.185545061445363313e-04,-7.723763570608317697e-02,-1.531328621014360972e-02,-6.910257176261695722e-03,-5.042195923815536579e-02,-2.469546732984154863e-02,1.270834481155812441e-03,-9.768751704887368437e-02,2.313599587000725322e-02,-5.366609240540889947e-02,3.250764112140217682e-02,-9.114062214053926525e-03,-1.392162611245729154e-01,-6.439378434041341526e-02,-2.950055311960560200e-02,2.338263029279621930e-02,-1.396932901784130321e-01,1.242426838200475642e-03,-1.239491050196697697e-02,-2.311254136263274708e-02,-2.353245042129449571e-02,-1.634549632599941182e-01,3.680685365334650933e-02,2.966853366865602332e-02,-4.419341241085620459e-02,-3.932488384748276250e-03,1.311062044649118907e-01,-3.936369184262517029e-03,-8.320890715541687244e-02,3.742189201433789719e-02,-5.554469772998869154e-02,1.128074955475776159e-01,-1.021756464030136036e-01,-5.289721647832956036e-02,1.789415311632129232e-02,-4.211383004417042220e-02,-2.938839453765988707e-02,1.483395175361071627e-01,-5.441067491177070670e-02,6.161186526375714637e-03,-5.694514100352127128e-02,-7.585891329215897760e-02,8.345385968166432744e-02,1.694601328801007556e-01,1.174338603271938772e-02,1.295091749564620531e-01,-6.871056006216261225e-02,4.115887403337334383e-02,-2.128382590355849119e-03,-1.275705675009693001e-02,9.008933057700764524e-02,-2.120074738489997196e-02,1.459493194039575108e-02,9.447490291423933650e-02,7.005881903071653483e-02,-7.476576704407986163e-02,-3.786647106626417347e-02,3.744493256785281360e-02,1.027409845461310298e-03,-3.533354449078109777e-02,-2.371841040298336770e-02,3.215718491213635788e-02,2.891125537595924666e-02,-6.693080128478043345e-02,6.057622432519250977e-02,-6.482269610813513883e-02,-4.908987774370471291e-02,7.609591160718774672e-02,1.137358564876938746e-02,-3.162373565890658211e-02,1.226921046322428793e-02,-5.513240872656832714e-02,5.551713279139679907e-03,-5.770785209870711585e-04,1.192727961083898253e-01,2.548624677575813541e-02,-1.450865010558765045e-01,-5.303245570740847126e-02,7.992452983068761008e-02,-2.444064330122884196e-02,-7.073257145468715701e-02,3.000264324250895981e-02,-7.624854743065961893e-02,-8.058504130888870243e-02,-1.295278100020676865e-02,9.217547110049593984e-02,3.498616980118681580e-02,1.119869106662516323e-01,1.102865232856929667e-01,-2.302745795542983712e-02,-1.469422938617901547e-02,-1.117915885135518060e-02,6.530177393891475890e-02,-4.820995027699587904e-02,-6.507340447470599809e-02,-1.208398815666327474e-02,-2.308602144894989980e-02,-1.565704349626587388e-01,-1.097596104631071157e-01,1.288888826387896668e-02,-1.711342058088806059e-02,-1.446785793093070469e-02,-8.927490358105358292e-02,-4.811650281012704727e-02,-4.316766428690618079e-02,-5.096283629723857844e-02,-7.661700623315079173e-02,-7.798000872429568242e-02 -4.156785620910281864e-02,2.672407846034181855e-02,1.076647418391799699e-01,-6.707698339813919497e-02,-7.064830604309917106e-02,-1.485061018547813479e-02,-5.057309714142334267e-02,9.070779218392516913e-03,3.409386456748360328e-02,-6.243028963954814353e-02,-3.669629834479171127e-02,8.117929277163574153e-02,1.147120654097906617e-01,1.333260378619980646e-01,-5.570571309027461809e-02,-5.171831624995303772e-03,-1.532840952256953396e-02,2.841739228501426184e-02,-7.999961399683302998e-02,4.822281901247563868e-02,-3.193214156010824867e-02,-4.608986085004772304e-02,1.205448867948972075e-01,5.513082116386097686e-03,-3.494055323453555684e-02,-7.932311919331774819e-02,2.012085209961793353e-02,7.394321703612331820e-02,-4.532800303286806110e-02,-4.141050753519645578e-02,1.300818676119500777e-02,8.493845104006243762e-02,-1.170561526070344610e-01,-1.910079448796322271e-02,1.259790501057392320e-01,7.577408222085228739e-02,-5.887069551415737012e-02,-8.162963731733380995e-03,6.682048885616921619e-02,-1.628630795960675157e-02,-5.830687667829945187e-02,3.979363379793364464e-02,2.258647025898705141e-02,-3.786785585226075390e-02,1.885610769876094189e-02,4.520235891009784929e-02,-1.886481796051737991e-02,4.790371205308768116e-02,-1.071824454002465316e-01,7.432330246488334791e-02,-1.361023807460535751e-01,5.911797553022413165e-02,1.198557388091758130e-02,7.924387316820270810e-02,1.055071434351646242e-01,-2.181874040141193113e-02,-2.907858422332712175e-02,1.030676450753408024e-03,2.675954395634115926e-02,-2.255235423491125726e-02,-4.442920787258113069e-02,9.940345782631657134e-03,2.612102751758888647e-02,1.878079925388484320e-03,7.047256035677899877e-02,4.353065837588790399e-02,-7.540472024910974125e-02,1.541737481498582293e-02,3.167635964245055571e-03,-6.469967782055664751e-02,-7.135978973098578565e-03,-9.006034325465370194e-02,3.834749027171591162e-02,-3.357255080811445691e-02,-8.689141291180095539e-02,2.683770525980257171e-02,-1.062235745122882408e-01,-1.044357468963517596e-02,-4.906352214801729117e-02,7.604546583100139576e-03,4.466658727456448985e-02,-1.807716069516911855e-02,-2.372838680485211613e-02,1.122413795650693408e-01,1.765790455171756257e-02,1.003095401814588228e-01,-4.357374731347652547e-02,2.946506721358135156e-02,-8.876607144249905323e-03,-4.439588033902271136e-02,-4.921313280922516575e-02,-9.143732071468584077e-02,-1.224383937344101364e-02,1.078657903309648419e-01,-2.295671663293925163e-02,-7.871749288388470156e-02,7.077203427176916029e-02,-9.946687845190134436e-02,5.589766472613361820e-02,-9.131174955600800114e-02,-5.416697806022020140e-02,-1.286217873044505078e-01,9.335656361536090464e-02,-9.395695493484092220e-02,9.047806086019714172e-03,-5.879189922132129625e-02,3.212219697217751080e-02,-1.440877287696728723e-02,3.191678542677040681e-02,5.875641994445673921e-02,5.803100217947802786e-02,5.345565697925842158e-03,-5.986106764789466544e-02,-1.336585295416361507e-01,7.521166825879978257e-02,-3.383500939831296517e-02,-1.510991909511422149e-01,4.434655467062861151e-02,-2.707139351777862468e-02,6.937244240334287590e-03,1.776716984887860007e-01,-1.603685542841805714e-02,8.468007274865563261e-02,-9.360042512333130449e-02,-4.878411134613153816e-02,2.030517730508625204e-02,3.096890455144673746e-02,-2.461345512519777859e-02,-2.531229193432258523e-02,-3.257413816265027073e-02,1.676120649543330968e-02,2.921713723921219238e-02,-7.963033472471703345e-02,-8.796254746830402260e-02,-3.996954370984417276e-02,9.230029283903448678e-02,7.289583703970538633e-03,-1.583851681389767188e-02,-8.178246934565827631e-02,9.638232692016167336e-02,-3.500177472061193062e-02,2.219460824386535608e-02,-1.186529632029679898e-01,-5.142844521079126263e-02,-4.076229085102004573e-02,-2.526025308443918295e-03,-5.664834189001870940e-02,6.081270794474659402e-02,1.305133940999683717e-01,-5.923380099476632799e-02,-5.380401001799354349e-02,5.619408570944885162e-02,7.197212716351192308e-02,2.096381338908979985e-02,-8.416072064695255539e-02,6.485246470726137302e-02,7.113231548216066014e-02,1.400124023051299571e-02,-3.213369814198764607e-02,3.153455926423887751e-02,4.087419417954631096e-02,7.575126834086531868e-02,2.259104632676858446e-02,5.594064104066861359e-02,-1.243633800212606486e-02,1.855813045351929064e-02,7.753679752464996688e-02,-1.245640917454062802e-01,4.545652943315023342e-02,3.640289599159240996e-03,8.520920641450531116e-03,4.086592024925587291e-02,1.172952763329957598e-01,-2.507752497043991627e-02,2.789320680194014648e-02,3.255040530934691367e-02,-9.381013175639500712e-02,1.293838922625610843e-01,-9.104108129828521190e-02,2.350256268434595952e-03,4.078588398446375551e-02,2.210798096636757554e-02,8.063989650919252405e-02,-2.183361121693343926e-02,7.010910026557357067e-02,8.693639057223292999e-02,-5.605199069492834002e-02,5.397430393273821431e-02,-7.096356494822540772e-02,2.239806987201838767e-02,8.931761221363391026e-04,-9.968105125247568588e-02,-6.853209098998180870e-02,6.577169444793780351e-02,-7.843570983259157137e-02,-7.084903892937860492e-02,3.940954573721144188e-02,-2.323532724144031919e-02,-4.573931452222902705e-02,8.792975851073067672e-03,1.725634377642596209e-03,-5.241056103165101648e-02,2.312509405597962253e-02,2.606872833086872759e-02,2.838054736939099598e-02,4.994855312192158364e-02,-3.037928092689126669e-02,7.204094079442885379e-02,1.239501319297255932e-01,-4.847967124673084258e-03,2.294187206279547572e-02,7.628168316617037203e-02,-3.295468238302404901e-03,8.605474128291293323e-02,-3.727092162048099844e-02,-2.403723328168300838e-02,-9.868879507742671364e-03,3.897842892787183045e-02,-1.151963577872425898e-03,8.708904326197079715e-02,-6.306462825723624377e-02,-2.369971372164458856e-02,4.466633296892664334e-02,3.034029036069730428e-02,-2.729438926697486806e-03,1.727496124750186601e-01,9.481784573205618433e-03,7.641675794049642562e-03,3.790812316224243667e-02,-1.793550678635360829e-01,-2.542678108588412172e-02,2.507130610997839598e-02,-2.238032881136467131e-02,3.018875255197539520e-02,4.467455470356192226e-02,-2.543261024195129366e-02,-5.826745083701736355e-02,-2.394317910413635078e-03,-5.595812100845929532e-02,1.076908930794923669e-01,-1.405828449913154067e-03,9.262582008737222858e-03,3.645449043385756704e-02,-8.501533735027516636e-03,-1.134121096654051516e-02,-4.211229743679775761e-02,1.024253854841158939e-01,3.576080381972203404e-02,-6.245253476327664727e-02,4.215063242955974926e-02,7.550802137500356448e-02,-1.022480545100081656e-01,-2.243609943634811490e-02,4.385833388710601993e-02,3.319833280846395257e-02,2.293651810771702576e-02 diff --git a/models/hitnet_series/hitnet_1x240x320_model_float16_quant.onnx b/models/hitnet_series/hitnet_1x240x320_model_float16_quant.onnx new file mode 100644 index 00000000..2f494d23 Binary files /dev/null and b/models/hitnet_series/hitnet_1x240x320_model_float16_quant.onnx differ diff --git a/models/hitnet_series/hitnet_1x240x320_model_float16_quant_opt.onnx b/models/hitnet_series/hitnet_1x240x320_model_float16_quant_opt.onnx new file mode 100644 index 00000000..17fa720e Binary files /dev/null and b/models/hitnet_series/hitnet_1x240x320_model_float16_quant_opt.onnx differ diff --git a/models/hitnet_series/hitnet_1x240x320_model_float16_quant_opt.trt b/models/hitnet_series/hitnet_1x240x320_model_float16_quant_opt.trt new file mode 100644 index 00000000..420da54f Binary files /dev/null and b/models/hitnet_series/hitnet_1x240x320_model_float16_quant_opt.trt differ diff --git a/models/hitnet_series/hitnet_1x240x320_model_float32.onnx b/models/hitnet_series/hitnet_1x240x320_model_float32.onnx new file mode 100644 index 00000000..d480da87 Binary files /dev/null and b/models/hitnet_series/hitnet_1x240x320_model_float32.onnx differ diff --git a/models/hitnet_series/hitnet_1x240x320_model_float32_opt.onnx b/models/hitnet_series/hitnet_1x240x320_model_float32_opt.onnx new file mode 100644 index 00000000..14901026 Binary files /dev/null and b/models/hitnet_series/hitnet_1x240x320_model_float32_opt.onnx differ diff --git a/models/hitnet_series/hitnet_model_240x320_fp16_quant_opt.onnx b/models/hitnet_series/hitnet_model_240x320_fp16_quant_opt.onnx new file mode 100644 index 00000000..bd2a1bc9 Binary files /dev/null and b/models/hitnet_series/hitnet_model_240x320_fp16_quant_opt.onnx differ diff --git a/models/hitnet_series/hitnet_model_240x320_fp16_quant_opt.trt b/models/hitnet_series/hitnet_model_240x320_fp16_quant_opt.trt new file mode 100644 index 00000000..62e2d306 Binary files /dev/null and b/models/hitnet_series/hitnet_model_240x320_fp16_quant_opt.trt differ diff --git a/models/mean_.csv b/models/mean_.csv deleted file mode 100644 index 9568bed0..00000000 --- a/models/mean_.csv +++ /dev/null @@ -1,256 +0,0 @@ --3.972420456880055434e-03 --1.189853570187697149e-02 --6.216613020676394798e-03 -7.434216427636865590e-03 --1.147791257491159039e-02 --1.310975002568145857e-02 --2.969533102688735882e-03 -6.227360818848870147e-03 -1.552777655818699323e-02 --1.975923049278710975e-03 --8.765487908035841588e-03 --6.558365782943642239e-03 -1.227759593174985488e-02 --1.466019825083964868e-02 --6.101331636092653145e-03 -3.899847726318086960e-03 -1.588138004879456958e-02 --4.052143058241252337e-03 --2.045261708701638995e-02 --6.043767114899238797e-03 -7.835544684494884227e-03 --4.261883809691723556e-03 -8.463711496391827446e-03 --6.377619336916715183e-03 -5.123942077867192114e-03 -9.634211921913783325e-03 -5.557588342736520179e-03 --7.921551140451756762e-03 -5.909340070307444667e-04 --2.948125678444912020e-03 --1.428854938049669209e-03 --3.584174887603770732e-03 --1.513007812372225459e-02 --8.358545222827045221e-03 --6.066430590973602779e-03 -5.857381452407793565e-03 -8.194329449365527623e-03 --6.040757006030313299e-03 --5.407608774134087075e-03 --2.615336216329978286e-02 -3.176443366505545029e-04 -8.364623430723758593e-03 -9.049799770156678747e-03 -2.754187373100775056e-03 -1.099465249652265865e-02 -4.725330401766904276e-03 --1.387181056149354094e-02 --2.855530083615289826e-03 -1.710536718525191733e-03 --1.726636485656283995e-03 -1.893548544794760626e-03 --2.666765273122538235e-03 -1.396065729775063970e-02 -3.471211788590936455e-04 --1.772978183981931480e-02 -3.152652660089308879e-03 -1.533595738221681451e-02 -3.914368191673057301e-03 -9.741692558962275483e-03 -1.006276415549362327e-02 --1.841027186971826548e-03 -7.863211113339886246e-03 -5.881441212874463989e-03 --5.626402216347475491e-03 --4.109574541898563735e-03 --4.990200061498910322e-03 --9.083144093806387598e-03 --8.233539991262174562e-03 --5.543951117997464317e-03 --6.718543751579239380e-03 --7.712090395507241979e-03 --1.171435993980445879e-02 -8.048593326297598036e-03 -1.842223663907392034e-02 --1.345942797175925893e-03 --1.353425416968458833e-02 -1.052228749396845291e-02 --6.869050861562767986e-03 --8.701509070132352249e-04 -1.759405359164302809e-02 -1.031895647795471639e-02 -5.977887775567396200e-03 -1.191550001291636490e-03 -4.731231946489311210e-03 -1.523625773727761180e-02 -7.068495110109777359e-03 --4.731699545693240395e-03 --4.315981716579413846e-03 --5.117836967213706435e-03 --6.501904235640475205e-03 --2.011392862227362466e-02 --1.727144912508112810e-03 -1.159879157042008509e-02 --4.209749950903516995e-03 --3.418644690291164703e-03 -1.350908056377361256e-02 --5.611547709008572055e-03 --2.200109890546879041e-02 -1.432603331495337778e-02 --9.438711950690879121e-03 -8.899123437658187571e-03 -8.077060963352183237e-03 --6.461144413403058576e-03 --5.169994295104749470e-03 -8.789886770025628774e-04 --1.804556804669489806e-03 -1.146521473757197360e-02 --1.312559241858067723e-02 -1.757189858737747204e-02 --3.620947812331346081e-03 --2.039874438634717291e-02 -2.207829816345853891e-03 -9.718925850900464458e-03 --6.280576715974813679e-03 --1.236443271118955849e-02 -7.197216954923561436e-03 -4.463688798402235328e-03 -1.570570934625068610e-02 --4.622102568523043208e-03 --7.512334646339464440e-03 -1.886716648238268706e-02 -1.304560894890529135e-02 -6.190998429703155293e-04 -5.929886154798392518e-03 -4.216211199879821986e-03 -1.239517625674537923e-02 --1.111429147175239419e-02 -6.438331881094758180e-03 --1.134100977735270736e-02 -8.649820552969606641e-03 --7.073306619546975912e-03 --2.717326107216147795e-04 --1.863090637466571255e-03 -2.302250052305769703e-05 --2.172869702424252286e-02 -6.986716463928671107e-03 --2.756658936175076634e-03 --2.342750055345552487e-02 --4.455245399432352683e-03 -7.154197783476572842e-03 -1.138394022140863659e-02 -1.429933011873657164e-02 --9.347819653496460338e-03 --7.538613898432985558e-03 -1.044811033164433703e-02 --2.029208641222332576e-02 -2.852028459745401413e-03 --3.671026410968283686e-03 -8.463000020649640021e-03 -5.398676106964117073e-04 --5.002903798367693521e-03 --7.313814736406449787e-03 -7.529081642390986727e-03 -1.051321973607222994e-02 -1.868698867562353571e-03 --9.399454160109302131e-04 -1.366576207711539710e-02 --4.496428301479951477e-03 -1.032078294436296340e-02 --5.350812468509350169e-03 --4.111678390918011299e-04 --3.655399595705121230e-03 --1.385049068271133020e-02 --3.506507668961501064e-03 -1.250878632493090080e-02 --2.588639147448339346e-03 -5.324895408468390684e-03 --6.829359897689468023e-03 --7.378611784636663132e-03 -1.378897030822290488e-02 --7.284703471003847047e-03 -2.135600496134500755e-03 -1.938893434811496397e-02 --2.788875504087448207e-02 --2.215142773558072872e-03 --2.291906357541883404e-02 --1.157614184465421405e-03 -4.249793380186601792e-03 --5.982932309026459637e-03 --4.067382964659698071e-03 --7.010938845179739975e-03 -1.017579535491506051e-02 --1.126416632053338016e-02 -1.134193514700810625e-02 -1.019425485405679385e-02 -4.024070416345846071e-03 --9.714252212682744289e-03 -3.964797255381753878e-03 -9.623338071319382202e-04 -5.965225659393676811e-04 --2.874376820577296317e-03 --3.277790779377900369e-03 --1.348663670643559555e-02 -4.247880850952610345e-03 -2.187637240313234158e-02 -2.033086738037568914e-03 --1.424159848496617080e-02 -3.206026144961680657e-05 --2.886092253874902695e-03 -2.550257099495084698e-03 --6.670005435863806537e-03 -1.168368685879583738e-03 --4.807002114985914343e-03 -1.584601141619685488e-02 --1.398540596337057886e-02 -4.421178370249362853e-03 --1.859385002110872445e-02 -4.861602303567915187e-03 --1.091629847269894732e-02 -1.773473168221077496e-03 -6.698796269759220955e-03 -1.878110290141596361e-02 --8.013174807513819345e-03 -1.947829750832752839e-03 -4.667635899480767486e-03 -5.477937346543961493e-03 --8.911275395623370621e-03 --9.114469819166875000e-03 --2.858957906511949785e-03 --1.166607418146539329e-02 -7.838791263758330616e-03 --1.375631125445770167e-02 --1.724183879677371883e-02 --7.304488447058069857e-03 --2.926008969709650701e-04 --8.650121979545318321e-03 -1.533422410977918447e-02 --9.599851104292502976e-03 -1.694254470776707100e-02 --1.160157042993979065e-02 -8.193850297093074886e-03 -3.893242712242958939e-03 -1.678853258434974405e-02 --1.169692085002056736e-02 -5.171458285102884748e-04 --5.024714886321064722e-03 -8.191868198949566610e-03 --2.260418900411602199e-03 -9.688089337292730627e-03 --5.755073003034595451e-03 -4.671408777963274904e-04 -1.913111709388124740e-03 -7.195600121738968645e-03 --3.572546732613457975e-03 --2.653565190634513040e-03 --4.329864485487190488e-03 --1.871565784569307819e-02 -5.626156363568331036e-03 -9.721249193965426569e-03 -1.601941050318102071e-03 --1.067255059358967854e-02 -5.737753562242061886e-03 --1.292724681233348681e-02 --4.383120722186195401e-04 --1.848665156266822082e-02 -2.499795497857102908e-02 diff --git a/models/netvlad_series/mobilenetvlad_dyn_size.onnx b/models/netvlad_series/mobilenetvlad_dyn_size.onnx new file mode 100644 index 00000000..dda0020f Binary files /dev/null and b/models/netvlad_series/mobilenetvlad_dyn_size.onnx differ diff --git a/models/superpoint_series/superpoint_v1_dyn_size.onnx b/models/superpoint_series/superpoint_v1_dyn_size.onnx new file mode 100644 index 00000000..0d2bc279 Binary files /dev/null and b/models/superpoint_series/superpoint_v1_dyn_size.onnx differ diff --git a/models/superpoint_series/superpoint_v1_sim_int32.onnx b/models/superpoint_series/superpoint_v1_sim_int32.onnx new file mode 100755 index 00000000..7db4473a Binary files /dev/null and b/models/superpoint_series/superpoint_v1_sim_int32.onnx differ diff --git a/quadcam_depth_est/.vscode/c_cpp_properties.json b/quadcam_depth_est/.vscode/c_cpp_properties.json index 252a72e7..0cd52fc3 100644 --- a/quadcam_depth_est/.vscode/c_cpp_properties.json +++ b/quadcam_depth_est/.vscode/c_cpp_properties.json @@ -4,16 +4,16 @@ "name": "Linux", "includePath": [ "${workspaceFolder}/**", - "/home/xuhao/d2slam_ws/src/D2SLAM/d2common/include", "/usr/include", "/opt/ros/noetic/include", - "/home/xuhao/d2slam_ws/src/swarm_msgs/swarm_msgs/include", "/usr/local/include/eigen3", "/usr/local/include/opencv4", "/usr/local/include", "/usr/include/pcl-1.10", - "/home/xuhao/d2slam_ws/src/D2SLAM/camera_models/include", - "/home/xuhao/d2slam_ws/src/D2SLAM/d2frontend/include" + "/root/swarm_ws/src/D2SLAM/camera_models/include", + "/root/swarm_ws/src/D2SLAM/d2frontend/include", + "../../d2frontend/include/**", + "../../d2common/include/**" ], "defines": [], "compilerPath": "/opt/gcc-arm-none-eabi-9-2020-q2-update/bin/arm-none-eabi-gcc", diff --git a/quadcam_depth_est/.vscode/launch.json b/quadcam_depth_est/.vscode/launch.json index 6c068eee..60dfa71a 100644 --- a/quadcam_depth_est/.vscode/launch.json +++ b/quadcam_depth_est/.vscode/launch.json @@ -1,13 +1,10 @@ { - // 使用 IntelliSense 了解相关属性。 - // 悬停以查看现有属性的描述。 - // 欲了解更多信息，请访问: https://go.microsoft.com/fwlink/?linkid=830387 "version": "0.2.0", "configurations": [{ "name": "(gdb) quadcam_depth_est_test", "type": "cppdbg", "request": "launch", - "program": "/home/xuhao/d2slam_ws/devel/lib/quadcam_depth_est/quadcam_depth_est_test", + "program": "/root/swarm_ws/devel/lib/quadcam_depth_est/quadcam_depth_est_test", "MIMode": "gdb", "cwd": "${workspaceFolder}", "args": ["--calib", "~/d2slam_ws/src/D2SLAM/config/quadcam/quad_cam_calib-camchain-imucam.yaml", @@ -15,7 +12,7 @@ "--name0", "cam3", "--name1", "cam2", "-l", "/home/xuhao/output/quadvins-output/imgs/fisheye_000900_3.jpg", "-r", "/home/xuhao/output/quadvins-output/imgs/fisheye_000900_2.jpg", - "--engine", "/home/xuhao/d2slam_ws/src/D2SLAM/quadcam_depth_est/models/model_float32.onnx"], + "--engine", "/root/swarm_ws/src/D2SLAM/quadcam_depth_est/models/model_float32.onnx"], "setupCommands": [ { "description": "Enable pretty-printing for gdb", @@ -28,7 +25,7 @@ "name": "(gdb) quadcam_depth_est_node", "type": "cppdbg", "request": "launch", - "program": "/home/xuhao/d2slam_ws/devel/lib/quadcam_depth_est/quadcam_depth_est_node", + "program": "/root/swarm_ws/devel/lib/quadcam_depth_est/quadcam_depth_est_node", "MIMode": "gdb", "cwd": "${workspaceFolder}", "setupCommands": [ diff --git a/quadcam_depth_est/CMakeLists.txt b/quadcam_depth_est/CMakeLists.txt index 2a2e62f6..1e314371 100644 --- a/quadcam_depth_est/CMakeLists.txt +++ b/quadcam_depth_est/CMakeLists.txt @@ -10,10 +10,11 @@ add_compile_options(-Wno-deprecated-declarations -Wno-reorder -Wno-format -Wno- find_package(catkin REQUIRED) SET("OpenCV_DIR" "/usr/local/share/OpenCV/") +SET("TENSORRT_UTILS_INC" "../tensorrt_utils/include/") find_package(OpenCV REQUIRED) find_package(Eigen3 REQUIRED) find_package(yaml-cpp REQUIRED) - +find_package(CUDA REQUIRED) find_package(d2frontend REQUIRED) find_package(catkin REQUIRED COMPONENTS roscpp @@ -30,11 +31,6 @@ find_package(catkin REQUIRED COMPONENTS pcl_ros ) -set(ONNXRUNTIME_LIB_DIR "/home/xuhao/source/onnxruntime-linux-x64-gpu-1.12.1/lib/" CACHE STRING "Path of ONNXRUNTIME_LIB_DIR") -set(ONNXRUNTIME_INC_DIR "/home/xuhao/source/onnxruntime-linux-x64-gpu-1.12.1/include/" CACHE STRING "Path of ONNXRUNTIME_INC_DIR") - -add_definitions("-D USE_ONNX") - catkin_package( # INCLUDE_DIRS include # LIBRARIES quadcam_depth_est @@ -42,41 +38,59 @@ catkin_package( # DEPENDS system_lib ) -link_directories(${ONNXRUNTIME_LIB_DIR}) - include_directories( # include +${CMAKE_CURRENT_SOURCE_DIR}/include +${TENSORRT_UTILS_INC} ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ${d2frontend_INCLUDE_DIRS} ${YAML_CPP_INCLUDE_DIRS} ${ONNXRUNTIME_INC_DIR} +${CUDA_INCLUDE_DIRS} ) +add_subdirectory(../tensorrt_utils tensorrt_utils) + ## Declare a C++ library add_library(${PROJECT_NAME} - src/quadcam_depth_est.cpp src/virtual_stereo.cpp + src/hitnet.cpp + src/quadcam_depth_est_trt.cpp ) target_link_libraries( ${PROJECT_NAME} ${catkin_LIBRARIES} ${d2frontend_LIBRARIES} + ${CUDA_LIBRARIES} onnxruntime + nvinfer + nvinfer_plugin + nvonnxparser + tensorrt_utils + yaml-cpp ) +add_definitions("-Wall -g") + add_executable(${PROJECT_NAME}_node src/quadcam_depth_est_node.cpp) -add_executable(${PROJECT_NAME}_test src/quadcam_depth_est.cpp test/test_quadcam_depth_est.cpp) + target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES} ${PROJECT_NAME} -) -target_link_libraries(${PROJECT_NAME}_test - ${catkin_LIBRARIES} - ${YAML_CPP_LIBRARIES} - ${PROJECT_NAME} - ${d2frontend_LIBRARIES} - onnxruntime + ${CUDA_LIBRARIES} + nvinfer + nvinfer_plugin + nvonnxparser + tensorrt_utils ) +# add_executable(${PROJECT_NAME}_test src/quadcam_depth_est.cpp test/test_quadcam_depth_est.cpp) +# target_link_libraries(${PROJECT_NAME}_test +# ${catkin_LIBRARIES} +# ${YAML_CPP_LIBRARIES} +# ${PROJECT_NAME} +# ${d2frontend_LIBRARIES} +# onnxruntime +# ) diff --git a/quadcam_depth_est/include/hitnet.hpp b/quadcam_depth_est/include/hitnet.hpp new file mode 100644 index 00000000..a48d36b3 --- /dev/null +++ b/quadcam_depth_est/include/hitnet.hpp @@ -0,0 +1,82 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "tensorrt_utils/buffers.h" + +constexpr char kInputTensorName[] = "input"; +constexpr char kOutputTensorName[] = "reference_output_disparity"; + +namespace TensorRTHitnet{ + +class HitnetLogger: public nvinfer1::ILogger{ + void log(Severity severity, const char* msg) noexcept override{ + if(severity <= Severity::kWARNING){ + std::cout << msg < engine_ptr, + std::string input_tensor_name = "input", + std::string output_tensor_name = "output"); + int32_t setInputImages(const cv::Mat& input); + int32_t doInference(); + int32_t copyBack(); + int32_t synchronize(); + int32_t getOutput(cv::Mat& output); + + private: + std::shared_ptr engine_ptr_ = nullptr; + nvinfer1::IExecutionContext* nv_context_ptr_ = nullptr; + std::shared_ptr buffer_manager_ptr_; //TODO: risky + cudaStream_t stream_; + std::string input_tensor_name_ = "input"; + std::string output_tensor_name_ = "output"; + int32_t input_size_ = 0; + int32_t output_size_ = 0; + int32_t input_index_ = -1; + int32_t output_index_ = -1; +}; + +class HitnetTrt{ + public: + HitnetTrt(bool show_info):show_info_(show_info){}; + ~HitnetTrt(); + int32_t init(const std::string& onnx_model_path, const std::string& trt_engine_path, int32_t stream_number); + int32_t doInference(const cv::Mat input[4]);//input 4 1x2x240x320 bcwh output 4 1x1x240x320 disparity + int32_t getOutput(cv::Mat output[4]); + private: + int32_t deserializeEngine(const std::string& trt_engine_path); + int32_t buildEngine(const std::string& onnx_model_path, const std::string& trt_engine_path); + + int32_t stream_number_ = 4; + std::vector executors_; + std::shared_ptr nv_engine_ptr_ = nullptr; + HitnetLogger logger_; + bool show_info_; +}; + + + +} + diff --git a/quadcam_depth_est/src/pcl_utils.hpp b/quadcam_depth_est/include/pcl_utils.hpp similarity index 100% rename from quadcam_depth_est/src/pcl_utils.hpp rename to quadcam_depth_est/include/pcl_utils.hpp diff --git a/quadcam_depth_est/include/quadcam_depth_est_trt.hpp b/quadcam_depth_est/include/quadcam_depth_est_trt.hpp new file mode 100644 index 00000000..83fdfe24 --- /dev/null +++ b/quadcam_depth_est/include/quadcam_depth_est_trt.hpp @@ -0,0 +1,116 @@ +#pragma once + +#include +#include +#include "virtual_stereo.hpp" +#include +#include +#include +#include "pcl_utils.hpp" +#include +#include +#include +#include +#include "hitnet.hpp" + +namespace D2QuadCamDepthEst{ +const int32_t kCamerasNum = 4; +using D2Common::CameraConfig; + +cv::Mat quadReadVingette(const std::string & mask_file, double avg_brightness); + +class QuadcamDepthEstTrt{ + public: + QuadcamDepthEstTrt(ros::NodeHandle & nh); + ~QuadcamDepthEstTrt(); + void startAllService(); + void stopAllService(); + private: + void quadcamImageCb(const sensor_msgs::ImageConstPtr & images); + void loadVirtualCameras(YAML::Node & config, std::string configPath); + + void rawImageProcessThread(); + void inferrenceThread(); + void publishThread(); + void stoprawImageProcessThread(){ + raw_image_process_thread_running_ = 0; + }; + void stopinfrenceThread(){ + inference_thread_running_ = 0; + }; + void stoppublishThread(){ + publish_thread_running_ = 0; + }; + + std::pair intrinsicsFromNode(const YAML::Node & node); + + std::vector raw_cameras_; //fisheye cameras + std::vector undistortors_; //undistortors + std::vector virtual_stereos_; //virtual stereo + + std::vector raw_cam_extrinsics_; //extrinsics of fisheye cameras + std::vector virtual_left_extrinsics_; //extrinsics of virtual left cameras + + std::unique_ptr hitnet_ = nullptr; //hitnet + ros::NodeHandle nh_; + image_transport::ImageTransport * image_transport_; + image_transport::Subscriber image_sub_; + + ros::Publisher pub_pcl_; + PointCloud * pcl_ = nullptr; + PointCloudRGB * pcl_color_ = nullptr; + CameraConfig camera_config_ = D2Common::FOURCORNER_FISHEYE; + + //configurations + //image size + int width_= 320; + int height_= 240; + int pixel_step_ = 1; + int image_step_ = 1; + bool enable_texture_ = false; + bool show_ = false; + double min_z_ = 0.1; + double max_z_ = 10; + int image_count_ = 0; + bool cnn_input_rgb_ = false; + + int32_t fps_ = 10; + std::string onnx_path_; + std::string trt_engine_path_; + + std::thread raw_image_process_thread_; + std::thread inference_thread_; + std::thread publish_thread_; + int32_t raw_image_process_thread_running_ = 1; + int32_t inference_thread_running_ = 1; + int32_t publish_thread_running_ = 1; + + //ros rate + std::unique_ptr raw_image_process_rate_ = nullptr; + std::unique_ptr inference_rate_ = nullptr; + std::unique_ptr publish_rate_ = nullptr; + + std::string image_format_ = "raw"; + std::string image_topic_ = "/oak_ffc_4p/assemble_image"; + const std::string kPointCloudTopic_ = "/depth_estimation/pointcloud"; + + //buffers to store images double buffe + cv::Mat raw_image_; + std_msgs::Header raw_image_header_; + std::mutex raw_image_mutex_; + + cv::Mat split_raw_images_[kCamerasNum]; + cv::cuda::GpuMat rectified_images_[kCamerasNum][2];//{left,right},{left,right},{left,right},{left,right} + cv::Mat input_tensors_[kCamerasNum]; + std::mutex input_tensors_mutex_; + + cv::Mat output_tensors_[kCamerasNum]; + std::mutex output_tensors_mutex_; + + cv::Mat recity_images_for_show_and_texture_[kCamerasNum][2]; + cv::Mat publish_disparity_[kCamerasNum]; + cv::Mat photometric_inv_vingette_[kCamerasNum]; + +}; + +} \ No newline at end of file diff --git a/quadcam_depth_est/src/virtual_stereo.hpp b/quadcam_depth_est/include/virtual_stereo.hpp similarity index 62% rename from quadcam_depth_est/src/virtual_stereo.hpp rename to quadcam_depth_est/include/virtual_stereo.hpp index 36a2f501..96d71367 100644 --- a/quadcam_depth_est/src/virtual_stereo.hpp +++ b/quadcam_depth_est/include/virtual_stereo.hpp @@ -1,5 +1,7 @@ #include #include + +#pragma once namespace camodocal { class Camera; typedef boost::shared_ptr< Camera > CameraPtr; @@ -30,7 +32,7 @@ struct VirtualStereoConfig { class VirtualStereo { -protected: + protected: Swarm::Pose pose_left, pose_right; Swarm::Pose rect_pose_left, rect_pose_right; double baseline = 0.0; @@ -49,26 +51,52 @@ class VirtualStereo { //Rectify the images from pinhole images. bool input_is_stereo = false; cv::cuda::GpuMat inv_vingette_l, inv_vingette_r; -public: + public: bool enable_texture = true; int cam_idx_a = 0; int cam_idx_b = 1; + int cam_idx_a_right_half_id = 1; + int cam_idx_b_left_half_id = 0; + int stereo_id = 0; + Swarm::Pose extrinsic; std::vector rectifyImage(const cv::Mat & left, const cv::Mat & right); + int32_t rectifyImage(const cv::Mat & left, const cv::Mat & right, + cv::cuda::GpuMat & rect_left, cv::cuda::GpuMat & rect_right); + cv::Mat estimateDisparityOCV(const cv::Mat & left, const cv::Mat & right); cv::Mat estimateDisparity(const cv::Mat & left, const cv::Mat & right); - std::pair estimateDisparityViaRaw(const cv::Mat & left, const cv::Mat & right, const cv::Mat & left_color, bool show = false); - std::pair estimatePointsViaRaw(const cv::Mat & left, const cv::Mat & right, const cv::Mat & left_color, bool show = false); + std::pair estimateDisparityViaRaw(const cv::Mat & left, const cv::Mat & right, + const cv::Mat & left_color, bool show = false); + std::pair estimatePointsViaRaw(const cv::Mat & left, const cv::Mat & right, + const cv::Mat & left_color, bool show = false); + + cv::Mat getStereoPose(){ + return this->Q; + } + + int32_t showDispartiy(const cv::Mat & disparity, + cv::Mat & left_rect_mat, cv::Mat & right_rect_mat); + + + + VirtualStereo(int _idx_a, int _idx_b, + const Swarm::Pose & baseline, + D2Common::FisheyeUndist* _undist_left, + D2Common::FisheyeUndist* _undist_right, + int _undist_id_l, + int _undist_id_r, HitnetONNX* _hitnet, CREStereoONNX * _crestereo); + VirtualStereo(int _idx_a, int _idx_b, - const Swarm::Pose & baseline, - D2Common::FisheyeUndist* _undist_left, - D2Common::FisheyeUndist* _undist_right, - int _undist_id_l, - int _undist_id_r, HitnetONNX* _hitnet, CREStereoONNX * _crestereo); + const Swarm::Pose & baseline, + D2Common::FisheyeUndist* _undist_left, + D2Common::FisheyeUndist* _undist_right, + int _undist_id_l, int _undist_id_r); + VirtualStereo(const Swarm::Pose & baseline, - camodocal::CameraPtr cam_left, - camodocal::CameraPtr cam_right, - HitnetONNX* _hitnet, CREStereoONNX * _crestereo); + camodocal::CameraPtr cam_left, + camodocal::CameraPtr cam_right, + HitnetONNX* _hitnet, CREStereoONNX * _crestereo); void initVingette(const cv::Mat & _inv_vingette_l, const cv::Mat & _inv_vingette_r); void initRecitfy(const Swarm::Pose & baseline, cv::Mat K0, cv::Mat D0, cv::Mat K1, cv::Mat D1); }; diff --git a/quadcam_depth_est/launch/depth-node.launch b/quadcam_depth_est/launch/depth-node.launch index e5be79b4..17026b7b 100644 --- a/quadcam_depth_est/launch/depth-node.launch +++ b/quadcam_depth_est/launch/depth-node.launch @@ -1,13 +1,9 @@ - - - - + - - - + + \ No newline at end of file diff --git a/quadcam_depth_est/readme.md b/quadcam_depth_est/readme.md new file mode 100644 index 00000000..e69de29b diff --git a/quadcam_depth_est/src/crestereo_onnx.hpp b/quadcam_depth_est/src/crestereo_onnx.hpp deleted file mode 100644 index 9bbf00e2..00000000 --- a/quadcam_depth_est/src/crestereo_onnx.hpp +++ /dev/null @@ -1,139 +0,0 @@ -#pragma once -#include -#include - -using D2Common::Utility::TicToc; - -namespace D2QuadCamDepthEst { -// inline void hwc_to_chw(cv::Mat src, cv::Mat & dst) { -// const int src_h = src.rows; -// const int src_w = src.cols; -// const int src_c = src.channels(); -// cv::Mat hw_c = src.reshape(1, src_h * src_w); -// // const std::array dims = {src_c, src_h, src_w}; -// // dst.create(3, &dims[0], CV_MAKETYPE(src.depth(), 1)); -// cv::Mat dst_1d = dst.reshape(1, {src_c, src_h, src_w}); -// cv::transpose(hw_c, dst_1d); -// } - -inline void hwc_to_chw(const cv::Mat & src, float * buf) { - const int src_h = src.rows; - const int src_w = src.cols; - const int src_c = src.channels(); - const int aera = src_h * src_w; - if (src.type() == CV_8UC3) { - for (int i = 0; i < src_h; i++) { - for (int j = 0; j < src_w; j++) { - const cv::Vec3b bgr = src.at(i, j); - buf[i * src_w + j] = bgr[0]; //Red - buf[aera + i * src_w + j] = bgr[1]; //Green - buf[aera*2 + i * src_w + j] = bgr[2]; //Blue - } - } - } - if (src.type() == CV_32FC3) { - for (int i = 0; i < src_h; i++) { - for (int j = 0; j < src_w; j++) { - const cv::Vec3f bgr = src.at(i, j); - buf[i * src_w + j] = bgr[0]; //Red - buf[aera + i * src_w + j] = bgr[1]; //Green - buf[aera*2 + i * src_w + j] = bgr[2]; //Blue - } - } - } - -} - -class CREStereoONNX: public D2FrontEnd::ONNXInferenceGeneric { -protected: - float * results_; - std::array output_shape_; - std::array input_shape; - std::array input_shape_half; - float* input_l, *input_r, * input_l_half, *input_r_half; - std::vector inputs; - bool combined = false; - Ort::MemoryInfo memory_info; - void setInputs(Ort::MemoryInfo & memory_info, int width, int height) { - inputs.clear(); - const std::array dims = {3, height, width}; - input_l = new float[width*height*3]; - input_r = new float[width*height*3]; - if (combined) { - const std::array dims = {3, height/2, width/2}; - input_l_half = new float[width/2*height/2*3]; - input_r_half = new float[width/2*height/2*3]; - inputs.emplace_back(Ort::Value::CreateTensor(memory_info, - input_l_half, 3*width*height/4, input_shape_half.data(), 4)); - inputs.emplace_back(Ort::Value::CreateTensor(memory_info, - input_r_half, 3*width*height/4, input_shape_half.data(), 4)); - } - inputs.emplace_back(Ort::Value::CreateTensor(memory_info, - input_l, 3*width*height, input_shape.data(), 4)); - inputs.emplace_back(Ort::Value::CreateTensor(memory_info, - input_r, 3*width*height, input_shape.data(), 4)); - } -public: - CREStereoONNX(std::string engine_path, int _width, int _height, bool use_tensorrt = true, bool use_fp16 = true, bool use_int8 = false): - ONNXInferenceGeneric(engine_path, "left", "output", _width, _height, use_tensorrt, use_fp16, use_int8), - output_shape_{1, 2, _height, _width}, //Output shape is 2 channels, height, width - input_shape{1, 3, _height, _width}, //CREStereo is batch channel height width - input_shape_half{1, 3, _height/2, _width/2}, //CREStereo is batch channel height width - memory_info(Ort::MemoryInfo::CreateCpu(OrtArenaAllocator, OrtMemTypeDefault)) { - std::cout << "Trying to init CREStereoONNX@" << engine_path << std::endl; - if (session_->GetInputCount() == 2) { - combined = false; - } else { - combined = true; - } - setInputs(memory_info, _width, _height); - results_ = new float[2*width*height]; - output_tensor_ = Ort::Value::CreateTensor(memory_info, - results_, 2*width*height, output_shape_.data(), output_shape_.size()); - - if (!combined) { - printf("CREStereoONNX Init initialized\n"); - } else { - printf("CREStereoONNX Combine initialized\n"); - } - } - - virtual void doInference() { - const char* input_names[] = {"init_left", "init_right", "next_left", "next_right"}; - const char* output_names[] = {"next_output"}; - if (combined) { - session_->Run(Ort::RunOptions{nullptr}, input_names, inputs.data(), 4, output_names, &output_tensor_, 1); - } else { - input_names[0] = "left"; - input_names[1] = "right"; - output_names[0] = "output"; - session_->Run(Ort::RunOptions{nullptr}, input_names, inputs.data(), 2, output_names, &output_tensor_, 1); - } - } - - cv::Mat inference(const cv::Mat & input_left, const cv::Mat & input_right) { - cv::Mat _input_left, _input_right; - if (input_left.channels() != 3) { - cv::cvtColor(input_left, _input_left, cv::COLOR_GRAY2RGB); - cv::cvtColor(input_right, _input_right, cv::COLOR_GRAY2RGB); - } else { - _input_left = input_left; - _input_right = input_right; - } - if (_input_left.rows != height || _input_left.cols != width) { - cv::resize(_input_left, _input_left, cv::Size(width, height)); - cv::resize(_input_right, _input_right, cv::Size(width, height)); - } - cv::Mat input_half_left, input_half_right; - cv::resize(_input_left, input_half_left, cv::Size(width/2, height/2)); - cv::resize(_input_right, input_half_right, cv::Size(width/2, height/2)); - hwc_to_chw(_input_left, input_l); - hwc_to_chw(_input_right, input_r); - hwc_to_chw(input_half_left, input_l_half); - hwc_to_chw(input_half_right, input_r_half); - doInference(); - cv::Mat res(height, width, CV_32F, results_); - return res; - } -}; -} diff --git a/quadcam_depth_est/src/hitnet.cpp b/quadcam_depth_est/src/hitnet.cpp new file mode 100644 index 00000000..98846fcf --- /dev/null +++ b/quadcam_depth_est/src/hitnet.cpp @@ -0,0 +1,286 @@ +#include "hitnet.hpp" +#include +#include +#include +#include +#include +#include "tensorrt_utils/buffers.h" +#include "tensorrt_utils/logger.h" +#include "tensorrt_utils/common.h" + +namespace TensorRTHitnet{ +int32_t HitnetTrt::init(const std::string& onnx_model_path, const std::string& trt_engine_path, int32_t stream_number){ + if(stream_number <= 0){ + stream_number = 1; + } + stream_number_ = stream_number; + + //Check TRT engine file can be loaded, if not create engine from onnx model and save to trt_engine_path + if (access(trt_engine_path.c_str(), F_OK) == -1){ + spdlog::info("TRT engine file not found, create engine from onnx model"); + if (access(onnx_model_path.c_str(), F_OK) == -1){ + spdlog::error("onnx model file not found"); + return -2; + } + if (buildEngine(onnx_model_path, trt_engine_path) != 0){ + spdlog::error("buildEngine failed"); + return -3; + } + } else { + spdlog::info("TRT engine file found, load engine from file"); + if (deserializeEngine(trt_engine_path) != 0){ + spdlog::error("deserializeEngine failed"); + return -4; + } + } + + //Create executors + for(int32_t i = 0; i < stream_number_; i++){ + HitnetExcutor excutor; + int32_t ret = excutor.init(this->nv_engine_ptr_,std::string(kInputTensorName),std::string(kOutputTensorName)); + if(ret != 0){ + spdlog::error("Hitnet multistream Excutor init failed"); + return -5; + } + this->executors_.push_back(std::move(excutor)); + } + printf("Success to init HitnetTrt\n"); + return 0; +} + +//TODO: inference stucked +int32_t HitnetTrt::doInference(const cv::Mat input[4]){ + for(int32_t i = 0; i < this->stream_number_; i++){ + if(input[i].empty()){ + return 0; + } + int32_t ret = this->executors_[i].setInputImages(input[i]); + if(ret != 0){ + std::cout << "setInputImages failed" << std::endl; + return -2; + } + } + + for(int32_t i = 0; i < this->stream_number_; i++){ + int32_t ret = this->executors_[i].doInference(); + if(ret != 0){ + std::cout << "doInference failed" << std::endl; + return -3; + } + } + + for(int32_t i = 0; i < this->stream_number_; i++){ + int32_t ret = this->executors_[i].copyBack(); + if(ret != 0){ + std::cout << "doInference failed" << std::endl; + return -3; + } + } + + for(int32_t i = 0; i < this->stream_number_; i++){ + int32_t ret = this->executors_[i].synchronize(); + if(ret != 0){ + std::cout << "synchronize failed" << std::endl; + return -4; + } + } + + #ifdef DEBUG + printf ("inferenced\n"); + #endif + + return 0; +} + +int32_t HitnetTrt::getOutput(cv::Mat output[4]){ + for(int32_t i = 0; i < this->stream_number_; i++){ + int32_t ret = this->executors_[i].getOutput(output[i]); + if(ret != 0){ + std::cout << "getOutput failed" << std::endl; + return -5; + } + } + return 0; +} + +HitnetTrt::~HitnetTrt(){ + for(int32_t i = 0; i < this->stream_number_; i++){ + this->executors_[i].~HitnetExcutor(); + } + usleep(100); + this->nv_engine_ptr_->destroy(); //TODO: deconstruct bug here +} + + +int32_t HitnetTrt::deserializeEngine(const std::string& trt_engine_path){ + std::ifstream engine_file(trt_engine_path.c_str(), std::ios::binary); + if (engine_file.is_open()){ + spdlog::info("load engine from file: {}", trt_engine_path); + engine_file.seekg(0, std::ios::end); + size_t size = engine_file.tellg(); + engine_file.seekg(0, std::ios::beg); + char * engine_data = new char[size]; + engine_file.read(engine_data, size); + engine_file.close(); + IRuntime* nv_runtime = createInferRuntime(tensorrt_log::gLogger); + this->nv_engine_ptr_ = std::shared_ptr(nv_runtime->deserializeCudaEngine(engine_data, size, nullptr)); + delete[] engine_data; + if(this->nv_engine_ptr_ == nullptr){ + spdlog::error("deserializeCudaEngine failed"); + return -3; + } + spdlog::info("Success to load engine from file"); + return 0; + } else { + return -2; + } +} + +int32_t HitnetTrt::buildEngine(const std::string& onnx_model_path, const std::string& trt_engine_path){ + auto builder = tensorrt_common::TensorRTUniquePtr(nvinfer1::createInferBuilder(tensorrt_log::gLogger.getTRTLogger())); + if (builder == nullptr){ + spdlog::error("createInferBuilder failed"); + return -1; + } + auto network = tensorrt_common::TensorRTUniquePtr(builder->createNetworkV2( + 1U << static_cast(nvinfer1::NetworkDefinitionCreationFlag::kEXPLICIT_BATCH))); + if (network == nullptr){ + spdlog::error("createNetworkV2 failed"); + return -2; + } + auto config = tensorrt_common::TensorRTUniquePtr(builder->createBuilderConfig()); + if (config == nullptr){ + spdlog::error("createBuilderConfig failed"); + return -3; + } + auto parser = tensorrt_common::TensorRTUniquePtr( + nvonnxparser::createParser(*network, tensorrt_log::gLogger.getTRTLogger())); + if (parser == nullptr){ + spdlog::error("createParser failed"); + return -4; + } + auto profile = builder->createOptimizationProfile(); + if (profile == nullptr){ + spdlog::error("createOptimizationProfile failed"); + return -5; + } + profile->setDimensions(kInputTensorName, nvinfer1::OptProfileSelector::kOPT, nvinfer1::Dims4{1, 1, 240, 320}); + config->addOptimizationProfile(profile); + + auto parsed = parser->parseFromFile(onnx_model_path.c_str(), static_cast(tensorrt_log::gLogger.getReportableSeverity())); + if (!parsed){ + spdlog::error("parseFromFile failed"); + return -6; + } + config ->setMaxWorkspaceSize(1024_MiB); + config ->setFlag(nvinfer1::BuilderFlag::kFP16); + config ->setFlag(nvinfer1::BuilderFlag::kSTRICT_TYPES); + + auto profile_stream = tensorrt_common::makeCudaStream(); + if (profile_stream == nullptr){ + spdlog::error("makeCudaStream failed"); + return -7; + } + config->setProfileStream(*profile_stream); + tensorrt_common::TensorRTUniquePtr plan(builder->buildSerializedNetwork(*network, *config)); + if (plan == nullptr){ + spdlog::error("buildSerializedNetwork failed"); + return -8; + } + tensorrt_common::TensorRTUniquePtr runtime{createInferRuntime(tensorrt_log::gLogger.getTRTLogger())}; + if (runtime == nullptr){ + spdlog::error("createInferRuntime failed"); + return -9; + } + this->nv_engine_ptr_ = std::shared_ptr(runtime->deserializeCudaEngine(plan->data(), plan->size(), nullptr)); + if (this->nv_engine_ptr_ == nullptr){ + spdlog::error("deserializeCudaEngine failed"); + return -10; + } + + //Save engine to file + nvinfer1::IHostMemory* engine_data = this->nv_engine_ptr_->serialize(); + std::ofstream engine_file(trt_engine_path.c_str(), std::ios::binary); + if (engine_file.is_open()){ + engine_file.write(static_cast(engine_data->data()), engine_data->size()); + engine_file.close(); + spdlog::info("Success to save engine to file: {}", trt_engine_path); + } else { + spdlog::error("Failed to save engine to file: {}", trt_engine_path); + return -11; + } + return 0; +} + + + +int32_t HitnetExcutor::init(std::shared_ptr engine_ptr, + std::string input_tensor_name, + std::string output_tensor_name){ + this->engine_ptr_ = engine_ptr; + this->nv_context_ptr_ = this->engine_ptr_->createExecutionContext(); + if(this->nv_context_ptr_ == nullptr){ + std::cout << "createExecutionContext failed" << std::endl; + return -1; + } + cudaStreamCreate(&this->stream_); + if(this->stream_ == nullptr){ + std::cout << "cudaStreamCreate failed" << std::endl; + return -2; + } + this->buffer_manager_ptr_ = std::make_unique(this->engine_ptr_, + 0,this->nv_context_ptr_); + if(this->buffer_manager_ptr_ == nullptr){ + std::cout << "make_unique BufferManager failed" << std::endl; + return -3; + } + + this->input_tensor_name_ = input_tensor_name; + this->output_tensor_name_ = output_tensor_name; + this->input_index_ = this->engine_ptr_->getBindingIndex(input_tensor_name.c_str()); + if(this->input_index_ < 0){ + std::cout << "getBindingIndex failed" << std::endl; + return -4; + } + auto input_dim = this->engine_ptr_->getBindingDimensions(this->input_index_); + //Easy Coredump is donnot know your data type + this->input_size_ = input_dim.d[0] * input_dim.d[1] * input_dim.d[2] * input_dim.d[3] * sizeof(float); + this->output_index_ = this->engine_ptr_->getBindingIndex(output_tensor_name.c_str()); + if(this->output_index_ < 0){ + std::cout << "getBindingIndex failed" << std::endl; + return -5; + } + auto output_dim = this->engine_ptr_->getBindingDimensions(this->output_index_ ); + this->output_size_ = output_dim.d[0] * output_dim.d[1] * output_dim.d[2] * output_dim.d[3] * sizeof(float); + return 0; +} + +int32_t HitnetExcutor::setInputImages(const cv::Mat& input){ + memcpy(this->buffer_manager_ptr_->getHostBuffer(this->input_tensor_name_), input.data, this->input_size_); + buffer_manager_ptr_->copyInputToDeviceAsync(this->stream_); + return 0; +} + +int32_t HitnetExcutor::doInference(){ + bool status = this->nv_context_ptr_->enqueueV2(this->buffer_manager_ptr_->getDeviceBindings().data(), this->stream_, nullptr); + if (!status){ + std::cout << "enqueueV2 failed" << std::endl; + return -1; + } + return 0; +} + +int32_t HitnetExcutor::copyBack(){ + buffer_manager_ptr_->copyOutputToHostAsync(this->stream_); + return 0; +} + +int32_t HitnetExcutor::synchronize(){ + return cudaStreamSynchronize(this->stream_); +} + +int32_t HitnetExcutor::getOutput(cv::Mat& output){ + memcpy(output.data, this->buffer_manager_ptr_->getHostBuffer(this->output_tensor_name_), this->output_size_); + return 0; +} +} \ No newline at end of file diff --git a/quadcam_depth_est/src/hitnet_onnx.hpp b/quadcam_depth_est/src/hitnet_onnx.hpp deleted file mode 100644 index cbc30bda..00000000 --- a/quadcam_depth_est/src/hitnet_onnx.hpp +++ /dev/null @@ -1,58 +0,0 @@ -#pragma once -#include -#include - -using D2Common::Utility::TicToc; - -namespace D2QuadCamDepthEst { -class HitnetONNX: public D2FrontEnd::ONNXInferenceGeneric { -protected: - float * results_; - std::array output_shape_; - std::array input_shape_; - cv::Mat input_image_mat; -public: - HitnetONNX(std::string engine_path, int _width, int _height, bool use_tensorrt = true, bool use_fp16 = true, bool use_int8 = false): - ONNXInferenceGeneric(engine_path, "input", "reference_output_disparity", _width, _height, use_tensorrt, use_fp16, use_int8), - output_shape_{1, _height, _width, 1}, - input_shape_{1, 2, _height, _width} { - std::cout << "Trying to init HitnetONNX@" << engine_path << std::endl; - input_image_mat = cv::Mat(height*2, width, CV_32F); - auto memory_info = Ort::MemoryInfo::CreateCpu(OrtDeviceAllocator, OrtMemTypeCPU); - input_tensor_ = Ort::Value::CreateTensor(memory_info, - (float*)input_image_mat.data, 2*width*height, input_shape_.data(), 4); - results_ = new float[width*height]; - output_tensor_ = Ort::Value::CreateTensor(memory_info, - results_, width*height, output_shape_.data(), output_shape_.size()); - printf("HitnetONNX initialized\n"); - } - - virtual void doInference() { - const char* input_names[] = {m_InputBlobName.c_str()}; - const char* output_names[] = {output_name.c_str()}; - session_->Run(Ort::RunOptions{nullptr}, input_names, &input_tensor_, 1, output_names, &output_tensor_, 1); - } - - cv::Mat inference(const cv::Mat & input_left, const cv::Mat & input_right) { - TicToc tic; - cv::Mat _input_left, _input_right; - if (input_left.channels() == 3) { - cv::cvtColor(input_left, _input_left, cv::COLOR_BGR2GRAY); - cv::cvtColor(input_right, _input_right, cv::COLOR_BGR2GRAY); - } else { - _input_left = input_left; - _input_right = input_right; - } - if (_input_left.rows != height || _input_left.cols != width) { - cv::resize(_input_left, _input_left, cv::Size(width, height)); - cv::resize(_input_right, _input_right, cv::Size(width, height)); - } - cv::Mat input; - cv::vconcat(_input_left, _input_right, input); - input.convertTo(input_image_mat, CV_32F, 1.0/255.0); - doInference(); - // printf("HitnetONNX::inference() took %f ms\n", tic.toc()); - return cv::Mat(height, width, CV_32F, results_); - } -}; -} diff --git a/quadcam_depth_est/src/quadcam_depth_est.cpp b/quadcam_depth_est/src/quadcam_depth_est.cpp deleted file mode 100644 index 3162508c..00000000 --- a/quadcam_depth_est/src/quadcam_depth_est.cpp +++ /dev/null @@ -1,278 +0,0 @@ -#include "quadcam_depth_est.hpp" -#include -#include "hitnet_onnx.hpp" -#include "crestereo_onnx.hpp" -#include -#include -#include -#include - - -namespace D2FrontEnd { - std::pair readCameraConfig(const std::string & camera_name, const YAML::Node & config); -}; - -namespace D2QuadCamDepthEst { -std::pair intrinsicsFromNode(const YAML::Node & node); - -QuadCamDepthEst::QuadCamDepthEst(ros::NodeHandle & _nh): nh(_nh) { - std::string quadcam_depth_config_file; - nh.param("quadcam_depth_config_file", quadcam_depth_config_file, "quadcam_depth_config.yaml"); - nh.param("show", show, false); - pub_pcl = nh.advertise("/depth_estimation/point_cloud", 1); - printf("[QuadCamDepthEst] readConfig from: %s\n", quadcam_depth_config_file.c_str()); - int pn = quadcam_depth_config_file.find_last_of('/'); - std::string configPath = quadcam_depth_config_file.substr(0, pn); - YAML::Node config = YAML::LoadFile(quadcam_depth_config_file); - enable_texture = config["enable_texture"].as(); - pixel_step = config["pixel_step"].as(); - image_step = config["image_step"].as(); - min_z = config["min_z"].as(); - max_z = config["max_z"].as(); - loadCNN(config); - loadCameraConfig(config, configPath); - std::string format = "compressed"; //TODO: make it configurable - image_transport::TransportHints hints(format, ros::TransportHints().tcpNoDelay(true)); - it_ = new image_transport::ImageTransport(nh); - if (camera_config == CameraConfig::FOURCORNER_FISHEYE) { - image_sub = it_->subscribe("/arducam/image", 1000, &QuadCamDepthEst::imageCallback, this, hints); - } else { - left_sub = new ImageSubscriber(*it_, "/cam0/image_raw", 1000, hints); - right_sub = new ImageSubscriber(*it_, "/cam1/image_raw", 1000, hints); - sync = new message_filters::TimeSynchronizer (*left_sub, *right_sub, 1000); - sync->registerCallback(boost::bind(&QuadCamDepthEst::stereoImagesCallback, this, _1, _2)); - } - if (enable_texture) { - pcl_color = new PointCloudRGB; - pcl_color->points.reserve(virtual_stereos.size()*width*height); - } else { - pcl = new PointCloud; - pcl->points.reserve(virtual_stereos.size()*width*height); - } -} - -void QuadCamDepthEst::loadCNN(YAML::Node & config) { - std::string quadcam_depth_config_file; - enable_cnn = config["enable_cnn"].as(); - std::string cnn_type = config["cnn_type"].as(); - width = config["width"].as(); - height = config["height"].as(); - if (enable_cnn) { - std::string cnn_model_path; - bool cnn_use_tensorrt = config["cnn_use_tensorrt"].as(); - bool cnn_int8 = config["cnn_int8"].as(); - bool cnn_fp16 = config["cnn_fp16"].as(); - nh.param("cnn_model_path", cnn_model_path, ""); - if (cnn_type == "hitnet") { - hitnet = new HitnetONNX(cnn_model_path, width, height, cnn_use_tensorrt, cnn_fp16, cnn_int8); - cnn_rgb = false; - } - if (cnn_type == "crestereo") { - crestereo = new CREStereoONNX(cnn_model_path, width, height, cnn_use_tensorrt, cnn_fp16, cnn_int8); - cnn_rgb = true; - } - printf("[QuadCamDepthEst] Load CNN from %s tensorrt %d FP16 %d INT8 %d width %d height %d\n", cnn_model_path.c_str(), - cnn_use_tensorrt, cnn_fp16, cnn_int8, width, height); - - }else { - printf("[QuadCamDepthEst] CNN disabled. Use OpenCV SGBM\n"); - } -} - -void QuadCamDepthEst::stereoImagesCallback(const sensor_msgs::ImageConstPtr left, const sensor_msgs::ImageConstPtr right) { - if (image_count % image_step != 0) { - image_count++; - return; - } - TicToc t; - cv_bridge::CvImagePtr cv_ptr_l = cv_bridge::toCvCopy(left, sensor_msgs::image_encodings::MONO8); - cv_bridge::CvImagePtr cv_ptr_r = cv_bridge::toCvCopy(right, sensor_msgs::image_encodings::MONO8); - if (enable_texture) { - pcl_conversions::toPCL(left->header.stamp, pcl_color->header.stamp); - pcl_color->header.frame_id = "imu"; - pcl_color->points.clear(); - } else { - pcl_conversions::toPCL(left->header.stamp, pcl->header.stamp); - pcl->header.frame_id = "imu"; - pcl->points.clear(); - } - std::pair ret = virtual_stereos[0]->estimatePointsViaRaw(cv_ptr_l->image, cv_ptr_r->image, cv_ptr_l->image, show); - if (enable_texture) { - addPointsToPCL(ret.first, ret.second, virtual_stereos[0]->extrinsic, *pcl_color, pixel_step, min_z, max_z); - } else { - addPointsToPCL(ret.first, ret.second, virtual_stereos[0]->extrinsic, *pcl, pixel_step, min_z, max_z); - } - if (show) { - cv::waitKey(1); - } - if (enable_texture) { - pub_pcl.publish(*pcl_color); - } else { - pub_pcl.publish(*pcl); - } - image_count++; - printf("[QuadCamDepthEst] count %d process time %.1fms\n", image_count, t.toc()); -} - -void QuadCamDepthEst::imageCallback(const sensor_msgs::ImageConstPtr & left) { - if (image_count % image_step != 0) { - image_count++; - return; - } - TicToc t; - cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(left, sensor_msgs::image_encodings::BGR8); - cv::Mat img = cv_ptr->image; - std::vector imgs; - std::vector imgs_gray; - const int num_imgs = 4; - for (int i = 0; i < 4; i++) { - imgs.emplace_back(img(cv::Rect(i * img.cols /num_imgs, 0, img.cols /num_imgs, img.rows))); - if (!cnn_rgb) { - cv::Mat img_gray; - cv::cvtColor(imgs[i], img_gray, cv::COLOR_BGR2GRAY); - imgs_gray.emplace_back(img_gray); - } - } - if (enable_texture) { - pcl_conversions::toPCL(left->header.stamp, pcl_color->header.stamp); - pcl_color->header.frame_id = "imu"; - pcl_color->points.clear(); - } else { - pcl_conversions::toPCL(left->header.stamp, pcl->header.stamp); - pcl->header.frame_id = "imu"; - pcl->points.clear(); - } - int size = 0; - for (auto stereo: virtual_stereos) { - std::pair ret; - if (cnn_rgb) { - ret = stereo->estimatePointsViaRaw(imgs[stereo->cam_idx_a], imgs[stereo->cam_idx_b], cv::Mat(), show); - } else { - ret = stereo->estimatePointsViaRaw(imgs_gray[stereo->cam_idx_a], imgs_gray[stereo->cam_idx_b], imgs[stereo->cam_idx_a], show); - } - if (enable_texture) { - addPointsToPCL(ret.first, ret.second, stereo->extrinsic, *pcl_color, pixel_step, min_z, max_z); - } else { - addPointsToPCL(ret.first, ret.second, stereo->extrinsic, *pcl, pixel_step, min_z, max_z); - } - } - if (show) { - cv::waitKey(1); - } - if (enable_texture) { - pub_pcl.publish(*pcl_color); - } else { - pub_pcl.publish(*pcl); - } - image_count++; - printf("[QuadCamDepthEst] count %d process time %.1fms\n", image_count, t.toc()); -} - -cv::Mat readVingette(const std::string & mask_file, double avg_brightness) { - cv::Mat photometric_inv; - cv::Mat photometric_calib = cv::imread(mask_file, cv::IMREAD_GRAYSCALE); - std::cout << photometric_calib.type() << std::endl; - if (photometric_calib.type() == CV_8U) { - photometric_calib.convertTo(photometric_calib, CV_32FC1, 1.0/255.0); - } else if (photometric_calib.type() == CV_16S) { - photometric_calib.convertTo(photometric_calib, CV_32FC1, 1.0/65535.0); - } - cv::divide(avg_brightness, photometric_calib, photometric_inv); - return photometric_inv; -} - -void QuadCamDepthEst::loadCameraConfig(YAML::Node & config, std::string configPath) { - int _camera_config = config["camera_configuration"].as(); - camera_config = (CameraConfig)_camera_config; - cv::Mat photometric_inv, photometric_inv_1; - float avg_brightness = config["avg_brightness"].as(); - if (config["photometric_calib"]) { - photometric_inv = readVingette(configPath + "/" + config["photometric_calib"].as(), avg_brightness); - } - if (config["photometric_calib_1"]) { - photometric_inv_1 = readVingette(configPath + "/" + config["photometric_calib_1"].as(), avg_brightness); - } - std::string calib_file_path = config["calib_file_path"].as(); - printf("[QuadCamDepthEst] Load camera config from %s\n", calib_file_path.c_str()); - calib_file_path = configPath + "/" + calib_file_path; - YAML::Node config_cams = YAML::LoadFile(calib_file_path); - - for (const auto& kv : config_cams) { - std::string camera_name = kv.first.as(); - printf("[QuadCamDepthEst] Load camera %s\n", camera_name.c_str()); - const YAML::Node& value = kv.second; - auto ret = D2FrontEnd::readCameraConfig(camera_name, value); - raw_cameras.emplace_back(ret.first); - if (camera_config == CameraConfig::FOURCORNER_FISHEYE) { - double fov = config["fov"].as(); - undistortors.push_back(new D2Common::FisheyeUndist(ret.first, 0, fov, true, - D2Common::FisheyeUndist::UndistortPinhole2, width, height, photometric_inv)); - } - raw_cam_extrinsics.emplace_back(ret.second); - } - - if (camera_config == CameraConfig::STEREO_PINHOLE) { - //In stereo mode - Matrix4d T; - for (int i = 0; i < 4; i++) { - for (int j = 0; j < 4; j++) { - T(i, j) = config_cams["cam1"]["T_cn_cnm1"][i][j].as(); - } - } - auto baseline = Swarm::Pose(T.block<3, 3>(0, 0), T.block<3, 1>(0, 3)); - auto stereo = new VirtualStereo(baseline, raw_cameras[0], raw_cameras[1], hitnet, crestereo); - stereo->extrinsic = raw_cam_extrinsics[0]; - stereo->enable_texture = enable_texture; - stereo->initVingette(photometric_inv, photometric_inv_1); - virtual_stereos.emplace_back(stereo); - } else if (camera_config == CameraConfig::FOURCORNER_FISHEYE) { - for (const auto & kv: config["stereos"]) { - auto node = kv.second; - std::string stereo_name = kv.first.as(); - int cam_idx_l = node["cam_idx_l"].as(); - int cam_idx_r = node["cam_idx_r"].as(); - int idx_l = node["idx_l"].as(); - int idx_r = node["idx_r"].as(); - std::string stereo_calib_file = configPath + "/" + node["stereo_config"].as(); - Swarm::Pose baseline; - YAML::Node stereo_calib = YAML::LoadFile(stereo_calib_file); - Matrix4d T; - for (int i = 0; i < 4; i++) { - for (int j = 0; j < 4; j++) { - T(i, j) = stereo_calib["cam1"]["T_cn_cnm1"][i][j].as(); - } - } - baseline = Swarm::Pose(T.block<3, 3>(0, 0), T.block<3, 1>(0, 3)); - auto KD0 = intrinsicsFromNode(stereo_calib["cam0"]); - auto KD1 = intrinsicsFromNode(stereo_calib["cam1"]); - - printf("[QuadCamDepthEst] Load stereo %s, stereo %d(%d):%d(%d) baseline: %s\n", - stereo_name.c_str(), cam_idx_l, idx_l, cam_idx_r, idx_r, baseline.toStr().c_str()); - auto stereo = new VirtualStereo(cam_idx_l, cam_idx_r, baseline, - undistortors[cam_idx_l], undistortors[cam_idx_r], idx_l, idx_r, hitnet, crestereo); - auto att = undistortors[cam_idx_l]->t[idx_l]; - stereo->extrinsic = raw_cam_extrinsics[cam_idx_l] * Swarm::Pose(att, Vector3d(0, 0, 0)); - stereo->enable_texture = enable_texture; - stereo->initRecitfy(baseline, KD0.first, KD0.second, KD1.first, KD1.second); - virtual_stereos.emplace_back(stereo); - } - } -} - -std::pair intrinsicsFromNode(const YAML::Node & node) { - cv::Mat K = cv::Mat::eye(3, 3, CV_64FC1); - K.at(0, 0) = node["intrinsics"][0].as(); - K.at(1, 1) = node["intrinsics"][1].as(); - K.at(0, 2) = node["intrinsics"][2].as(); - K.at(1, 2) = node["intrinsics"][3].as(); - - cv::Mat D = cv::Mat::zeros(4, 1, CV_64FC1); - D.at(0, 0) = node["distortion_coeffs"][0].as(); - D.at(1, 0) = node["distortion_coeffs"][1].as(); - D.at(2, 0) = node["distortion_coeffs"][2].as(); - D.at(3, 0) = node["distortion_coeffs"][3].as(); - return std::make_pair(K, D); -} - -} - diff --git a/quadcam_depth_est/src/quadcam_depth_est.hpp b/quadcam_depth_est/src/quadcam_depth_est.hpp deleted file mode 100644 index 0d22f98d..00000000 --- a/quadcam_depth_est/src/quadcam_depth_est.hpp +++ /dev/null @@ -1,54 +0,0 @@ -#pragma once -#include -#include -#include "virtual_stereo.hpp" -#include -#include -#include -#include "pcl_utils.hpp" -#include -#include -#include - -typedef image_transport::SubscriberFilter ImageSubscriber; - -namespace D2QuadCamDepthEst { -using D2Common::CameraConfig; -class QuadCamDepthEst { - std::vector raw_cam_extrinsics; - std::vector virtual_left_extrinsics; - std::vector virtual_stereos; - std::vector undistortors; - std::vector raw_cameras; - HitnetONNX * hitnet = nullptr; - CREStereoONNX * crestereo = nullptr; - std::string cnn_type = "crestereo"; - int width = 320, height = 240; - int pixel_step = 1; - int image_step = 1; - bool enable_texture = false; - bool show; - int image_count = 0; - double min_z = 0.1; - double max_z = 10; - bool cnn_rgb = false; - bool enable_cnn; - - ros::NodeHandle nh; - image_transport::ImageTransport * it_; - image_transport::Subscriber image_sub; - ImageSubscriber * left_sub, *right_sub; - message_filters::TimeSynchronizer * sync; - ros::Publisher pub_pcl; - PointCloud * pcl = nullptr; - PointCloudRGB * pcl_color = nullptr; - CameraConfig camera_config = D2Common::STEREO_PINHOLE; - - void loadCNN(YAML::Node & config); - void loadCameraConfig(YAML::Node & config, std::string configPath); - void imageCallback(const sensor_msgs::ImageConstPtr & left); - void stereoImagesCallback(const sensor_msgs::ImageConstPtr left, const sensor_msgs::ImageConstPtr right); -public: - QuadCamDepthEst(ros::NodeHandle & _nh); -}; -} \ No newline at end of file diff --git a/quadcam_depth_est/src/quadcam_depth_est_node.cpp b/quadcam_depth_est/src/quadcam_depth_est_node.cpp index fdf0e29e..7df22fb2 100644 --- a/quadcam_depth_est/src/quadcam_depth_est_node.cpp +++ b/quadcam_depth_est/src/quadcam_depth_est_node.cpp @@ -1,12 +1,16 @@ #include -#include "quadcam_depth_est.hpp" +#include "quadcam_depth_est_trt.hpp" int main(int argc, char** argv) { ros::init(argc, argv, "quadcam_depth_est"); ros::NodeHandle nh("~"); - D2QuadCamDepthEst::QuadCamDepthEst quadcam_depth_est(nh); - ros::MultiThreadedSpinner spinner(3); - spinner.spin(); + // D2QuadCamDepthEst::QuadCamDepthEst quadcam_depth_est(nh); + // ros::MultiThreadedSpinner spinner(3); + D2QuadCamDepthEst::QuadcamDepthEstTrt quadcam_depth_est(nh); + quadcam_depth_est.startAllService(); + ros::spin(); + quadcam_depth_est.stopAllService(); + usleep(100); return 0; } \ No newline at end of file diff --git a/quadcam_depth_est/src/quadcam_depth_est_trt.cpp b/quadcam_depth_est/src/quadcam_depth_est_trt.cpp new file mode 100644 index 00000000..3bd93911 --- /dev/null +++ b/quadcam_depth_est/src/quadcam_depth_est_trt.cpp @@ -0,0 +1,424 @@ +#include "quadcam_depth_est_trt.hpp" +#include +#include +#include +#include +#include +#include +#include +#include + +#include "pcl_utils.hpp" + + +// namespace D2FrontEnd { +// std::pair readCameraConfig(const std::string & camera_name, const YAML::Node & config, int32_t extrinsic_parameter_type = 1); +// }; + +namespace D2QuadCamDepthEst +{ + +cv::Mat quadReadVingette(const std::string & mask_file, double avg_brightness) { + cv::Mat photometric_inv; + cv::Mat photometric_calib = cv::imread(mask_file, cv::IMREAD_GRAYSCALE); + std::cout << photometric_calib.type() << std::endl; + if (photometric_calib.type() == CV_8U) { + photometric_calib.convertTo(photometric_calib, CV_32FC1, 1.0/255.0); + } else if (photometric_calib.type() == CV_16S) { + photometric_calib.convertTo(photometric_calib, CV_32FC1, 1.0/65535.0); + } + cv::divide(avg_brightness, photometric_calib, photometric_inv); + return photometric_inv; +} + +QuadcamDepthEstTrt::QuadcamDepthEstTrt(ros::NodeHandle & nh):nh_(nh){ + std::string config_file_path; + nh.getParam("depth_config",config_file_path); + nh.getParam("show",show_); + printf("[QuadCamDepthEstTrt]:read config from:%s show: %d\n",config_file_path.c_str(),show_); + YAML::Node config = YAML::LoadFile(config_file_path); + std::string config_dir = config_file_path.substr(0,config_file_path.find_last_of("/")); + this->enable_texture_ = config["enable_texture"].as(); + this->pixel_step_ = config["pixel_step"].as(); + this->image_step_ = config["image_step"].as(); + this->min_z_ = config["min_z"].as(); + this->max_z_ = config["max_z"].as(); + this->width_ = config["width"].as(); + this->height_ = config["height"].as(); + this->fps_ = config["fps"].as(); + this->raw_image_process_rate_ = std::make_unique(ros::Rate(this->fps_)); + this->inference_rate_ = std::make_unique(ros::Rate(this->fps_)); + this->publish_rate_ = std::make_unique(ros::Rate(this->fps_)); + this->cnn_input_rgb_ = config["cnn_input_rgb"].as(); + + this->loadVirtualCameras(config,config_dir); + if(config["image_topic"].IsDefined()){ + this->image_topic_ = config["image_topic"].as(); + } + if(config["image_format"].IsDefined()){ + this->image_format_ = config["image_format"].as(); + } + //hitnet + for(int i = 0 ; ioutput_tensors_[i] = cv::Mat(this->height_,this->width_,CV_32F); + } + this->onnx_path_ = config["onnx_path"].as(); + this->trt_engine_path_ = config["trt_engine_path"].as(); + + this->hitnet_ = std::make_unique(true); + this->hitnet_->init(onnx_path_,trt_engine_path_, 4); + + //subscribe + image_transport::TransportHints hints(this->image_format_, ros::TransportHints().tcpNoDelay(true)); + image_transport_ = new image_transport::ImageTransport(nh_); + image_sub_ = image_transport_->subscribe(this->image_topic_, 1, &QuadcamDepthEstTrt::quadcamImageCb, this, hints); + if (enable_texture_){ + pcl_color_ = new PointCloudRGB(); + pcl_color_->points.reserve(virtual_stereos_.size() * width_ * height_); + } else { + pcl_ = new PointCloud(); + pcl_->points.reserve(virtual_stereos_.size() * width_ * height_); + } + + //publisher + this->pub_pcl_ = nh_.advertise(kPointCloudTopic_, 1); + printf("QuadcamDepthEtsTrt constructed\n"); +}; + +QuadcamDepthEstTrt::~QuadcamDepthEstTrt(){ + if(pcl_ != nullptr){ + delete pcl_; + pcl_ = nullptr; + } + if(pcl_color_ != nullptr){ + delete pcl_color_; + pcl_color_ = nullptr; + } + if (this->hitnet_ != nullptr){ + this->hitnet_ = nullptr; + } +}; + +void QuadcamDepthEstTrt::loadVirtualCameras(YAML::Node & config, std::string configPath){ + float avg_brightness = config["avg_brightness"].as(); + std::string photometric_calib_path = config["photometric_calib_path"].as(); + //Read photometric calibration masks + if (access(photometric_calib_path.c_str(),F_OK) == 0){ + printf("[QuadcamDepthEstTrt]: loadVirtualCameras from %s\n",photometric_calib_path.c_str()); + for(int i=0 ; i < kCamerasNum ; i++){ + std::string mask_file = photometric_calib_path + "/" + std::string("cam_") + std::to_string(i) + std::string("_vig_mask.png");//search image "cam_i_vig_mask.png" + if(access(mask_file.c_str(),F_OK) == 0){ + photometric_inv_vingette_[i] = quadReadVingette(mask_file, avg_brightness); + printf("[QuadcamDepthEstTrt]: read vignette mask from %s\n",mask_file.c_str()); + } else { + photometric_inv_vingette_[i] = cv::Mat (1280, 720, CV_8UC3, cv::Scalar(255, 255, 255)); + } + } + } else { + for(int i=0 ; i < kCamerasNum ; i++){ + photometric_inv_vingette_[i] = cv::Mat (1280, 720, CV_8UC3, cv::Scalar(255, 255, 255)); + } + } + //Read fisheye cameras intrinsic and extrinsic parameters + std::string cam_calib_file_path = config["cam_calib_file_path"].as(); + printf("[QuadcamDepthEstTrt]: load camera calibration from %s\n",cam_calib_file_path.c_str()); + if(access(cam_calib_file_path.c_str(),R_OK) == 0){ + YAML::Node fisheye_configs = YAML::LoadFile(cam_calib_file_path); + int32_t photometric_inv_idx = 0; + for (const auto & cam_para : fisheye_configs){ + std::string camera_name = cam_para.first.as(); + printf("[QuadcamDepthEstTrt] Load camera %s\n", camera_name.c_str()); + //fisheye camera parameters + const YAML::Node & camera_parameters = cam_para.second; + auto cam_model = D2FrontEnd::D2FrontendParams::readCameraConfig(camera_name,camera_parameters); + this->raw_cameras_.push_back(cam_model.first); + //load distotors and photometric calibration + double fov = config["fov"].as(); + if(photometric_inv_idx >=4 || photometric_inv_idx < 0){ + photometric_inv_idx = 0; + } + printf("[Debug ]undistortor matrix init with size width:%d height:%d\n",this->width_,this->height_); + this->undistortors_.push_back(new D2Common::FisheyeUndist(cam_model.first, 0, fov, true, + D2Common::FisheyeUndist::UndistortPinhole2, this->width_, this->height_, photometric_inv_vingette_[photometric_inv_idx])); + printf("[Debug] undistorter width and height:%d %d\n",this->width_,this->height_); + photometric_inv_idx++; + //set extrinsic paratmers + this->raw_cam_extrinsics_.push_back(cam_model.second); + } + + //Create virtual stereo + for(const auto & vstereos:config["stereos"]){ + auto stereo_node = vstereos.second; + std::string stereo_name = vstereos.first.as(); + int cam_idx_l = stereo_node["cam_idx_l"].as(); + int cam_idx_r = stereo_node["cam_idx_r"].as(); + int idx_l = stereo_node["idx_l"].as(); + int idx_r = stereo_node["idx_r"].as(); + std::string stereo_calib_file = stereo_node["stereo_config"].as(); + Swarm::Pose baseline; + YAML::Node stereo_calib = YAML::LoadFile(stereo_calib_file); + Matrix4d T; + for (int i = 0; i < 4; i++) { + for (int j = 0; j < 4; j++) { + T(i, j) = stereo_calib["cam1"]["T_cn_cnm1"][i][j].as(); + } + } + baseline = Swarm::Pose(T.block<3, 3>(0, 0), T.block<3, 1>(0, 3)); + auto KD0 = intrinsicsFromNode(stereo_calib["cam0"]); + auto KD1 = intrinsicsFromNode(stereo_calib["cam1"]); + + printf("[QuadCamDepthEst] Load stereo %s, stereo %d(%d):%d(%d) baseline: %s\n", + stereo_name.c_str(), cam_idx_l, idx_l, cam_idx_r, idx_r, baseline.toStr().c_str()); + auto stereo = new VirtualStereo(cam_idx_l, cam_idx_r, baseline, + undistortors_[cam_idx_l], undistortors_[cam_idx_r], idx_l, idx_r); + auto att = undistortors_[cam_idx_l]->t[idx_l]; + stereo->extrinsic = raw_cam_extrinsics_[cam_idx_l] * Swarm::Pose(att, Vector3d(0, 0, 0)); + stereo->enable_texture = enable_texture_; + stereo->initRecitfy(baseline, KD0.first, KD0.second, KD1.first, KD1.second); + virtual_stereos_.emplace_back(stereo); + } + printf("[QuadCamDepthEst] Init virtual cameras successfully\n"); + } else { + printf("QuadcamDepthEstTrt][Failed]: read camera calibration from %s\n",cam_calib_file_path.c_str()); + return ; + } + return ; +} + +std::pair QuadcamDepthEstTrt::intrinsicsFromNode(const YAML::Node & node) { + cv::Mat K = cv::Mat::eye(3, 3, CV_64FC1); + printf("calibration parameters in size height:%d width:%d\n",node["resolution"][1].as(),node["resolution"][0].as()); + + K.at(0, 0) = node["intrinsics"][0].as(); + K.at(1, 1) = node["intrinsics"][1].as(); + K.at(0, 2) = node["intrinsics"][2].as(); + K.at(1, 2) = node["intrinsics"][3].as(); + + cv::Mat D = cv::Mat::zeros(4, 1, CV_64FC1); + D.at(0, 0) = node["distortion_coeffs"][0].as(); + D.at(1, 0) = node["distortion_coeffs"][1].as(); + D.at(2, 0) = node["distortion_coeffs"][2].as(); + D.at(3, 0) = node["distortion_coeffs"][3].as(); + return std::make_pair(K, D); +} + +void QuadcamDepthEstTrt::startAllService(){ + this->raw_image_process_thread_ = std::thread(&QuadcamDepthEstTrt::rawImageProcessThread,this); + this->inference_thread_ = std::thread(&QuadcamDepthEstTrt::inferrenceThread,this); + this->publish_thread_ = std::thread(&QuadcamDepthEstTrt::publishThread,this); + printf("[QuadcamDepthEstTrt]: start all service\n"); +} + +void QuadcamDepthEstTrt::stopAllService(){ + if(this->inference_thread_.joinable()){ + this->stopinfrenceThread(); + this->inference_thread_.join(); + } + if(this->publish_thread_.joinable()){ + this->stoppublishThread(); + this->publish_thread_.join(); + } + if(this->raw_image_process_thread_.joinable()){ + this->stoprawImageProcessThread(); + this->raw_image_process_thread_.join(); + } +} + +void QuadcamDepthEstTrt::quadcamImageCb(const sensor_msgs::ImageConstPtr & images){ + if (!raw_image_mutex_.try_lock()){ + return; + } else { + raw_image_ = cv_bridge::toCvCopy(images, sensor_msgs::image_encodings::BGR8)->image; + this->raw_image_header_ = images->header; + raw_image_mutex_.unlock(); + } + return; +} + +//TODO:kCamearsNum = size of vitual_stereos_ +void QuadcamDepthEstTrt::rawImageProcessThread(){ + while(raw_image_process_thread_running_){ + static cv::Mat raw_image; + /* Because raw_image_ always get new memory addr, + so here we handle the memory and release raw_image_ for cb */ + if (raw_image_mutex_.try_lock()){ + if (raw_image_.empty()){ + this->raw_image_mutex_.unlock(); + this->raw_image_process_rate_->sleep(); + continue; + } else { + raw_image = raw_image_; + this->raw_image_mutex_.unlock(); + } + } else { + this->raw_image_process_rate_->sleep(); + continue; + } + + for(int32_t i = 0; i< kCamerasNum; i++){ + cv::Mat splited_image = raw_image(cv::Rect(i * raw_image_.cols /kCamerasNum, 0, + raw_image.cols /kCamerasNum, raw_image.rows)); + if(!this->cnn_input_rgb_){ + + if (splited_image.empty()){ + printf("[QuadcamDepthEstTrt]: splited image is empty\n"); + this->raw_image_process_rate_->sleep(); + continue; + } + + cv::cvtColor(splited_image,split_raw_images_[i],cv::COLOR_BGR2GRAY);//TODO: Bug openCV segement fault + #ifdef DEBUG + printf("[QuadcamDepthEstTrt]: split raw image to gray\n"); + char window_name[20]; + sprintf(window_name,"raw_image_%d",i); + cv::imshow(window_name,split_raw_images_[i]); + cv::waitKey(1); + #endif + } else { + split_raw_images_[i] = splited_image; + } + } + #ifdef DEBUG + // printf("[QuadcamDepthEstTrt]: split raw image\n"); + cv::imshow("raw_image",raw_image_); + cv::waitKey(1); + #endif + + //get rectify images + for(auto && stereo: this->virtual_stereos_){ + stereo->rectifyImage(split_raw_images_[stereo->cam_idx_a],split_raw_images_[stereo->cam_idx_b], + rectified_images_[stereo->cam_idx_a][stereo->cam_idx_a_right_half_id], + rectified_images_[stereo->cam_idx_b][stereo->cam_idx_b_left_half_id]); + } + + #ifdef DEBUG + //show all pairs of rectified images + for(auto && stereo: this->virtual_stereos_){ + char window_name[20]; + cv::Mat show_image; + cv::Mat left(rectified_images_[stereo->cam_idx_a][stereo->cam_idx_a_right_half_id]); + cv::Mat right(rectified_images_[stereo->cam_idx_b][stereo->cam_idx_b_left_half_id]); + cv::hconcat(left,right,show_image); + sprintf(window_name,"rectified_image_%d_%d",stereo->cam_idx_a,stereo->cam_idx_b); + cv::imshow(window_name,show_image); + cv::waitKey(1); + } + #endif + + //construct input images for hitnet inferrence and TODO: can gpu mat be used directly? + cv::Mat temp_left , temp_right, input_image[4]; + + for (auto && stereo : this->virtual_stereos_){ + temp_left = cv::Mat(rectified_images_[stereo->cam_idx_a][stereo->cam_idx_a_right_half_id]); + temp_right = cv::Mat(rectified_images_[stereo->cam_idx_b][stereo->cam_idx_b_left_half_id]); + recity_images_for_show_and_texture_[stereo->cam_idx_a][stereo->cam_idx_a_right_half_id] = temp_left; + recity_images_for_show_and_texture_[stereo->cam_idx_b][stereo->cam_idx_b_left_half_id] = temp_right; + // redundant undistort image is already in size + // cv::resize(temp_left,temp_left,cv::Size(this->width_,this->height_)); + // cv::resize(temp_right,temp_right,cv::Size(this->width_,this->height_)); + cv::vconcat(temp_left,temp_right,input_image[stereo->stereo_id]); + } + + //to reduce the time of mutex lock + if (!input_tensors_mutex_.try_lock()){ + this->raw_image_process_rate_->sleep(); + continue; + } else { + for (auto && stereo : this->virtual_stereos_){ + input_image[stereo->stereo_id].convertTo(input_tensors_[stereo->stereo_id],CV_32FC1,1.0/255.0); + } + input_tensors_mutex_.unlock(); + } + this->raw_image_process_rate_->sleep(); + } + return ; +} + +void QuadcamDepthEstTrt::inferrenceThread(){ + static cv::Mat input_tensors[4]; + while(inference_thread_running_){ + if(input_tensors_mutex_.try_lock()){ + //if input_tensors_ is empty, wait for next loop + if (this->input_tensors_[0].empty()){ + this->input_tensors_mutex_.unlock(); + this->inference_rate_->sleep(); + continue; + } + + for (auto stereo : this->virtual_stereos_){ + input_tensors[stereo->stereo_id] = input_tensors_[stereo->stereo_id]; + } + input_tensors_mutex_.unlock(); + } else { + this->inference_rate_->sleep(); + continue; + } + this->hitnet_->doInference(input_tensors); + + if (output_tensors_mutex_.try_lock()){ + this->hitnet_->getOutput(output_tensors_); + output_tensors_mutex_.unlock(); + } else { + this->inference_rate_->sleep(); + continue; + } + this->inference_rate_->sleep(); + } + return ; +} + +void QuadcamDepthEstTrt::publishThread(){ + //TODO: publish pointcloud and do visualization + while(publish_thread_running_){ + //if output_tensors_ is empty, wait for next loop + if (this->output_tensors_[0].empty()){ + this->publish_rate_->sleep(); + continue; + } + + //copy data to local + if (output_tensors_mutex_.try_lock()){ + for (auto stereo : this->virtual_stereos_){ + publish_disparity_[stereo->stereo_id] = output_tensors_[stereo->stereo_id]; + } + output_tensors_mutex_.unlock(); + } else { + this->publish_rate_->sleep(); + continue; + } + //debug show disparity + if(show_){ + if (recity_images_for_show_and_texture_[0][0].empty()){ + this->publish_rate_->sleep(); + continue; + } + for (auto stereo : this->virtual_stereos_){ + stereo->showDispartiy(publish_disparity_[stereo->stereo_id], + recity_images_for_show_and_texture_[stereo->cam_idx_a][stereo->cam_idx_a_right_half_id], + recity_images_for_show_and_texture_[stereo->cam_idx_b][stereo->cam_idx_b_left_half_id]); + } + } + //prepeare pcl + if (pcl_ == nullptr){ + printf("[QuadcamDepthEstTrt]: pcl is nullptr\n"); + this->publish_rate_->sleep(); + continue; + } + pcl_conversions::toPCL(raw_image_header_.stamp, pcl_->header.stamp); + pcl_->header.frame_id = "imu"; + pcl_->points.clear(); + //TODO: if enable texture + for (auto stereo : this->virtual_stereos_){ + cv::Mat points; + cv::reprojectImageTo3D(publish_disparity_[stereo->stereo_id], + points, stereo->getStereoPose(), 3); + addPointsToPCL(points,recity_images_for_show_and_texture_[stereo->cam_idx_a][stereo->cam_idx_a_right_half_id], + stereo->extrinsic, *this->pcl_, this->pixel_step_, this->min_z_, this->max_z_); + } + pub_pcl_.publish(*pcl_); + this->publish_rate_->sleep(); + } + return ; +} + +} // namespace D2QuadCamDepthEst diff --git a/quadcam_depth_est/src/virtual_stereo.cpp b/quadcam_depth_est/src/virtual_stereo.cpp index 46d64c32..d6fd4154 100644 --- a/quadcam_depth_est/src/virtual_stereo.cpp +++ b/quadcam_depth_est/src/virtual_stereo.cpp @@ -1,15 +1,15 @@ #include "virtual_stereo.hpp" -#include + #include #include #include #include #include -#include "hitnet_onnx.hpp" -#include "crestereo_onnx.hpp" #include #include +#include "d2common/fisheye_undistort.h" + namespace D2QuadCamDepthEst { VirtualStereo::VirtualStereo(int _cam_idx_a, int _cam_idx_b, const Swarm::Pose & baseline, @@ -20,10 +20,19 @@ VirtualStereo::VirtualStereo(int _cam_idx_a, int _cam_idx_b, HitnetONNX* _hitnet, CREStereoONNX* _crestereo): cam_idx_a(_cam_idx_a), cam_idx_b(_cam_idx_b), undist_left(_undist_left), undist_right(_undist_right), undist_id_l(_undist_id_l), undist_id_r(_undist_id_r), hitnet(_hitnet), crestereo(_crestereo) { - auto cam_param = static_cast(undist_left->cam_side.get())->getParameters(); - img_size = cv::Size(cam_param.imageWidth(), cam_param.imageHeight()); - cv::Mat K = (cv::Mat_(3,3) << cam_param.fx(), 0, cam_param.cx(), 0, cam_param.fy(), cam_param.cy(), 0, 0, 1); - initRecitfy(baseline, K, cv::Mat(), K, cv::Mat()); + auto cam_param_left = static_cast(undist_left->cam_side.get())->getParameters(); + printf("[Debug]camera_left size:%d %d\n",cam_param_left.imageWidth(),cam_param_left.imageHeight()); + this->img_size = cv::Size(cam_param_left.imageWidth(), cam_param_left.imageHeight()); + cv::Mat K_left = (cv::Mat_(3,3) << cam_param_left.fx(), 0, cam_param_left.cx(), 0, cam_param_left.fy(), cam_param_left.cy(), 0, 0, 1); + auto cam_param_right = static_cast(_undist_right->cam_side.get())->getParameters(); + // img_size = cv::Size(cam_param.imageWidth(), cam_param.imageHeight()); + cv::Mat K_right = (cv::Mat_(3,3) << cam_param_right.fx(), 0, cam_param_right.cx(), 0, cam_param_right.fy(), cam_param_right.cy(), 0, 0, 1); + this->cam_idx_a_right_half_id = undist_id_l; + this->cam_idx_b_left_half_id = undist_id_r; + + //stereo based on left image id so use left cam id instead + this->stereo_id = cam_idx_a; + initRecitfy(baseline, K_left, cv::Mat(), K_right, cv::Mat()); } void VirtualStereo::initVingette(const cv::Mat & _inv_vingette_l, const cv::Mat & _inv_vingette_r) { @@ -76,20 +85,41 @@ VirtualStereo::VirtualStereo(const Swarm::Pose & baseline, cuda_rmap_2.upload(rmap_2); input_is_stereo = true; - // std::cout << "img_size" << img_size << std::endl; - // std::cout << "K0: " << K0 << std::endl; - // std::cout << "K1: " << K1 << std::endl; - // std::cout << "D0: " << D0 << std::endl; - // std::cout << "D1: " << D1 << std::endl; - // std::cout << "xi0: " << xi0 << std::endl; - // std::cout << "xi1: " << xi1 << std::endl; - // std::cout << "R: " << R << std::endl; - // std::cout << "T: " << T << std::endl; - // std::cout << "R1: " << R1 << std::endl; - // std::cout << "R2: " << R2 << std::endl; - // std::cout << "P1: " << P1 << std::endl; - // std::cout << "P2: " << P2 << std::endl; - // std::cout << "Q: " << Q << std::endl; +// std::cout << "img_size" << img_size << std::endl; +// std::cout << "K0: " << K0 << std::endl; +// std::cout << "K1: " << K1 << std::endl; +// std::cout << "D0: " << D0 << std::endl; +// std::cout << "D1: " << D1 << std::endl; +// std::cout << "xi0: " << xi0 << std::endl; +// std::cout << "xi1: " << xi1 << std::endl; +// std::cout << "R: " << R << std::endl; +// std::cout << "T: " << T << std::endl; +// std::cout << "R1: " << R1 << std::endl; +// std::cout << "R2: " << R2 << std::endl; +// std::cout << "P1: " << P1 << std::endl; +// std::cout << "P2: " << P2 << std::endl; +// std::cout << "Q: " << Q << std::endl; +} + +VirtualStereo::VirtualStereo(int cam_idx_a, int cam_idx_b, + const Swarm::Pose & baseline, + D2Common::FisheyeUndist* _undist_left, + D2Common::FisheyeUndist* _undist_right, + int _undist_id_l, int _undist_id_r):cam_idx_a(cam_idx_a), cam_idx_b(cam_idx_b), undist_left(_undist_left), undist_right(_undist_right), + undist_id_l(_undist_id_l), undist_id_r(_undist_id_r){ + auto cam_param_left = static_cast(undist_left->cam_side.get())->getParameters(); + printf("[Debug]camera_left size:%d %d\n",cam_param_left.imageWidth(),cam_param_left.imageHeight()); + this->img_size = cv::Size(cam_param_left.imageWidth(), cam_param_left.imageHeight()); + cv::Mat K_left = (cv::Mat_(3,3) << cam_param_left.fx(), 0, cam_param_left.cx(), 0, cam_param_left.fy(), cam_param_left.cy(), 0, 0, 1); + auto cam_param_right = static_cast(_undist_right->cam_side.get())->getParameters(); + // img_size = cv::Size(cam_param.imageWidth(), cam_param.imageHeight()); + cv::Mat K_right = (cv::Mat_(3,3) << cam_param_right.fx(), 0, cam_param_right.cx(), 0, cam_param_right.fy(), cam_param_right.cy(), 0, 0, 1); + this->cam_idx_a_right_half_id = undist_id_l; + this->cam_idx_b_left_half_id = undist_id_r; + + //stereo based on left image id so use left cam id instead + this->stereo_id = cam_idx_a; + initRecitfy(baseline, K_left, cv::Mat(), K_right, cv::Mat()); } void VirtualStereo::initRecitfy(const Swarm::Pose & baseline, cv::Mat K0, cv::Mat D0, cv::Mat K1, cv::Mat D1) { @@ -98,6 +128,11 @@ void VirtualStereo::initRecitfy(const Swarm::Pose & baseline, cv::Mat K0, cv::Ma cv::stereoRectify(K0, D0, K1, D1, img_size, R, T, R1, R2, T1, T2, Q, 1024, -1, cv::Size(), &roi_l, &roi_r); initUndistortRectifyMap(K0, D0, R1, T1, img_size, CV_32FC1, lmap_1, lmap_2); initUndistortRectifyMap(K1, D1, R2, T2, img_size, CV_32FC1, rmap_1, rmap_2); + // printf("[Debug] rmap init\n"); + std::cout << "[Debug]K0: " << K0 << std::endl; + std::cout << "[Debug]K1: " << K1 << std::endl; + std::cout << "[Debug]D0: " << D0 << std::endl; + std::cout << "[Debug]D1: " << D1 << std::endl; cuda_lmap_1.upload(lmap_1); cuda_lmap_2.upload(lmap_2); cuda_rmap_1.upload(rmap_1); @@ -115,9 +150,18 @@ std::pair VirtualStereo::estimatePointsViaRaw(const cv::Mat & } + +//retrun disparity and left_rect_image std::pairVirtualStereo::estimateDisparityViaRaw(const cv::Mat & left, const cv::Mat & right, const cv::Mat & left_color, bool show) { + // printf("[Debug] debug ouput\n"); auto ret = rectifyImage(left, right); cv::Mat limg_rect(ret[0]), rimg_rect(ret[1]); + + // cv::imshow("raw",left); + // cv::imshow("rectify",limg_rect); + // cv::waitKey(0); + + // printf("l_img_shape width %d hight %d\n",limg_rect.cols,limg_rect.rows); auto disp = estimateDisparity(limg_rect, rimg_rect); if (show) { cv::Mat show; @@ -169,6 +213,7 @@ std::pairVirtualStereo::estimateDisparityViaRaw(const cv::Mat std::vector VirtualStereo::rectifyImage(const cv::Mat & left, const cv::Mat & right) { cv::cuda::GpuMat leftRectify, rightRectify, img_cuda_l, img_cuda_r; if (input_is_stereo) { + // printf("[Debug] stereo rectify\n"); img_cuda_l.upload(left); img_cuda_r.upload(right); if (!inv_vingette_l.empty()) { @@ -181,11 +226,78 @@ std::vector VirtualStereo::rectifyImage(const cv::Mat & left, img_cuda_l = undist_left->undist_id_cuda(left, undist_id_l, true); img_cuda_r = undist_right->undist_id_cuda(right, undist_id_r, true); } + //Bug Here lamp_1 and lmap_2 generation faield + + cv::Mat undist_left(img_cuda_l); + cv::imshow("undist_left",undist_left); + printf("[Debug]undist_left size %d %d\n",undist_left.cols,undist_left.rows); + cv::cuda::remap(img_cuda_l, leftRectify, cuda_lmap_1, cuda_lmap_2, cv::INTER_LINEAR); cv::cuda::remap(img_cuda_r, rightRectify, cuda_rmap_1, cuda_rmap_2, cv::INTER_LINEAR); + cv::Mat rectify_l(leftRectify); + printf("[Debug]rectify image size:%d %d\n",rectify_l.cols,rectify_l.rows); + return {leftRectify, rightRectify}; } + +// input [left_fisheye right_fisheye] output [rect_left rect_right] +int32_t VirtualStereo::rectifyImage(const cv::Mat & left, const cv::Mat & right, + cv::cuda::GpuMat & rect_left, cv::cuda::GpuMat & rect_right) { + cv::cuda::GpuMat img_cuda_l, img_cuda_r; + if (input_is_stereo) { + // printf("[Debug] stereo rectify\n"); + img_cuda_l.upload(left); + img_cuda_r.upload(right); + if (!inv_vingette_l.empty()) { + img_cuda_l.convertTo(img_cuda_l, CV_32FC1); + img_cuda_r.convertTo(img_cuda_r, CV_32FC1); + cv::cuda::multiply(img_cuda_l, inv_vingette_l, img_cuda_l); + cv::cuda::multiply(img_cuda_r, inv_vingette_r, img_cuda_r); + } + } else { + img_cuda_l = undist_left->undist_id_cuda(left, undist_id_l, true); + img_cuda_r = undist_right->undist_id_cuda(right, undist_id_r, true); + } + //Bug Here lamp_1 and lmap_2 generation faield + + #ifdef DEBUG + cv::Mat undist_left(img_cuda_l); + cv::imshow("undist_left",undist_left); + printf("[Debug]undist_left size %d %d\n",undist_left.cols,undist_left.rows); + #endif + + cv::cuda::remap(img_cuda_l, rect_left, cuda_lmap_1, cuda_lmap_2, cv::INTER_LINEAR); + cv::cuda::remap(img_cuda_r, rect_right, cuda_rmap_1, cuda_rmap_2, cv::INTER_LINEAR); + return 0; +} + +//show disparity for debug +int32_t VirtualStereo::showDispartiy(const cv::Mat & disparity, + cv::Mat & left_rect_mat, cv::Mat & right_rect_mat){ + cv::Mat disparity_show; + disparity.convertTo(disparity_show, CV_8U, 255.0/32.0); + cv::applyColorMap(disparity_show, disparity_show, cv::COLORMAP_JET); + cv::rectangle(disparity_show, roi_l, cv::Scalar(0, 0, 255), 2); + cv::Mat limg_rect_show, rimg_rect_show, show; + left_rect_mat.convertTo(limg_rect_show, CV_8U); + right_rect_mat.convertTo(rimg_rect_show, CV_8U); + cv::rectangle(limg_rect_show, roi_l, cv::Scalar(0, 0, 255), 2); + cv::rectangle(rimg_rect_show, roi_r, cv::Scalar(0, 0, 255), 2); + if (left_rect_mat.channels() == 1) { + cv::cvtColor(limg_rect_show, limg_rect_show, cv::COLOR_GRAY2BGR); + cv::cvtColor(rimg_rect_show, rimg_rect_show, cv::COLOR_GRAY2BGR); + } + cv::hconcat(limg_rect_show, rimg_rect_show, show); + cv::hconcat(show, disparity_show, show); + char buf[64]; + sprintf(buf, "VirtualStereo %d<->%d", cam_idx_a, cam_idx_b); + cv::imshow(buf, show); + cv::waitKey(1); + return 0; +} + + cv::Mat VirtualStereo::estimateDisparityOCV(const cv::Mat & left, const cv::Mat & right) { auto sgbm = cv::StereoSGBM::create(config.minDisparity, config.numDisparities, config.blockSize, config.P1, config.P2, config.disp12MaxDiff, config.preFilterCap, config.uniquenessRatio, @@ -204,20 +316,7 @@ cv::Mat VirtualStereo::estimateDisparityOCV(const cv::Mat & left, const cv::Mat } cv::Mat VirtualStereo::estimateDisparity(const cv::Mat & left, const cv::Mat & right) { - if (config.use_cnn && (hitnet != nullptr || crestereo!=nullptr)) { - if (hitnet!=nullptr) { - if (left.channels() == 3) { - cv::Mat left_gray; - cv::cvtColor(left, left_gray, cv::COLOR_BGR2GRAY); - return estimateDisparity(left_gray, right); - } - return hitnet->inference(left, right); - } else { - return crestereo->inference(left, right); - } - } else { - return estimateDisparityOCV(left, right); - } + return estimateDisparityOCV(left, right); } } diff --git a/quadcam_tools/graco_dataset_tool.py b/quadcam_tools/graco_dataset_tool.py new file mode 100644 index 00000000..ba34c494 --- /dev/null +++ b/quadcam_tools/graco_dataset_tool.py @@ -0,0 +1,70 @@ +#!/usr/bin/env python3 +from transformations import * +from math import * +from cv_bridge import CvBridge +import cv2 as cv +import argparse +from sensor_msgs.msg import CompressedImage +import pathlib +import rosbag + +bridge = CvBridge() +first_print = False + +def compress_image_msg(msg, resize=0.4): + cv_image = bridge.imgmsg_to_cv2(msg, desired_encoding='passthrough') + cv_image = cv.resize(cv_image, (0, 0), fx=resize, fy=resize) + comp_img = CompressedImage() + comp_img.header = msg.header + comp_img.format = "mono8; jpeg compressed" + succ, _data = cv.imencode(".jpg", cv_image, encode_param) + comp_img.data = _data.flatten().tolist() + global first_print + if not first_print: + print(f"Compress image from {msg.height}x{msg.width} to {cv_image.shape}") + first_print = True + return comp_img, cv_image + +def generate_bagname(bag, output_path, comp=False): + from pathlib import Path + p = Path(bag) + if comp: + bagname = p.stem + "-resize-comp.bag" + else: + bagname = p.stem + "-resize.bag" + output_bag = output_path.joinpath(bagname) + return output_bag + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument('bags', metavar='bags', type=str, nargs='+', + help='bags to be synchronized') + parser.add_argument('-c', '--comp', action='store_true', help='compress the image topics', default=True) + parser.add_argument('-q', '--quality', type=int, default=90, help='quality of the compressed image') + parser.add_argument('-r', '--resize', type=float, default=0.4, help='resize scale of the image') + parser.add_argument('-s', '--show', action='store_true', help="show while sync") + parser.add_argument('-o', '--output', default="", type=str, help='output path') + args = parser.parse_args() + bags = args.bags + + encode_param = [int(cv.IMWRITE_JPEG_QUALITY), args.quality] + + output_path = pathlib.Path(bags[0]).parents[0] if args.output == "" else pathlib.Path(args.output) + print(f"{len(bags)} bags to process. Will write to", output_path) + for bag in bags: + output_bag = generate_bagname(bag, output_path, args.comp) + print("Write bag to", output_bag) + with rosbag.Bag(output_bag, 'w') as outbag: + for topic, msg, t in rosbag.Bag(bag).read_messages(): + if msg._type == "sensor_msgs/Image" and args.comp: + comp_img, cv_image = compress_image_msg(msg, args.resize) + outbag.write(topic+"_resize/compressed", comp_img, t) + if args.show: + # Show timestamp, size on the imageimage + cv.putText(cv_image, f"{t.to_sec():.3f}", (10, 30), cv.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2) + cv.putText(cv_image, f"{cv_image.shape[1]}x{cv_image.shape[0]}", (10, 60), cv.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2) + cv.imshow(topic, cv_image) + cv.waitKey(1) + else: + outbag.write(topic, msg, t) + \ No newline at end of file diff --git a/quadcam_tools/sync_multi_bags.py b/quadcam_tools/sync_multi_bags.py index cab9e659..28ff22f3 100755 --- a/quadcam_tools/sync_multi_bags.py +++ b/quadcam_tools/sync_multi_bags.py @@ -88,7 +88,7 @@ def get_pose0(bag): # ypr = print(f"Will use {t0} as start yaw0 {y0} pos0 {pos0} qcalib {q_calib}") return pos0, q_calib, y0 - elif topic == "/SwarmNode1/pose": + elif topic == "/SwarmNode1/pose" or topic=="/leica/pose/relative": quat0 = msg.pose.orientation pos0 = msg.pose.position y0, p0, r0 = quat2eulers(quat0.w, quat0.x, quat0.y, quat0.z) @@ -100,8 +100,8 @@ def get_pose0(bag): return None, None, None def compress_image_msg(msg): - img16 = bridge.imgmsg_to_cv2(msg, desired_encoding='passthrough') - cv_image = (img16/256).astype('uint8') + cv_image = bridge.imgmsg_to_cv2(msg, desired_encoding='passthrough') + # cv_image = (img16/256).astype('uint8') comp_img = CompressedImage() comp_img.header = msg.header comp_img.format = "mono8; jpeg compressed" @@ -119,6 +119,9 @@ def compress_image_msg(msg): parser.add_argument('-r', '--realsense', action='store_true', help="is realsense not TUM") parser.add_argument('-o', '--output', default="", type=str, help='output path') parser.add_argument('-p', '--sync-path', default="", action='store_true', help='sync by path command') + parser.add_argument('-t','--start-time', nargs='+', help=' Set flag', required=False, type=float) + parser.add_argument('-u','--duration', nargs='+', help=' Set flag', required=False, type=float) + args = parser.parse_args() bags = args.bags dts = {} @@ -143,13 +146,17 @@ def compress_image_msg(msg): dts[bag] = t0 - t0s[bag] else: t0 = get_time0(bags[0], is_realsense=args.realsense) - for bag in bags: - t = get_time0(bag, is_realsense=args.realsense) - print(f"Bag {bag} start at {t.to_sec()}") + + for i in range(len(bags)): + bag = bags[i] + t = t_ = get_time0(bag, is_realsense=args.realsense) + if len(args.start_time) > 0: + t = t_ + rospy.Duration(args.start_time[i]) + print(f"Bag {bag} start at {t_.to_sec()}, we will use from {t.to_sec()}") dts[bag] = t0 - t t0s[bag] = t - pos0, q_calib, y0 = get_pose0(bags[0]) + # pos0, q_calib, y0 = get_pose0(bags[0]) import pathlib output_path = pathlib.Path(bags[0]).resolve() if args.output == "" else pathlib.Path(args.output) @@ -157,11 +164,12 @@ def compress_image_msg(msg): bridge = CvBridge() encode_param = [int(cv.IMWRITE_JPEG_QUALITY), args.quality] - for bag in bags: + for i in range(len(bags)): + bag = bags[i] output_bag = generate_bagname(bag, output_path, args.comp) print("Write bag to", output_bag) _dt = dts[bag] - with rosbag.Bag(output_bag, 'w') as outbag: + with rosbag.Bag(output_bag, 'w', compression="bz2") as outbag: from nav_msgs.msg import Path path = Path() path_arr = [] @@ -169,6 +177,8 @@ def compress_image_msg(msg): for topic, msg, t in rosbag.Bag(bag).read_messages(): if t < t0s[bag]: continue + if args.duration is not None and t - t0s[bag] > rospy.Duration(args.duration[i]): + break if msg._has_header: if msg.header.stamp.to_sec() > 0: msg.header.stamp = msg.header.stamp + _dt @@ -183,7 +193,7 @@ def compress_image_msg(msg): cv.waitKey(1) continue outbag.write(topic, msg, t + _dt ) - if topic == "/vrpn_client/raw_transform" or topic == "/SwarmNode1/pose": + if topic == "/vrpn_client/raw_transform" or topic == "/SwarmNode1/pose" or topic=="/leica/pose/relative": if topic == "/vrpn_client/raw_transform": posestamp = PoseStamped() posestamp.header = msg.header @@ -194,7 +204,7 @@ def compress_image_msg(msg): quat = msg.transform.rotation quat = np.array([quat.w, quat.x, quat.y, quat.z]) quat = quaternion_multiply(q_calib, quat) - elif topic == "/SwarmNode1/pose": + elif topic == "/SwarmNode1/pose" or topic=="/leica/pose/relative": posestamp = PoseStamped() posestamp.header = msg.header posestamp.header.frame_id = "world" diff --git a/start_docker.sh b/start_docker.sh index 96d2e0b2..419b3678 100755 --- a/start_docker.sh +++ b/start_docker.sh @@ -3,11 +3,8 @@ # Usage: ./start_ffc_4p_docker.sh 1 to start docker only for image transportation. # Please do not move this file to other dir, it will cause the docker container can not find the current dir. SWARM_WS=/root/swarm_ws -DOCKERIMAGE="d2slam:pc" -DATA_SET=/media/khalil/ssd_data/data_set -HITNET=/home/khalil/workspace/d2slam_ws/src/ONNX-HITNET-Stereo-Depth-estimation -CRESTEREO=/home/khalil/workspace/d2slam_ws/src/ONNX-CREStereo-Depth-Estimation - +DOCKERIMAGE="hkustswarm/d2slam:jetson_orin" +DATA_SET=/home/dji/dataset if [ $# -eq 0 ]; then echo "[INFO] No start option, will start docker container only for application" START_OPTION=0 @@ -21,13 +18,9 @@ if [ ${START_OPTION} == 1 ]; then CURRENT_DIR=$(pwd) echo "${CURRENT_DIR} will be mapped in to docker container with start option 1" docker run -it --rm --runtime=nvidia --gpus all --net=host -v ${CURRENT_DIR}:${SWARM_WS}/src/D2SLAM \ - -v ${HITNET}:${SWARM_WS}/src/ONNX-HITNET-Stereo-Depth-estimation \ - -v ${CRESTEREO}:${SWARM_WS}/src/ONNX-CREStereo-Depth-Estimation \ -v /dev/:/dev/ -v ${DATA_SET}:/data/ --privileged -e DISPLAY -v /tmp/.X11-unix:/tmp/.X11-unix --name="d2slam_container" ${DOCKERIMAGE} /bin/bash else echo "Start docker container for image transportation only" docker run -it --rm --runtime=nvidia --gpus all --net=host -v /dev/:/dev/ \ - -v ${HITNET}:${SWARM_WS}/src/ONNX-HITNET-Stereo-Depth-estimation \ - -v ${CRESTEREO}:${SWARM_WS}/src/ONNX-CREStereo-Depth-Estimation \ --privileged -e DISPLAY -v ${DATA_SET}:/data/ -v /tmp/.X11-unix:/tmp/.X11-unix --name="d2slam_container" ${DOCKERIMAGE} /bin/bash fi \ No newline at end of file diff --git a/tensorrt_utils/CMakeLists.txt b/tensorrt_utils/CMakeLists.txt new file mode 100644 index 00000000..f26d9a26 --- /dev/null +++ b/tensorrt_utils/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.0.2) +project(tensorrt_utils) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) +set(CMAKE_CXX_FLAGS_RELEASE "-g -O3") +set(CMAKE_CXX_FLAGS_DEBUG "-g -O0") +add_compile_options(-Wno-deprecated-declarations -Wno-reorder -Wno-format -Wno-sign-compare) + +find_package(CUDA REQUIRED) + +include_directories( + ${CMAKE_CURRENT_SOURCE_DIR}/include/tensorrt_utils + ${CUDA_INCLUDE_DIRS} +) + +add_library(${PROJECT_NAME} + src/logger.cpp +) + +target_link_libraries(${PROJECT_NAME} + nvinfer + nvinfer_plugin + nvonnxparser +) diff --git a/tensorrt_utils/include/tensorrt_utils/buffers.h b/tensorrt_utils/include/tensorrt_utils/buffers.h new file mode 100644 index 00000000..99e6e0a1 --- /dev/null +++ b/tensorrt_utils/include/tensorrt_utils/buffers.h @@ -0,0 +1,418 @@ +/* + * Copyright (c) 2021, NVIDIA CORPORATION. All rights reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#ifndef TENSORRT_BUFFERS_H +#define TENSORRT_BUFFERS_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "common.h" +#include "half.h" + +namespace tensorrt_buffer { + +//! +//! \brief The GenericBuffer class is a templated class for buffers. +//! +//! \details This templated RAII (Resource Acquisition Is Initialization) class handles the allocation, +//! deallocation, querying of buffers on both the device and the host. +//! It can handle data of arbitrary types because it stores byte buffers. +//! The template parameters AllocFunc and FreeFunc are used for the +//! allocation and deallocation of the buffer. +//! AllocFunc must be a functor that takes in (void** ptr, size_t size) +//! and returns bool. ptr is a pointer to where the allocated buffer address should be stored. +//! size is the amount of memory in bytes to allocate. +//! The boolean indicates whether or not the memory allocation was successful. +//! FreeFunc must be a functor that takes in (void* ptr) and returns void. +//! ptr is the allocated buffer address. It must work with nullptr input. +//! + template + class GenericBuffer { + public: + //! + //! \brief Construct an empty buffer. + //! + GenericBuffer(nvinfer1::DataType type = nvinfer1::DataType::kFLOAT) + : mSize(0), mCapacity(0), mType(type), mBuffer(nullptr) { + } + + //! + //! \brief Construct a buffer with the specified allocation size in bytes. + //! + GenericBuffer(size_t size, nvinfer1::DataType type) + : mSize(size), mCapacity(size), mType(type) { + if (!allocFn(&mBuffer, this->nbBytes())) { + throw std::bad_alloc(); + } + } + + GenericBuffer(GenericBuffer &&buf) + : mSize(buf.mSize), mCapacity(buf.mCapacity), mType(buf.mType), mBuffer(buf.mBuffer) { + buf.mSize = 0; + buf.mCapacity = 0; + buf.mType = nvinfer1::DataType::kFLOAT; + buf.mBuffer = nullptr; + } + + GenericBuffer &operator=(GenericBuffer &&buf) { + if (this != &buf) { + freeFn(mBuffer); + mSize = buf.mSize; + mCapacity = buf.mCapacity; + mType = buf.mType; + mBuffer = buf.mBuffer; + // Reset buf. + buf.mSize = 0; + buf.mCapacity = 0; + buf.mBuffer = nullptr; + } + return *this; + } + + //! + //! \brief Returns pointer to underlying array. + //! + void *data() { + return mBuffer; + } + + //! + //! \brief Returns pointer to underlying array. + //! + const void *data() const { + return mBuffer; + } + + //! + //! \brief Returns the size (in number of elements) of the buffer. + //! + size_t size() const { + return mSize; + } + + //! + //! \brief Returns the size (in bytes) of the buffer. + //! + size_t nbBytes() const { + return this->size() * tensorrt_common::getElementSize(mType); + } + + //! + //! \brief Resizes the buffer. This is a no-op if the new size is smaller than or equal to the current capacity. + //! + void resize(size_t newSize) { + mSize = newSize; + if (mCapacity < newSize) { + freeFn(mBuffer); + if (!allocFn(&mBuffer, this->nbBytes())) { + throw std::bad_alloc{}; + } + mCapacity = newSize; + } + } + + //! + //! \brief Overload of resize that accepts Dims + //! + void resize(const nvinfer1::Dims &dims) { + return this->resize(tensorrt_common::volume(dims)); + } + + ~GenericBuffer() { + freeFn(mBuffer); + } + + private: + size_t mSize{0}, mCapacity{0}; + nvinfer1::DataType mType; + void *mBuffer; + AllocFunc allocFn; + FreeFunc freeFn; + }; + + class DeviceAllocator { + public: + bool operator()(void **ptr, size_t size) const { + return cudaMalloc(ptr, size) == cudaSuccess; + } + }; + + class DeviceFree { + public: + void operator()(void *ptr) const { + cudaFree(ptr); + } + }; + + class HostAllocator { + public: + bool operator()(void **ptr, size_t size) const { + *ptr = malloc(size); + return *ptr != nullptr; + } + }; + + class HostFree { + public: + void operator()(void *ptr) const { + free(ptr); + } + }; + + using DeviceBuffer = GenericBuffer; + using HostBuffer = GenericBuffer; + +//! +//! \brief The ManagedBuffer class groups together a pair of corresponding device and host buffers. +//! + class ManagedBuffer { + public: + DeviceBuffer deviceBuffer; + HostBuffer hostBuffer; + }; + +//! +//! \brief The BufferManager class handles host and device buffer allocation and deallocation. +//! +//! \details This RAII class handles host and device buffer allocation and deallocation, +//! memcpy between host and device buffers to aid with inference, +//! and debugging dumps to validate inference. The BufferManager class is meant to be +//! used to simplify buffer management and any interactions between buffers and the engine. +//! + class BufferManager { + public: + static const size_t kINVALID_SIZE_VALUE = ~size_t(0); + + //! + //! \brief Create a BufferManager for handling buffer interactions with engine. + //! + BufferManager(std::shared_ptr engine, const int batchSize = 0, + const nvinfer1::IExecutionContext *context = nullptr) + : mEngine(engine), mBatchSize(batchSize) { + // Full Dims implies no batch size. + assert(engine->hasImplicitBatchDimension() || mBatchSize == 0); + // Create host and device buffers + for (int i = 0; i < mEngine->getNbBindings(); i++) { + auto dims = context ? context->getBindingDimensions(i) : mEngine->getBindingDimensions(i); + size_t vol = context || !mBatchSize ? 1 : static_cast(mBatchSize); + nvinfer1::DataType type = mEngine->getBindingDataType(i); + int vecDim = mEngine->getBindingVectorizedDim(i); + if (-1 != vecDim) // i.e., 0 != lgScalarsPerVector + { + int scalarsPerVec = mEngine->getBindingComponentsPerElement(i); + dims.d[vecDim] = tensorrt_common::divUp(dims.d[vecDim], scalarsPerVec); + vol *= scalarsPerVec; + } + vol *= tensorrt_common::volume(dims); + std::unique_ptr manBuf{new ManagedBuffer()}; + manBuf->deviceBuffer = DeviceBuffer(vol, type); + manBuf->hostBuffer = HostBuffer(vol, type); + mDeviceBindings.emplace_back(manBuf->deviceBuffer.data()); + mManagedBuffers.emplace_back(std::move(manBuf)); + } + } + + //! + //! \brief Returns a vector of device buffers that you can use directly as + //! bindings for the execute and enqueue methods of IExecutionContext. + //! + std::vector &getDeviceBindings() { + return mDeviceBindings; + } + + //! + //! \brief Returns a vector of device buffers. + //! + const std::vector &getDeviceBindings() const { + return mDeviceBindings; + } + + //! + //! \brief Returns the device buffer corresponding to tensorName. + //! Returns nullptr if no such tensor can be found. + //! + void *getDeviceBuffer(const std::string &tensorName) const { + return getBuffer(false, tensorName); + } + + //! + //! \brief Returns the host buffer corresponding to tensorName. + //! Returns nullptr if no such tensor can be found. + //! + void *getHostBuffer(const std::string &tensorName) const { + return getBuffer(true, tensorName); + } + + //! + //! \brief Returns the size of the host and device buffers that correspond to tensorName. + //! Returns kINVALID_SIZE_VALUE if no such tensor can be found. + //! + size_t size(const std::string &tensorName) const { + int index = mEngine->getBindingIndex(tensorName.c_str()); + if (index == -1) + return kINVALID_SIZE_VALUE; + return mManagedBuffers[index]->hostBuffer.nbBytes(); + } + + //! + //! \brief Dump host buffer with specified tensorName to ostream. + //! Prints error message to std::ostream if no such tensor can be found. + //! + void dumpBuffer(std::ostream &os, const std::string &tensorName) { + int index = mEngine->getBindingIndex(tensorName.c_str()); + if (index == -1) { + os << "Invalid tensor name" << std::endl; + return; + } + void *buf = mManagedBuffers[index]->hostBuffer.data(); + size_t bufSize = mManagedBuffers[index]->hostBuffer.nbBytes(); + nvinfer1::Dims bufDims = mEngine->getBindingDimensions(index); + size_t rowCount = static_cast(bufDims.nbDims > 0 ? bufDims.d[bufDims.nbDims - 1] : mBatchSize); + int leadDim = mBatchSize; + int *trailDims = bufDims.d; + int nbDims = bufDims.nbDims; + + // Fix explicit Dimension networks + if (!leadDim && nbDims > 0) { + leadDim = bufDims.d[0]; + ++trailDims; + --nbDims; + } + + os << "[" << leadDim; + for (int i = 0; i < nbDims; i++) + os << ", " << trailDims[i]; + os << "]" << std::endl; + switch (mEngine->getBindingDataType(index)) { + case nvinfer1::DataType::kINT32: + print(os, buf, bufSize, rowCount); + break; + case nvinfer1::DataType::kFLOAT: + print(os, buf, bufSize, rowCount); + break; + case nvinfer1::DataType::kHALF: + print(os, buf, bufSize, rowCount); + break; + case nvinfer1::DataType::kINT8: + assert(0 && "Int8 network-level input and output is not supported"); + // break; + case nvinfer1::DataType::kBOOL: + assert(0 && "Bool network-level input and output are not supported"); + // break; + } + } + + //! + //! \brief Templated print function that dumps buffers of arbitrary type to std::ostream. + //! rowCount parameter controls how many elements are on each line. + //! A rowCount of 1 means that there is only 1 element on each line. + //! + template + void print(std::ostream &os, void *buf, size_t bufSize, size_t rowCount) { + assert(rowCount != 0); + assert(bufSize % sizeof(T) == 0); + T *typedBuf = static_cast(buf); + size_t numItems = bufSize / sizeof(T); + for (int i = 0; i < static_cast(numItems); i++) { + // Handle rowCount == 1 case + if (rowCount == 1 && i != static_cast(numItems) - 1) + os << typedBuf[i] << std::endl; + else if (rowCount == 1) + os << typedBuf[i]; + // Handle rowCount > 1 case + else if (i % rowCount == 0) + os << typedBuf[i]; + else if (i % rowCount == rowCount - 1) + os << " " << typedBuf[i] << std::endl; + else + os << " " << typedBuf[i]; + } + } + + //! + //! \brief Copy the contents of input host buffers to input device buffers synchronously. + //! + void copyInputToDevice() { + memcpyBuffers(true, false, false); + } + + //! + //! \brief Copy the contents of output device buffers to output host buffers synchronously. + //! + void copyOutputToHost() { + memcpyBuffers(false, true, false); + } + + //! + //! \brief Copy the contents of input host buffers to input device buffers asynchronously. + //! + void copyInputToDeviceAsync(const cudaStream_t &stream = 0) { + memcpyBuffers(true, false, true, stream); + } + + //! + //! \brief Copy the contents of output device buffers to output host buffers asynchronously. + //! + void copyOutputToHostAsync(const cudaStream_t &stream = 0) { + memcpyBuffers(false, true, true, stream); + } + + ~BufferManager() = default; + + private: + void *getBuffer(const bool isHost, const std::string &tensorName) const { + int index = mEngine->getBindingIndex(tensorName.c_str()); + if (index == -1) + return nullptr; + return (isHost ? mManagedBuffers[index]->hostBuffer.data() : mManagedBuffers[index]->deviceBuffer.data()); + } + + void + memcpyBuffers(const bool copyInput, const bool deviceToHost, const bool async, const cudaStream_t &stream = 0) { + for (int i = 0; i < mEngine->getNbBindings(); i++) { + void *dstPtr + = deviceToHost ? mManagedBuffers[i]->hostBuffer.data() + : mManagedBuffers[i]->deviceBuffer.data(); + const void *srcPtr + = deviceToHost ? mManagedBuffers[i]->deviceBuffer.data() + : mManagedBuffers[i]->hostBuffer.data(); + const size_t byteSize = mManagedBuffers[i]->hostBuffer.nbBytes(); + const cudaMemcpyKind memcpyType = deviceToHost ? cudaMemcpyDeviceToHost : cudaMemcpyHostToDevice; + if ((copyInput && mEngine->bindingIsInput(i)) || (!copyInput && !mEngine->bindingIsInput(i))) { + if (async) + CHECK(cudaMemcpyAsync(dstPtr, srcPtr, byteSize, memcpyType, stream)); + else + CHECK(cudaMemcpy(dstPtr, srcPtr, byteSize, memcpyType)); + } + } + } + + std::shared_ptr mEngine; //!< The pointer to the engine + int mBatchSize; //!< The batch size for legacy networks, 0 otherwise. + std::vector> mManagedBuffers; //!< The vector of pointers to managed buffers + std::vector mDeviceBindings; //!< The vector of device buffers needed for engine execution + }; + +} // namespace tensorrt_buffer + +#endif // TENSORRT_BUFFERS_H diff --git a/tensorrt_utils/include/tensorrt_utils/common.h b/tensorrt_utils/include/tensorrt_utils/common.h new file mode 100644 index 00000000..c0258e10 --- /dev/null +++ b/tensorrt_utils/include/tensorrt_utils/common.h @@ -0,0 +1,859 @@ +/* + * Copyright (c) 2021, NVIDIA CORPORATION. All rights reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef TENSORRT_COMMON_H +#define TENSORRT_COMMON_H + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "logger.h" + +using namespace nvinfer1; +using namespace plugin; + +#ifdef _MSC_VER +#define FN_NAME __FUNCTION__ +#else +#define FN_NAME __func__ +#endif + +#if defined(__aarch64__) || defined(__QNX__) +#define ENABLE_DLA_API 1 +#endif + +#define CHECK(status) \ + do \ + { \ + auto ret = (status); \ + if (ret != 0) \ + { \ + tensorrt_log::gLogError << "Cuda failure: " << ret << std::endl; \ + abort(); \ + } \ + } while (0) + +#define CHECK_RETURN_W_MSG(status, val, errMsg) \ + do \ + { \ + if (!(status)) \ + { \ + tensorrt_log::gLogError << errMsg << " Error in " << __FILE__ << ", function " << FN_NAME << "(), line " << __LINE__ \ + << std::endl; \ + return val; \ + } \ + } while (0) + +#undef ASSERT +#define ASSERT(condition) \ + do \ + { \ + if (!(condition)) \ + { \ + tensorrt_log::gLogError << "Assertion failure: " << #condition << std::endl; \ + abort(); \ + } \ + } while (0) + + +#define CHECK_RETURN(status, val) CHECK_RETURN_W_MSG(status, val, "") + +#define OBJ_GUARD(A) std::unique_ptr + +template +OBJ_GUARD(T) +makeObjGuard(T_ *t) { + CHECK(!(std::is_base_of::value || std::is_same::value)); + auto deleter = [](T *t) { t->destroy(); }; + return std::unique_ptr{static_cast(t), deleter}; +} + +constexpr long double operator "" _GiB(long double val) { + return val * (1 << 30); +} + +constexpr long double operator "" _MiB(long double val) { + return val * (1 << 20); +} + +constexpr long double operator "" _KiB(long double val) { + return val * (1 << 10); +} + +// These is necessary if we want to be able to write 1_GiB instead of 1.0_GiB. +// Since the return type is signed, -1_GiB will work as expected. +constexpr long long int operator "" _GiB(unsigned long long val) { + return val * (1 << 30); +} + +constexpr long long int operator "" _MiB(unsigned long long val) { + return val * (1 << 20); +} + +constexpr long long int operator "" _KiB(unsigned long long val) { + return val * (1 << 10); +} + +struct SimpleProfiler : public nvinfer1::IProfiler { + struct Record { + float time{0}; + int count{0}; + }; + + virtual void reportLayerTime(const char *layerName, float ms) noexcept { + mProfile[layerName].count++; + mProfile[layerName].time += ms; + if (std::find(mLayerNames.begin(), mLayerNames.end(), layerName) == mLayerNames.end()) { + mLayerNames.push_back(layerName); + } + } + + SimpleProfiler(const char *name, const std::vector &srcProfilers = std::vector()) + : mName(name) { + for (const auto &srcProfiler : srcProfilers) { + for (const auto &rec : srcProfiler.mProfile) { + auto it = mProfile.find(rec.first); + if (it == mProfile.end()) { + mProfile.insert(rec); + } else { + it->second.time += rec.second.time; + it->second.count += rec.second.count; + } + } + } + } + + friend std::ostream &operator<<(std::ostream &out, const SimpleProfiler &value) { + out << "========== " << value.mName << " profile ==========" << std::endl; + float totalTime = 0; + std::string layerNameStr = "TensorRT layer name"; + int maxLayerNameLength = std::max(static_cast(layerNameStr.size()), 70); + for (const auto &elem : value.mProfile) { + totalTime += elem.second.time; + maxLayerNameLength = std::max(maxLayerNameLength, static_cast(elem.first.size())); + } + + auto old_settings = out.flags(); + auto old_precision = out.precision(); + // Output header + { + out << std::setw(maxLayerNameLength) << layerNameStr << " "; + out << std::setw(12) << "Runtime, " + << "%" + << " "; + out << std::setw(12) << "Invocations" + << " "; + out << std::setw(12) << "Runtime, ms" << std::endl; + } + for (size_t i = 0; i < value.mLayerNames.size(); i++) { + const std::string layerName = value.mLayerNames[i]; + auto elem = value.mProfile.at(layerName); + out << std::setw(maxLayerNameLength) << layerName << " "; + out << std::setw(12) << std::fixed << std::setprecision(1) << (elem.time * 100.0F / totalTime) << "%" + << " "; + out << std::setw(12) << elem.count << " "; + out << std::setw(12) << std::fixed << std::setprecision(2) << elem.time << std::endl; + } + out.flags(old_settings); + out.precision(old_precision); + out << "========== " << value.mName << " total runtime = " << totalTime << " ms ==========" << std::endl; + + return out; + } + +private: + std::string mName; + std::vector mLayerNames; + std::map mProfile; +}; + +//! Locate path to file, given its filename or filepath suffix and possible dirs it might lie in. +//! Function will also walk back MAX_DEPTH dirs from CWD to check for such a file path. +inline std::string locateFile( + const std::string &filepathSuffix, const std::vector &directories, bool reportError = true) { + const int MAX_DEPTH{10}; + bool found{false}; + std::string filepath; + + for (auto &dir : directories) { + if (!dir.empty() && dir.back() != '/') { +#ifdef _MSC_VER + filepath = dir + "\\" + filepathSuffix; +#else + filepath = dir + "/" + filepathSuffix; +#endif + } else { + filepath = dir + filepathSuffix; + } + + for (int i = 0; i < MAX_DEPTH && !found; i++) { + const std::ifstream checkFile(filepath); + found = checkFile.is_open(); + if (found) { + break; + } + + filepath = "../" + filepath; // Try again in parent dir + } + + if (found) { + break; + } + + filepath.clear(); + } + + // Could not find the file + if (filepath.empty()) { + const std::string dirList = std::accumulate(directories.begin() + 1, directories.end(), directories.front(), + [](const std::string &a, const std::string &b) { + return a + "\n\t" + b; + }); + std::cout << "Could not find " << filepathSuffix << " in data directories:\n\t" << dirList << std::endl; + + if (reportError) { + std::cout << "&&&& FAILED" << std::endl; + exit(EXIT_FAILURE); + } + } + + return filepath; +} + +inline void readPGMFile(const std::string &fileName, uint8_t *buffer, int inH, int inW) { + std::ifstream infile(fileName, std::ifstream::binary); + assert(infile.is_open() && "Attempting to read from a file that is not open."); + std::string magic, h, w, max; + infile >> magic >> h >> w >> max; + infile.seekg(1, infile.cur); + infile.read(reinterpret_cast(buffer), inH * inW); +} + +namespace tensorrt_common { + +// Swaps endianness of an integral type. + template::value, int>::type = 0> + inline T swapEndianness(const T &value) { + uint8_t bytes[sizeof(T)]; + for (int i = 0; i < static_cast(sizeof(T)); ++i) { + bytes[sizeof(T) - 1 - i] = *(reinterpret_cast(&value) + i); + } + return *reinterpret_cast(bytes); + } + + class HostMemory { + public: + HostMemory() = delete; + + virtual void *data() const noexcept { + return mData; + } + + virtual std::size_t size() const noexcept { + return mSize; + } + + virtual DataType type() const noexcept { + return mType; + } + + virtual ~HostMemory() {} + + protected: + HostMemory(std::size_t size, DataType type) + : mSize(size), mType(type) { + } + + void *mData; + std::size_t mSize; + DataType mType; + }; + + template + class TypedHostMemory : public HostMemory { + public: + TypedHostMemory(std::size_t size) + : HostMemory(size, dataType) { + mData = new ElemType[size]; + }; + + ~TypedHostMemory() noexcept { + delete[](ElemType *) mData; + } + + ElemType *raw() noexcept { + return static_cast(data()); + } + }; + + using FloatMemory = TypedHostMemory; + using HalfMemory = TypedHostMemory; + using ByteMemory = TypedHostMemory; + + inline void *safeCudaMalloc(size_t memSize) { + void *deviceMem; + CHECK(cudaMalloc(&deviceMem, memSize)); + if (deviceMem == nullptr) { + std::cerr << "Out of memory" << std::endl; + exit(1); + } + return deviceMem; + } + + inline bool isDebug() { + return (std::getenv("TENSORRT_DEBUG") ? true : false); + } + + struct InferDeleter { + template + void operator()(T *obj) const { + delete obj; + } + }; + + template + using TensorRTUniquePtr = std::unique_ptr; + + static auto StreamDeleter = [](cudaStream_t *pStream) { + if (pStream) { + cudaStreamDestroy(*pStream); + delete pStream; + } + }; + + inline std::unique_ptr makeCudaStream() { + std::unique_ptr pStream(new cudaStream_t, StreamDeleter); + if (cudaStreamCreate(pStream.get()) != cudaSuccess) { + pStream.reset(nullptr); + } + + return pStream; + } + + template + std::shared_ptr infer_object(T *obj) { + if (!obj) { + throw std::runtime_error(std::string("Failed to create object")); + } + return std::shared_ptr(obj); + } + +//! Return vector of indices that puts magnitudes of sequence in descending order. + template + std::vector argMagnitudeSort(Iter begin, Iter end) { + std::vector indices(end - begin); + std::iota(indices.begin(), indices.end(), 0); + std::sort(indices.begin(), indices.end(), + [&begin](size_t i, size_t j) { return std::abs(begin[j]) < std::abs(begin[i]); }); + return indices; + } + + inline bool readReferenceFile(const std::string &fileName, std::vector &refVector) { + std::ifstream infile(fileName); + if (!infile.is_open()) { + std::cout << "ERROR: readReferenceFile: Attempting to read from a file that is not open." << std::endl; + return false; + } + std::string line; + while (std::getline(infile, line)) { + if (line.empty()) + continue; + refVector.push_back(line); + } + infile.close(); + return true; + } + + template + std::vector classify( + const std::vector &refVector, const std::vector &output, const size_t topK) { + const auto inds = tensorrt_common::argMagnitudeSort(output.cbegin(), output.cend()); + std::vector result; + result.reserve(topK); + for (size_t k = 0; k < topK; ++k) { + result.push_back(refVector[inds[k]]); + } + return result; + } + +// Returns indices of highest K magnitudes in v. + template + std::vector topKMagnitudes(const std::vector &v, const size_t k) { + std::vector indices = tensorrt_common::argMagnitudeSort(v.cbegin(), v.cend()); + indices.resize(k); + return indices; + } + + template + bool readASCIIFile(const std::string &fileName, const size_t size, std::vector &out) { + std::ifstream infile(fileName); + if (!infile.is_open()) { + std::cout << "ERROR readASCIIFile: Attempting to read from a file that is not open." << std::endl; + return false; + } + out.clear(); + out.reserve(size); + out.assign(std::istream_iterator(infile), std::istream_iterator()); + infile.close(); + return true; + } + + template + bool writeASCIIFile(const std::string &fileName, const std::vector &in) { + std::ofstream outfile(fileName); + if (!outfile.is_open()) { + std::cout << "ERROR: writeASCIIFile: Attempting to write to a file that is not open." << std::endl; + return false; + } + for (auto fn : in) { + outfile << fn << "\n"; + } + outfile.close(); + return true; + } + + inline void print_version() { + std::cout << " TensorRT version: " << NV_TENSORRT_MAJOR << "." << NV_TENSORRT_MINOR << "." << NV_TENSORRT_PATCH + << "." << NV_TENSORRT_BUILD << std::endl; + } + + inline std::string getFileType(const std::string &filepath) { + return filepath.substr(filepath.find_last_of(".") + 1); + } + + inline std::string toLower(const std::string &inp) { + std::string out = inp; + std::transform(out.begin(), out.end(), out.begin(), ::tolower); + return out; + } + + inline float getMaxValue(const float *buffer, int64_t size) { + assert(buffer != nullptr); + assert(size > 0); + return *std::max_element(buffer, buffer + size); + } + +// Ensures that every tensor used by a network has a scale. +// +// All tensors in a network must have a range specified if a calibrator is not used. +// This function is just a utility to globally fill in missing scales for the entire network. +// +// If a tensor does not have a scale, it is assigned inScales or outScales as follows: +// +// * If the tensor is the input to a layer or output of a pooling node, its scale is assigned inScales. +// * Otherwise its scale is assigned outScales. +// +// The default parameter values are intended to demonstrate, for final layers in the network, +// cases where scaling factors are asymmetric. + inline void setAllTensorScales(INetworkDefinition *network, float inScales = 2.0f, float outScales = 4.0f) { + // Ensure that all layer inputs have a scale. + for (int i = 0; i < network->getNbLayers(); i++) { + auto layer = network->getLayer(i); + for (int j = 0; j < layer->getNbInputs(); j++) { + ITensor *input{layer->getInput(j)}; + // Optional inputs are nullptr here and are from RNN layers. + if (input != nullptr && !input->dynamicRangeIsSet()) { + input->setDynamicRange(-inScales, inScales); + } + } + } + + // Ensure that all layer outputs have a scale. + // Tensors that are also inputs to layers are ingored here + // since the previous loop nest assigned scales to them. + for (int i = 0; i < network->getNbLayers(); i++) { + auto layer = network->getLayer(i); + for (int j = 0; j < layer->getNbOutputs(); j++) { + ITensor *output{layer->getOutput(j)}; + // Optional outputs are nullptr here and are from RNN layers. + if (output != nullptr && !output->dynamicRangeIsSet()) { + // Pooling must have the same input and output scales. + if (layer->getType() == LayerType::kPOOLING) { + output->setDynamicRange(-inScales, inScales); + } else { + output->setDynamicRange(-outScales, outScales); + } + } + } + } + } + + inline void setAllDynamicRanges(INetworkDefinition *network, float inRange = 2.0f, float outRange = 4.0f) { + return setAllTensorScales(network, inRange, outRange); + } + + inline void setDummyInt8DynamicRanges(const IBuilderConfig *c, INetworkDefinition *n) { + // Set dummy per-tensor dynamic range if Int8 mode is requested. + if (c->getFlag(BuilderFlag::kINT8)) { + tensorrt_log::gLogWarning + << "Int8 calibrator not provided. Generating dummy per-tensor dynamic range. Int8 accuracy is not guaranteed." + << std::endl; + setAllDynamicRanges(n); + } + } + + inline void enableDLA(IBuilder *builder, IBuilderConfig *config, int useDLACore, bool allowGPUFallback = true) { + if (useDLACore >= 0) { + if (builder->getNbDLACores() == 0) { + std::cerr << "Trying to use DLA core " << useDLACore << " on a platform that doesn't have any DLA cores" + << std::endl; + assert("Error: use DLA core on a platfrom that doesn't have any DLA cores" && false); + } + if (allowGPUFallback) { + config->setFlag(BuilderFlag::kGPU_FALLBACK); + } + if (!config->getFlag(BuilderFlag::kINT8)) { + // User has not requested INT8 Mode. + // By default run in FP16 mode. FP32 mode is not permitted. + config->setFlag(BuilderFlag::kFP16); + } + config->setDefaultDeviceType(DeviceType::kDLA); + config->setDLACore(useDLACore); + config->setFlag(BuilderFlag::kSTRICT_TYPES); + } + } + + inline int parseDLA(int argc, char **argv) { + for (int i = 1; i < argc; i++) { + std::string arg(argv[i]); + if (strncmp(argv[i], "--useDLACore=", 13) == 0) + return std::stoi(argv[i] + 13); + } + return -1; + } + + inline uint32_t getElementSize(nvinfer1::DataType t) noexcept { + switch (t) { + case nvinfer1::DataType::kINT32: + return 4; + case nvinfer1::DataType::kFLOAT: + return 4; + case nvinfer1::DataType::kHALF: + return 2; + case nvinfer1::DataType::kBOOL: + case nvinfer1::DataType::kINT8: + return 1; + } + return 0; + } + + inline int64_t volume(const nvinfer1::Dims &d) { + return std::accumulate(d.d, d.d + d.nbDims, 1, std::multiplies()); + } + + inline uint32_t elementSize(DataType t) noexcept { + switch (t) { + case DataType::kINT32: + case DataType::kFLOAT: + return 4; + case DataType::kHALF: + return 2; + case DataType::kBOOL: + case DataType::kINT8: + return 1; + } + return 0; + } + + template + inline A divUp(A x, B n) { + return (x + n - 1) / n; + } + + template + struct PPM { + std::string magic, fileName; + int h, w, max; + uint8_t buffer[C * H * W]; + }; + +// New vPPM(variable sized PPM) class with variable dimensions. + struct vPPM { + std::string magic, fileName; + int h, w, max; + std::vector buffer; + }; + + struct BBox { + float x1, y1, x2, y2; + }; + + template + void readPPMFile(const std::string &filename, tensorrt_common::PPM &ppm) { + ppm.fileName = filename; + std::ifstream infile(filename, std::ifstream::binary); + assert(infile.is_open() && "Attempting to read from a file that is not open."); + infile >> ppm.magic >> ppm.w >> ppm.h >> ppm.max; + infile.seekg(1, infile.cur); + infile.read(reinterpret_cast(ppm.buffer), ppm.w * ppm.h * 3); + } + + inline void readPPMFile(const std::string &filename, vPPM &ppm, std::vector &input_dir) { + ppm.fileName = filename; + std::ifstream infile(locateFile(filename, input_dir), std::ifstream::binary); + infile >> ppm.magic >> ppm.w >> ppm.h >> ppm.max; + infile.seekg(1, infile.cur); + + for (int i = 0; i < ppm.w * ppm.h * 3; ++i) { + ppm.buffer.push_back(0); + } + + infile.read(reinterpret_cast(&ppm.buffer[0]), ppm.w * ppm.h * 3); + } + + template + void writePPMFileWithBBox(const std::string &filename, PPM &ppm, const BBox &bbox) { + std::ofstream outfile("./" + filename, std::ofstream::binary); + assert(!outfile.fail()); + outfile << "P6" + << "\n" + << ppm.w << " " << ppm.h << "\n" + << ppm.max << "\n"; + + auto round = [](float x) -> int { return int(std::floor(x + 0.5f)); }; + const int x1 = std::min(std::max(0, round(int(bbox.x1))), W - 1); + const int x2 = std::min(std::max(0, round(int(bbox.x2))), W - 1); + const int y1 = std::min(std::max(0, round(int(bbox.y1))), H - 1); + const int y2 = std::min(std::max(0, round(int(bbox.y2))), H - 1); + + for (int x = x1; x <= x2; ++x) { + // bbox top border + ppm.buffer[(y1 * ppm.w + x) * 3] = 255; + ppm.buffer[(y1 * ppm.w + x) * 3 + 1] = 0; + ppm.buffer[(y1 * ppm.w + x) * 3 + 2] = 0; + // bbox bottom border + ppm.buffer[(y2 * ppm.w + x) * 3] = 255; + ppm.buffer[(y2 * ppm.w + x) * 3 + 1] = 0; + ppm.buffer[(y2 * ppm.w + x) * 3 + 2] = 0; + } + + for (int y = y1; y <= y2; ++y) { + // bbox left border + ppm.buffer[(y * ppm.w + x1) * 3] = 255; + ppm.buffer[(y * ppm.w + x1) * 3 + 1] = 0; + ppm.buffer[(y * ppm.w + x1) * 3 + 2] = 0; + // bbox right border + ppm.buffer[(y * ppm.w + x2) * 3] = 255; + ppm.buffer[(y * ppm.w + x2) * 3 + 1] = 0; + ppm.buffer[(y * ppm.w + x2) * 3 + 2] = 0; + } + + outfile.write(reinterpret_cast(ppm.buffer), ppm.w * ppm.h * 3); + } + + inline void writePPMFileWithBBox(const std::string &filename, vPPM ppm, std::vector &dets) { + std::ofstream outfile("./" + filename, std::ofstream::binary); + assert(!outfile.fail()); + outfile << "P6" + << "\n" + << ppm.w << " " << ppm.h << "\n" + << ppm.max << "\n"; + auto round = [](float x) -> int { return int(std::floor(x + 0.5f)); }; + + for (auto bbox : dets) { + for (int x = int(bbox.x1); x < int(bbox.x2); ++x) { + // bbox top border + ppm.buffer[(round(bbox.y1) * ppm.w + x) * 3] = 255; + ppm.buffer[(round(bbox.y1) * ppm.w + x) * 3 + 1] = 0; + ppm.buffer[(round(bbox.y1) * ppm.w + x) * 3 + 2] = 0; + // bbox bottom border + ppm.buffer[(round(bbox.y2) * ppm.w + x) * 3] = 255; + ppm.buffer[(round(bbox.y2) * ppm.w + x) * 3 + 1] = 0; + ppm.buffer[(round(bbox.y2) * ppm.w + x) * 3 + 2] = 0; + } + + for (int y = int(bbox.y1); y < int(bbox.y2); ++y) { + // bbox left border + ppm.buffer[(y * ppm.w + round(bbox.x1)) * 3] = 255; + ppm.buffer[(y * ppm.w + round(bbox.x1)) * 3 + 1] = 0; + ppm.buffer[(y * ppm.w + round(bbox.x1)) * 3 + 2] = 0; + // bbox right border + ppm.buffer[(y * ppm.w + round(bbox.x2)) * 3] = 255; + ppm.buffer[(y * ppm.w + round(bbox.x2)) * 3 + 1] = 0; + ppm.buffer[(y * ppm.w + round(bbox.x2)) * 3 + 2] = 0; + } + } + + outfile.write(reinterpret_cast(&ppm.buffer[0]), ppm.w * ppm.h * 3); + } + + class TimerBase { + public: + virtual void start() {} + + virtual void stop() {} + + float microseconds() const noexcept { + return mMs * 1000.f; + } + + float milliseconds() const noexcept { + return mMs; + } + + float seconds() const noexcept { + return mMs / 1000.f; + } + + void reset() noexcept { + mMs = 0.f; + } + + protected: + float mMs{0.0f}; + }; + + class GpuTimer : public TimerBase { + public: + GpuTimer(cudaStream_t stream) + : mStream(stream) { + CHECK(cudaEventCreate(&mStart)); + CHECK(cudaEventCreate(&mStop)); + } + + ~GpuTimer() { + CHECK(cudaEventDestroy(mStart)); + CHECK(cudaEventDestroy(mStop)); + } + + void start() { + CHECK(cudaEventRecord(mStart, mStream)); + } + + void stop() { + CHECK(cudaEventRecord(mStop, mStream)); + float ms{0.0f}; + CHECK(cudaEventSynchronize(mStop)); + CHECK(cudaEventElapsedTime(&ms, mStart, mStop)); + mMs += ms; + } + + private: + cudaEvent_t mStart, mStop; + cudaStream_t mStream; + }; // class GpuTimer + + template + class CpuTimer : public TimerBase { + public: + using clock_type = Clock; + + void start() { + mStart = Clock::now(); + } + + void stop() { + mStop = Clock::now(); + mMs += std::chrono::duration{mStop - mStart}.count(); + } + + private: + std::chrono::time_point mStart, mStop; + }; // class CpuTimer + + using PreciseCpuTimer = CpuTimer; + + inline std::vector splitString(std::string str, char delimiter = ',') { + std::vector splitVect; + std::stringstream ss(str); + std::string substr; + + while (ss.good()) { + getline(ss, substr, delimiter); + splitVect.emplace_back(std::move(substr)); + } + return splitVect; + } + +// Return m rounded up to nearest multiple of n + inline int roundUp(int m, int n) { + return ((m + n - 1) / n) * n; + } + + inline int getC(const Dims &d) { + return d.nbDims >= 3 ? d.d[d.nbDims - 3] : 1; + } + + inline int getH(const Dims &d) { + return d.nbDims >= 2 ? d.d[d.nbDims - 2] : 1; + } + + inline int getW(const Dims &d) { + return d.nbDims >= 1 ? d.d[d.nbDims - 1] : 1; + } + + inline void loadLibrary(const std::string &path) { +#ifdef _MSC_VER + void* handle = LoadLibrary(path.c_str()); +#else + void *handle = dlopen(path.c_str(), RTLD_LAZY); +#endif + if (handle == nullptr) { +#ifdef _MSC_VER + tensorrt_log::gLogError << "Could not load plugin library: " << path << std::endl; +#else + tensorrt_log::gLogError << "Could not load plugin library: " << path << ", due to: " << dlerror() + << std::endl; +#endif + } + } + + inline int32_t getSMVersion() { + int32_t deviceIndex = 0; + CHECK(cudaGetDevice(&deviceIndex)); + + int32_t major, minor; + CHECK(cudaDeviceGetAttribute(&major, cudaDevAttrComputeCapabilityMajor, deviceIndex)); + CHECK(cudaDeviceGetAttribute(&minor, cudaDevAttrComputeCapabilityMinor, deviceIndex)); + + return ((major << 8) | minor); + } + + inline bool isSMSafe() { + const int32_t smVersion = getSMVersion(); + return smVersion == 0x0700 || smVersion == 0x0702 || smVersion == 0x0705; + } +} // namespace tensorrt_common + +inline std::ostream &operator<<(std::ostream &os, const nvinfer1::Dims &dims) { + os << "("; + for (int i = 0; i < dims.nbDims; ++i) { + os << (i ? ", " : "") << dims.d[i]; + } + return os << ")"; +} + +#endif // TENSORRT_COMMON_H diff --git a/tensorrt_utils/include/tensorrt_utils/error_recorder.h b/tensorrt_utils/include/tensorrt_utils/error_recorder.h new file mode 100644 index 00000000..1707a449 --- /dev/null +++ b/tensorrt_utils/include/tensorrt_utils/error_recorder.h @@ -0,0 +1,139 @@ +/* + * Copyright (c) 2021, NVIDIA CORPORATION. All rights reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ERROR_RECORDER_H +#define ERROR_RECORDER_H + +#include +#include +#include +#include +#include +#include + +#if NV_IS_SAFETY +#include +#endif +using namespace nvinfer1; + +//! +//! A simple implementation of the IErrorRecorder interface for +//! use by samples. This interface also can be used as a reference +//! implementation. +//! The sample Error recorder is based on a vector that pairs the error +//! code and the error string into a single element. It also uses +//! standard mutex's and atomics in order to make sure that the code +//! works in a multi-threaded environment. +//! SampleErrorRecorder is not intended for use in automotive safety +//! environments. +//! +class TensorRTErrorRecorder : public IErrorRecorder { + using errorPair = std::pair; + using errorStack = std::vector; + +public: + TensorRTErrorRecorder() = default; + + virtual ~TensorRTErrorRecorder() noexcept {} + + int32_t getNbErrors() const noexcept final { + return mErrorStack.size(); + } + + ErrorCode getErrorCode(int32_t errorIdx) const noexcept final { + return invalidIndexCheck(errorIdx) ? ErrorCode::kINVALID_ARGUMENT : (*this)[errorIdx].first; + }; + + IErrorRecorder::ErrorDesc getErrorDesc(int32_t errorIdx) const noexcept final { + return invalidIndexCheck(errorIdx) ? "errorIdx out of range." : (*this)[errorIdx].second.c_str(); + } + + // This class can never overflow since we have dynamic resize via std::vector usage. + bool hasOverflowed() const noexcept final { + return false; + } + + // Empty the errorStack. + void clear() noexcept final { + try { + // grab a lock so that there is no addition while clearing. + std::lock_guard guard(mStackLock); + mErrorStack.clear(); + } + catch (const std::exception &e) { +#if NV_IS_SAFETY + std::cerr << "Internal Error: " << e.what() << std::endl; +#else + getLogger()->log(ILogger::Severity::kINTERNAL_ERROR, e.what()); +#endif + } + }; + + //! Simple helper function that + bool empty() const noexcept { + return mErrorStack.empty(); + } + + bool reportError(ErrorCode val, IErrorRecorder::ErrorDesc desc) noexcept final { + try { + std::lock_guard guard(mStackLock); + tensorrt_log::gLogError << "Error[" << static_cast(val) << "]: " << desc << std::endl; + mErrorStack.push_back(errorPair(val, desc)); + } + catch (const std::exception &e) { +#if NV_IS_SAFETY + std::cerr << "Internal Error: " << e.what() << std::endl; +#else + getLogger()->log(ILogger::Severity::kINTERNAL_ERROR, e.what()); +#endif + } + // All errors are considered fatal. + return true; + } + + // Atomically increment or decrement the ref counter. + IErrorRecorder::RefCount incRefCount() noexcept final { + return ++mRefCount; + } + + IErrorRecorder::RefCount decRefCount() noexcept final { + return --mRefCount; + } + +private: + // Simple helper functions. + const errorPair &operator[](size_t index) const noexcept { + return mErrorStack[index]; + } + + bool invalidIndexCheck(int32_t index) const noexcept { + // By converting signed to unsigned, we only need a single check since + // negative numbers turn into large positive greater than the size. + size_t sIndex = index; + return sIndex >= mErrorStack.size(); + } + + // Mutex to hold when locking mErrorStack. + std::mutex mStackLock; + + // Reference count of the class. Destruction of the class when mRefCount + // is not zero causes undefined behavior. + std::atomic mRefCount{0}; + + // The error stack that holds the errors recorded by TensorRT. + errorStack mErrorStack; +}; // class TensorRTErrorRecorder +#endif // ERROR_RECORDER_H diff --git a/tensorrt_utils/include/tensorrt_utils/half.h b/tensorrt_utils/include/tensorrt_utils/half.h new file mode 100644 index 00000000..4f7f6809 --- /dev/null +++ b/tensorrt_utils/include/tensorrt_utils/half.h @@ -0,0 +1,4317 @@ +// half - IEEE 754-based half-precision floating point library. +// +// Copyright (c) 2012-2017 Christian Rau +// +// Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated +// documentation files (the "Software"), to deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to +// permit persons to whom the Software is furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all copies or substantial portions of the +// Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE +// WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR +// COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +// OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + +/* + * Copyright (c) 2021, NVIDIA CORPORATION. All rights reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +// Version 1.12.0 + +/// \file +/// Main header file for half precision functionality. + +#ifndef HALF_HALF_HPP +#define HALF_HALF_HPP + +/// Combined gcc version number. +#define HALF_GNUC_VERSION (__GNUC__ * 100 + __GNUC_MINOR__) + +// check C++11 language features +#if defined(__clang__) // clang + #if __has_feature(cxx_static_assert) && !defined(HALF_ENABLE_CPP11_STATIC_ASSERT) +#define HALF_ENABLE_CPP11_STATIC_ASSERT 1 +#endif +#if __has_feature(cxx_constexpr) && !defined(HALF_ENABLE_CPP11_CONSTEXPR) +#define HALF_ENABLE_CPP11_CONSTEXPR 1 +#endif +#if __has_feature(cxx_noexcept) && !defined(HALF_ENABLE_CPP11_NOEXCEPT) +#define HALF_ENABLE_CPP11_NOEXCEPT 1 +#endif +#if __has_feature(cxx_user_literals) && !defined(HALF_ENABLE_CPP11_USER_LITERALS) +#define HALF_ENABLE_CPP11_USER_LITERALS 1 +#endif +#if (defined(__GXX_EXPERIMENTAL_CXX0X__) || __cplusplus >= 201103L) && !defined(HALF_ENABLE_CPP11_LONG_LONG) +#define HALF_ENABLE_CPP11_LONG_LONG 1 +#endif +/*#elif defined(__INTEL_COMPILER) //Intel C++ + #if __INTEL_COMPILER >= 1100 && !defined(HALF_ENABLE_CPP11_STATIC_ASSERT) ???????? + #define HALF_ENABLE_CPP11_STATIC_ASSERT 1 + #endif + #if __INTEL_COMPILER >= 1300 && !defined(HALF_ENABLE_CPP11_CONSTEXPR) ???????? + #define HALF_ENABLE_CPP11_CONSTEXPR 1 + #endif + #if __INTEL_COMPILER >= 1300 && !defined(HALF_ENABLE_CPP11_NOEXCEPT) ???????? + #define HALF_ENABLE_CPP11_NOEXCEPT 1 + #endif + #if __INTEL_COMPILER >= 1100 && !defined(HALF_ENABLE_CPP11_LONG_LONG) ???????? + #define HALF_ENABLE_CPP11_LONG_LONG 1 + #endif*/ +#elif defined(__GNUC__) // gcc +#if defined(__GXX_EXPERIMENTAL_CXX0X__) || __cplusplus >= 201103L +#if HALF_GNUC_VERSION >= 403 && !defined(HALF_ENABLE_CPP11_STATIC_ASSERT) +#define HALF_ENABLE_CPP11_STATIC_ASSERT 1 +#endif +#if HALF_GNUC_VERSION >= 406 && !defined(HALF_ENABLE_CPP11_CONSTEXPR) +#define HALF_ENABLE_CPP11_CONSTEXPR 1 +#endif +#if HALF_GNUC_VERSION >= 406 && !defined(HALF_ENABLE_CPP11_NOEXCEPT) +#define HALF_ENABLE_CPP11_NOEXCEPT 1 +#endif +#if HALF_GNUC_VERSION >= 407 && !defined(HALF_ENABLE_CPP11_USER_LITERALS) +#define HALF_ENABLE_CPP11_USER_LITERALS 1 +#endif +#if !defined(HALF_ENABLE_CPP11_LONG_LONG) +#define HALF_ENABLE_CPP11_LONG_LONG 1 +#endif +#endif +#elif defined(_MSC_VER) // Visual C++ + #if _MSC_VER >= 1900 && !defined(HALF_ENABLE_CPP11_CONSTEXPR) +#define HALF_ENABLE_CPP11_CONSTEXPR 1 +#endif +#if _MSC_VER >= 1900 && !defined(HALF_ENABLE_CPP11_NOEXCEPT) +#define HALF_ENABLE_CPP11_NOEXCEPT 1 +#endif +#if _MSC_VER >= 1900 && !defined(HALF_ENABLE_CPP11_USER_LITERALS) +#define HALF_ENABLE_CPP11_USER_LITERALS 1 +#endif +#if _MSC_VER >= 1600 && !defined(HALF_ENABLE_CPP11_STATIC_ASSERT) +#define HALF_ENABLE_CPP11_STATIC_ASSERT 1 +#endif +#if _MSC_VER >= 1310 && !defined(HALF_ENABLE_CPP11_LONG_LONG) +#define HALF_ENABLE_CPP11_LONG_LONG 1 +#endif +#define HALF_POP_WARNINGS 1 +#pragma warning(push) +#pragma warning(disable : 4099 4127 4146) // struct vs class, constant in if, negative unsigned +#endif + +// check C++11 library features +#include + +#if defined(_LIBCPP_VERSION) // libc++ + #if defined(__GXX_EXPERIMENTAL_CXX0X__) || __cplusplus >= 201103 +#ifndef HALF_ENABLE_CPP11_TYPE_TRAITS +#define HALF_ENABLE_CPP11_TYPE_TRAITS 1 +#endif +#ifndef HALF_ENABLE_CPP11_CSTDINT +#define HALF_ENABLE_CPP11_CSTDINT 1 +#endif +#ifndef HALF_ENABLE_CPP11_CMATH +#define HALF_ENABLE_CPP11_CMATH 1 +#endif +#ifndef HALF_ENABLE_CPP11_HASH +#define HALF_ENABLE_CPP11_HASH 1 +#endif +#endif +#elif defined(__GLIBCXX__) // libstdc++ +#if defined(__GXX_EXPERIMENTAL_CXX0X__) || __cplusplus >= 201103 +#ifdef __clang__ + #if __GLIBCXX__ >= 20080606 && !defined(HALF_ENABLE_CPP11_TYPE_TRAITS) +#define HALF_ENABLE_CPP11_TYPE_TRAITS 1 +#endif +#if __GLIBCXX__ >= 20080606 && !defined(HALF_ENABLE_CPP11_CSTDINT) +#define HALF_ENABLE_CPP11_CSTDINT 1 +#endif +#if __GLIBCXX__ >= 20080606 && !defined(HALF_ENABLE_CPP11_CMATH) +#define HALF_ENABLE_CPP11_CMATH 1 +#endif +#if __GLIBCXX__ >= 20080606 && !defined(HALF_ENABLE_CPP11_HASH) +#define HALF_ENABLE_CPP11_HASH 1 +#endif +#else +#if HALF_GNUC_VERSION >= 403 && !defined(HALF_ENABLE_CPP11_CSTDINT) +#define HALF_ENABLE_CPP11_CSTDINT 1 +#endif +#if HALF_GNUC_VERSION >= 403 && !defined(HALF_ENABLE_CPP11_CMATH) +#define HALF_ENABLE_CPP11_CMATH 1 +#endif +#if HALF_GNUC_VERSION >= 403 && !defined(HALF_ENABLE_CPP11_HASH) +#define HALF_ENABLE_CPP11_HASH 1 +#endif +#endif +#endif +#elif defined(_CPPLIB_VER) // Dinkumware/Visual C++ + #if _CPPLIB_VER >= 520 +#ifndef HALF_ENABLE_CPP11_TYPE_TRAITS +#define HALF_ENABLE_CPP11_TYPE_TRAITS 1 +#endif +#ifndef HALF_ENABLE_CPP11_CSTDINT +#define HALF_ENABLE_CPP11_CSTDINT 1 +#endif +#ifndef HALF_ENABLE_CPP11_HASH +#define HALF_ENABLE_CPP11_HASH 1 +#endif +#endif +#if _CPPLIB_VER >= 610 +#ifndef HALF_ENABLE_CPP11_CMATH +#define HALF_ENABLE_CPP11_CMATH 1 +#endif +#endif +#endif +#undef HALF_GNUC_VERSION + +// support constexpr +#if HALF_ENABLE_CPP11_CONSTEXPR +#define HALF_CONSTEXPR constexpr +#define HALF_CONSTEXPR_CONST constexpr +#else + #define HALF_CONSTEXPR +#define HALF_CONSTEXPR_CONST const +#endif + +// support noexcept +#if HALF_ENABLE_CPP11_NOEXCEPT +#define HALF_NOEXCEPT noexcept +#define HALF_NOTHROW noexcept +#else + #define HALF_NOEXCEPT +#define HALF_NOTHROW throw() +#endif + +#include +#include +#include +#include +#include +#include + +#if HALF_ENABLE_CPP11_TYPE_TRAITS +#include +#endif +#if HALF_ENABLE_CPP11_CSTDINT + +#include + +#endif +#if HALF_ENABLE_CPP11_HASH + +#include + +#endif + +/// Default rounding mode. +/// This specifies the rounding mode used for all conversions between [half](\ref half_float::half)s and `float`s as +/// well as for the half_cast() if not specifying a rounding mode explicitly. It can be redefined (before including +/// half.hpp) to one of the standard rounding modes using their respective constants or the equivalent values of +/// `std::float_round_style`: +/// +/// `std::float_round_style` | value | rounding +/// ---------------------------------|-------|------------------------- +/// `std::round_indeterminate` | -1 | fastest (default) +/// `std::round_toward_zero` | 0 | toward zero +/// `std::round_to_nearest` | 1 | to nearest +/// `std::round_toward_infinity` | 2 | toward positive infinity +/// `std::round_toward_neg_infinity` | 3 | toward negative infinity +/// +/// By default this is set to `-1` (`std::round_indeterminate`), which uses truncation (round toward zero, but with +/// overflows set to infinity) and is the fastest rounding mode possible. It can even be set to +/// `std::numeric_limits::round_style` to synchronize the rounding mode with that of the underlying +/// single-precision implementation. +#ifndef HALF_ROUND_STYLE +#define HALF_ROUND_STYLE 1 // = std::round_to_nearest +#endif + +/// Tie-breaking behaviour for round to nearest. +/// This specifies if ties in round to nearest should be resolved by rounding to the nearest even value. By default this +/// is defined to `0` resulting in the faster but slightly more biased behaviour of rounding away from zero in half-way +/// cases (and thus equal to the round() function), but can be redefined to `1` (before including half.hpp) if more +/// IEEE-conformant behaviour is needed. +#ifndef HALF_ROUND_TIES_TO_EVEN +#define HALF_ROUND_TIES_TO_EVEN 0 // ties away from zero +#endif + +/// Value signaling overflow. +/// In correspondence with `HUGE_VAL[F|L]` from `` this symbol expands to a positive value signaling the overflow +/// of an operation, in particular it just evaluates to positive infinity. +#define HUGE_VALH std::numeric_limits::infinity() + +/// Fast half-precision fma function. +/// This symbol is only defined if the fma() function generally executes as fast as, or faster than, a separate +/// half-precision multiplication followed by an addition. Due to the internal single-precision implementation of all +/// arithmetic operations, this is in fact always the case. +#define FP_FAST_FMAH 1 + +#ifndef FP_ILOGB0 +#define FP_ILOGB0 INT_MIN +#endif +#ifndef FP_ILOGBNAN +#define FP_ILOGBNAN INT_MAX +#endif +#ifndef FP_SUBNORMAL +#define FP_SUBNORMAL 0 +#endif +#ifndef FP_ZERO +#define FP_ZERO 1 +#endif +#ifndef FP_NAN +#define FP_NAN 2 +#endif +#ifndef FP_INFINITE +#define FP_INFINITE 3 +#endif +#ifndef FP_NORMAL +#define FP_NORMAL 4 +#endif + +/// Main namespace for half precision functionality. +/// This namespace contains all the functionality provided by the library. +namespace half_float { + class half; + +#if HALF_ENABLE_CPP11_USER_LITERALS +/// Library-defined half-precision literals. +/// Import this namespace to enable half-precision floating point literals: +/// ~~~~{.cpp} +/// using namespace half_float::literal; +/// half_float::half = 4.2_h; +/// ~~~~ + namespace literal { + half operator "" _h(long double); + } +#endif + +/// \internal +/// \brief Implementation details. + namespace detail { +#if HALF_ENABLE_CPP11_TYPE_TRAITS + /// Conditional type. +template +struct conditional : std::conditional +{ +}; + +/// Helper for tag dispatching. +template +struct bool_type : std::integral_constant +{ +}; +using std::false_type; +using std::true_type; + +/// Type traits for floating point types. +template +struct is_float : std::is_floating_point +{ +}; +#else +/// Conditional type. + template + struct conditional { + typedef T type; + }; + template + struct conditional { + typedef F type; + }; + +/// Helper for tag dispatching. + template + struct bool_type { + }; + typedef bool_type true_type; + typedef bool_type false_type; + +/// Type traits for floating point types. + template + struct is_float : false_type { + }; + template + struct is_float : is_float { + }; + template + struct is_float : is_float { + }; + template + struct is_float : is_float { + }; + template<> + struct is_float : true_type { + }; + template<> + struct is_float : true_type { + }; + template<> + struct is_float : true_type { + }; +#endif + +/// Type traits for floating point bits. + template + struct bits { + typedef unsigned char type; + }; + template + struct bits : bits { + }; + template + struct bits : bits { + }; + template + struct bits : bits { + }; + +#if HALF_ENABLE_CPP11_CSTDINT +/// Unsigned integer of (at least) 16 bits width. + typedef std::uint_least16_t uint16; + +/// Unsigned integer of (at least) 32 bits width. + template<> + struct bits { + typedef std::uint_least32_t type; + }; + +/// Unsigned integer of (at least) 64 bits width. + template<> + struct bits { + typedef std::uint_least64_t type; + }; +#else + /// Unsigned integer of (at least) 16 bits width. +typedef unsigned short uint16; + +/// Unsigned integer of (at least) 32 bits width. +template <> +struct bits : conditional::digits >= 32, unsigned int, unsigned long> +{ +}; + +#if HALF_ENABLE_CPP11_LONG_LONG +/// Unsigned integer of (at least) 64 bits width. +template <> +struct bits : conditional::digits >= 64, unsigned long, unsigned long long> +{ +}; +#else +/// Unsigned integer of (at least) 64 bits width. +template <> +struct bits +{ + typedef unsigned long type; +}; +#endif +#endif + +/// Tag type for binary construction. + struct binary_t { + }; + +/// Tag for binary construction. + HALF_CONSTEXPR_CONST binary_t binary = binary_t(); + +/// Temporary half-precision expression. +/// This class represents a half-precision expression which just stores a single-precision value internally. + struct expr { + /// Conversion constructor. + /// \param f single-precision value to convert + explicit HALF_CONSTEXPR expr(float f) HALF_NOEXCEPT: value_(f) {} + + /// Conversion to single-precision. + /// \return single precision value representing expression value + HALF_CONSTEXPR operator float() const HALF_NOEXCEPT { + return value_; + } + + private: + /// Internal expression value stored in single-precision. + float value_; + }; + +/// SFINAE helper for generic half-precision functions. +/// This class template has to be specialized for each valid combination of argument types to provide a corresponding +/// `type` member equivalent to \a T. +/// \tparam T type to return + template + struct enable { + }; + template + struct enable { + typedef T type; + }; + template + struct enable { + typedef T type; + }; + template + struct enable { + typedef T type; + }; + template + struct enable { + typedef T type; + }; + template + struct enable { + typedef T type; + }; + template + struct enable { + typedef T type; + }; + template + struct enable { + typedef T type; + }; + template + struct enable { + typedef T type; + }; + template + struct enable { + typedef T type; + }; + template + struct enable { + typedef T type; + }; + template + struct enable { + typedef T type; + }; + template + struct enable { + typedef T type; + }; + template + struct enable { + typedef T type; + }; + template + struct enable { + typedef T type; + }; + +/// Return type for specialized generic 2-argument half-precision functions. +/// This class template has to be specialized for each valid combination of argument types to provide a corresponding +/// `type` member denoting the appropriate return type. +/// \tparam T first argument type +/// \tparam U first argument type + template + struct result : enable { + }; + template<> + struct result { + typedef half type; + }; + +/// \name Classification helpers +/// \{ + +/// Check for infinity. +/// \tparam T argument type (builtin floating point type) +/// \param arg value to query +/// \retval true if infinity +/// \retval false else + template + bool builtin_isinf(T arg) { +#if HALF_ENABLE_CPP11_CMATH + return std::isinf(arg); +#elif defined(_MSC_VER) + return !::_finite(static_cast(arg)) && !::_isnan(static_cast(arg)); +#else + return arg == std::numeric_limits::infinity() || arg == -std::numeric_limits::infinity(); +#endif + } + +/// Check for NaN. +/// \tparam T argument type (builtin floating point type) +/// \param arg value to query +/// \retval true if not a number +/// \retval false else + template + bool builtin_isnan(T arg) { +#if HALF_ENABLE_CPP11_CMATH + return std::isnan(arg); +#elif defined(_MSC_VER) + return ::_isnan(static_cast(arg)) != 0; +#else + return arg != arg; +#endif + } + +/// Check sign. +/// \tparam T argument type (builtin floating point type) +/// \param arg value to query +/// \retval true if signbit set +/// \retval false else + template + bool builtin_signbit(T arg) { +#if HALF_ENABLE_CPP11_CMATH + return std::signbit(arg); +#else + return arg < T() || (arg == T() && T(1) / arg < T()); +#endif + } + +/// \} +/// \name Conversion +/// \{ + +/// Convert IEEE single-precision to half-precision. +/// Credit for this goes to [Jeroen van der Zijp](ftp://ftp.fox-toolkit.org/pub/fasthalffloatconversion.pdf). +/// \tparam R rounding mode to use, `std::round_indeterminate` for fastest rounding +/// \param value single-precision value +/// \return binary representation of half-precision value + template + uint16 float2half_impl(float value, true_type) { + typedef bits::type uint32; + uint32 bits; // = *reinterpret_cast(&value); //violating strict aliasing! + std::memcpy(&bits, &value, sizeof(float)); + /* uint16 hbits = (bits>>16) & 0x8000; + bits &= 0x7FFFFFFF; + int exp = bits >> 23; + if(exp == 255) + return hbits | 0x7C00 | (0x3FF&-static_cast((bits&0x7FFFFF)!=0)); + if(exp > 142) + { + if(R == std::round_toward_infinity) + return hbits | 0x7C00 - (hbits>>15); + if(R == std::round_toward_neg_infinity) + return hbits | 0x7BFF + (hbits>>15); + return hbits | 0x7BFF + (R!=std::round_toward_zero); + } + int g, s; + if(exp > 112) + { + g = (bits>>12) & 1; + s = (bits&0xFFF) != 0; + hbits |= ((exp-112)<<10) | ((bits>>13)&0x3FF); + } + else if(exp > 101) + { + int i = 125 - exp; + bits = (bits&0x7FFFFF) | 0x800000; + g = (bits>>i) & 1; + s = (bits&((1L<> (i+1); + } + else + { + g = 0; + s = bits != 0; + } + if(R == std::round_to_nearest) + #if HALF_ROUND_TIES_TO_EVEN + hbits += g & (s|hbits); + #else + hbits += g; + #endif + else if(R == std::round_toward_infinity) + hbits += ~(hbits>>15) & (s|g); + else if(R == std::round_toward_neg_infinity) + hbits += (hbits>>15) & (g|s); + */ + static const uint16 base_table[512] = {0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, + 0x0000, 0x0000, 0x0001, 0x0002, 0x0004, 0x0008, + 0x0010, 0x0020, 0x0040, 0x0080, 0x0100, 0x0200, 0x0400, 0x0800, + 0x0C00, 0x1000, 0x1400, 0x1800, 0x1C00, 0x2000, + 0x2400, 0x2800, 0x2C00, 0x3000, 0x3400, 0x3800, 0x3C00, 0x4000, + 0x4400, 0x4800, 0x4C00, 0x5000, 0x5400, 0x5800, + 0x5C00, 0x6000, 0x6400, 0x6800, 0x6C00, 0x7000, 0x7400, 0x7800, + 0x7C00, 0x7C00, 0x7C00, 0x7C00, 0x7C00, 0x7C00, + 0x7C00, 0x7C00, 0x7C00, 0x7C00, 0x7C00, 0x7C00, 0x7C00, 0x7C00, + 0x7C00, 0x7C00, 0x7C00, 0x7C00, 0x7C00, 0x7C00, + 0x7C00, 0x7C00, 0x7C00, 0x7C00, 0x7C00, 0x7C00, 0x7C00, 0x7C00, + 0x7C00, 0x7C00, 0x7C00, 0x7C00, 0x7C00, 0x7C00, + 0x7C00, 0x7C00, 0x7C00, 0x7C00, 0x7C00, 0x7C00, 0x7C00, 0x7C00, + 0x7C00, 0x7C00, 0x7C00, 0x7C00, 0x7C00, 0x7C00, + 0x7C00, 0x7C00, 0x7C00, 0x7C00, 0x7C00, 0x7C00, 0x7C00, 0x7C00, + 0x7C00, 0x7C00, 0x7C00, 0x7C00, 0x7C00, 0x7C00, + 0x7C00, 0x7C00, 0x7C00, 0x7C00, 0x7C00, 0x7C00, 0x7C00, 0x7C00, + 0x7C00, 0x7C00, 0x7C00, 0x7C00, 0x7C00, 0x7C00, + 0x7C00, 0x7C00, 0x7C00, 0x7C00, 0x7C00, 0x7C00, 0x7C00, 0x7C00, + 0x7C00, 0x7C00, 0x7C00, 0x7C00, 0x7C00, 0x7C00, + 0x7C00, 0x7C00, 0x7C00, 0x7C00, 0x7C00, 0x7C00, 0x7C00, 0x7C00, + 0x7C00, 0x7C00, 0x7C00, 0x7C00, 0x7C00, 0x7C00, + 0x7C00, 0x7C00, 0x7C00, 0x7C00, 0x7C00, 0x7C00, 0x7C00, 0x7C00, + 0x7C00, 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, + 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, + 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, + 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, + 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, + 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, + 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, + 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, + 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, + 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, + 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, + 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, + 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, + 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, + 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, 0x8000, + 0x8001, 0x8002, 0x8004, 0x8008, 0x8010, 0x8020, 0x8040, 0x8080, + 0x8100, 0x8200, 0x8400, 0x8800, 0x8C00, 0x9000, + 0x9400, 0x9800, 0x9C00, 0xA000, 0xA400, 0xA800, 0xAC00, 0xB000, + 0xB400, 0xB800, 0xBC00, 0xC000, 0xC400, 0xC800, + 0xCC00, 0xD000, 0xD400, 0xD800, 0xDC00, 0xE000, 0xE400, 0xE800, + 0xEC00, 0xF000, 0xF400, 0xF800, 0xFC00, 0xFC00, + 0xFC00, 0xFC00, 0xFC00, 0xFC00, 0xFC00, 0xFC00, 0xFC00, 0xFC00, + 0xFC00, 0xFC00, 0xFC00, 0xFC00, 0xFC00, 0xFC00, + 0xFC00, 0xFC00, 0xFC00, 0xFC00, 0xFC00, 0xFC00, 0xFC00, 0xFC00, + 0xFC00, 0xFC00, 0xFC00, 0xFC00, 0xFC00, 0xFC00, + 0xFC00, 0xFC00, 0xFC00, 0xFC00, 0xFC00, 0xFC00, 0xFC00, 0xFC00, + 0xFC00, 0xFC00, 0xFC00, 0xFC00, 0xFC00, 0xFC00, + 0xFC00, 0xFC00, 0xFC00, 0xFC00, 0xFC00, 0xFC00, 0xFC00, 0xFC00, + 0xFC00, 0xFC00, 0xFC00, 0xFC00, 0xFC00, 0xFC00, + 0xFC00, 0xFC00, 0xFC00, 0xFC00, 0xFC00, 0xFC00, 0xFC00, 0xFC00, + 0xFC00, 0xFC00, 0xFC00, 0xFC00, 0xFC00, 0xFC00, + 0xFC00, 0xFC00, 0xFC00, 0xFC00, 0xFC00, 0xFC00, 0xFC00, 0xFC00, + 0xFC00, 0xFC00, 0xFC00, 0xFC00, 0xFC00, 0xFC00, + 0xFC00, 0xFC00, 0xFC00, 0xFC00, 0xFC00, 0xFC00, 0xFC00, 0xFC00, + 0xFC00, 0xFC00, 0xFC00, 0xFC00, 0xFC00, 0xFC00, + 0xFC00, 0xFC00, 0xFC00, 0xFC00, 0xFC00, 0xFC00, 0xFC00, 0xFC00, + 0xFC00, 0xFC00, 0xFC00, 0xFC00, 0xFC00}; + static const unsigned char shift_table[512] = {24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, + 24, 24, + 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, + 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, + 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, + 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, + 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, + 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, + 24, 24, 23, 22, 21, 20, 19, 18, 17, 16, 15, 14, 13, 13, 13, + 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, + 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 24, + 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, + 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, + 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, + 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, + 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, + 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, + 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, + 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 13, + 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, + 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, + 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, + 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, + 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, + 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, + 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, + 24, 24, 24, 24, 24, 24, 23, 22, 21, 20, 19, 18, 17, 16, 15, + 14, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, + 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, + 13, 13, 13, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, + 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, + 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, + 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, + 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, + 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, + 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, + 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, + 24, 24, 24, 13}; + uint16 hbits = base_table[bits >> 23] + static_cast((bits & 0x7FFFFF) >> shift_table[bits >> 23]); + if (R == std::round_to_nearest) + hbits += (((bits & 0x7FFFFF) >> (shift_table[bits >> 23] - 1)) | (((bits >> 23) & 0xFF) == 102)) + & ((hbits & 0x7C00) != 0x7C00) +#if HALF_ROUND_TIES_TO_EVEN + & (((((static_cast(1) << (shift_table[bits >> 23] - 1)) - 1) & bits) != 0) | hbits) +#endif + ; + else if (R == std::round_toward_zero) + hbits -= ((hbits & 0x7FFF) == 0x7C00) & ~shift_table[bits >> 23]; + else if (R == std::round_toward_infinity) + hbits += ((((bits & 0x7FFFFF & ((static_cast(1) << (shift_table[bits >> 23])) - 1)) != 0) + | (((bits >> 23) <= 102) & ((bits >> 23) != 0))) + & (hbits < 0x7C00)) + - ((hbits == 0xFC00) & ((bits >> 23) != 511)); + else if (R == std::round_toward_neg_infinity) + hbits += ((((bits & 0x7FFFFF & ((static_cast(1) << (shift_table[bits >> 23])) - 1)) != 0) + | (((bits >> 23) <= 358) & ((bits >> 23) != 256))) + & (hbits < 0xFC00) & (hbits >> 15)) + - ((hbits == 0x7C00) & ((bits >> 23) != 255)); + return hbits; + } + +/// Convert IEEE double-precision to half-precision. +/// \tparam R rounding mode to use, `std::round_indeterminate` for fastest rounding +/// \param value double-precision value +/// \return binary representation of half-precision value + template + uint16 float2half_impl(double value, true_type) { + typedef bits::type uint32; + typedef bits::type uint64; + uint64 bits; // = *reinterpret_cast(&value); //violating strict aliasing! + std::memcpy(&bits, &value, sizeof(double)); + uint32 hi = bits >> 32, lo = bits & 0xFFFFFFFF; + uint16 hbits = (hi >> 16) & 0x8000; + hi &= 0x7FFFFFFF; + int exp = hi >> 20; + if (exp == 2047) + return hbits | 0x7C00 | (0x3FF & -static_cast((bits & 0xFFFFFFFFFFFFF) != 0)); + if (exp > 1038) { + if (R == std::round_toward_infinity) + return hbits | 0x7C00 - (hbits >> 15); + if (R == std::round_toward_neg_infinity) + return hbits | 0x7BFF + (hbits >> 15); + return hbits | 0x7BFF + (R != std::round_toward_zero); + } + int g, s = lo != 0; + if (exp > 1008) { + g = (hi >> 9) & 1; + s |= (hi & 0x1FF) != 0; + hbits |= ((exp - 1008) << 10) | ((hi >> 10) & 0x3FF); + } else if (exp > 997) { + int i = 1018 - exp; + hi = (hi & 0xFFFFF) | 0x100000; + g = (hi >> i) & 1; + s |= (hi & ((1L << i) - 1)) != 0; + hbits |= hi >> (i + 1); + } else { + g = 0; + s |= hi != 0; + } + if (R == std::round_to_nearest) +#if HALF_ROUND_TIES_TO_EVEN + hbits += g & (s | hbits); +#else + hbits += g; +#endif + else if (R == std::round_toward_infinity) + hbits += ~(hbits >> 15) & (s | g); + else if (R == std::round_toward_neg_infinity) + hbits += (hbits >> 15) & (g | s); + return hbits; + } + +/// Convert non-IEEE floating point to half-precision. +/// \tparam R rounding mode to use, `std::round_indeterminate` for fastest rounding +/// \tparam T source type (builtin floating point type) +/// \param value floating point value +/// \return binary representation of half-precision value + template + uint16 float2half_impl(T value, ...) { + uint16 hbits = static_cast(builtin_signbit(value)) << 15; + if (value == T()) + return hbits; + if (builtin_isnan(value)) + return hbits | 0x7FFF; + if (builtin_isinf(value)) + return hbits | 0x7C00; + int exp; + std::frexp(value, &exp); + if (exp > 16) { + if (R == std::round_toward_infinity) + return hbits | (0x7C00 - (hbits >> 15)); + else if (R == std::round_toward_neg_infinity) + return hbits | (0x7BFF + (hbits >> 15)); + return hbits | (0x7BFF + (R != std::round_toward_zero)); + } + if (exp < -13) + value = std::ldexp(value, 24); + else { + value = std::ldexp(value, 11 - exp); + hbits |= ((exp + 13) << 10); + } + T ival, frac = std::modf(value, &ival); + hbits += static_cast(std::abs(static_cast(ival))); + if (R == std::round_to_nearest) { + frac = std::abs(frac); +#if HALF_ROUND_TIES_TO_EVEN + hbits += (frac > T(0.5)) | ((frac == T(0.5)) & hbits); +#else + hbits += frac >= T(0.5); +#endif + } else if (R == std::round_toward_infinity) + hbits += frac > T(); + else if (R == std::round_toward_neg_infinity) + hbits += frac < T(); + return hbits; + } + +/// Convert floating point to half-precision. +/// \tparam R rounding mode to use, `std::round_indeterminate` for fastest rounding +/// \tparam T source type (builtin floating point type) +/// \param value floating point value +/// \return binary representation of half-precision value + template + uint16 float2half(T value) { + return float2half_impl( + value, + bool_type::is_iec559 && sizeof(typename bits::type) == sizeof(T)>()); + } + +/// Convert integer to half-precision floating point. +/// \tparam R rounding mode to use, `std::round_indeterminate` for fastest rounding +/// \tparam S `true` if value negative, `false` else +/// \tparam T type to convert (builtin integer type) +/// \param value non-negative integral value +/// \return binary representation of half-precision value + template + uint16 int2half_impl(T value) { +#if HALF_ENABLE_CPP11_STATIC_ASSERT && HALF_ENABLE_CPP11_TYPE_TRAITS + static_assert(std::is_integral::value, "int to half conversion only supports builtin integer types"); +#endif + if (S) + value = -value; + uint16 bits = S << 15; + if (value > 0xFFFF) { + if (R == std::round_toward_infinity) + bits |= 0x7C00 - S; + else if (R == std::round_toward_neg_infinity) + bits |= 0x7BFF + S; + else + bits |= 0x7BFF + (R != std::round_toward_zero); + } else if (value) { + uint32_t m = value, exp = 24; + for (; m < 0x400; m <<= 1, --exp); + for (; m > 0x7FF; m >>= 1, ++exp); + bits |= (exp << 10) + m; + if (exp > 24) { + if (R == std::round_to_nearest) + bits += (value >> (exp - 25)) & 1 +#if HALF_ROUND_TIES_TO_EVEN + & (((((1 << (exp - 25)) - 1) & value) != 0) | bits) +#endif + ; + else if (R == std::round_toward_infinity) + bits += ((value & ((1 << (exp - 24)) - 1)) != 0) & !S; + else if (R == std::round_toward_neg_infinity) + bits += ((value & ((1 << (exp - 24)) - 1)) != 0) & S; + } + } + return bits; + } + +/// Convert integer to half-precision floating point. +/// \tparam R rounding mode to use, `std::round_indeterminate` for fastest rounding +/// \tparam T type to convert (builtin integer type) +/// \param value integral value +/// \return binary representation of half-precision value + template + uint16 int2half(T value) { + return (value < 0) ? int2half_impl(value) : int2half_impl(value); + } + +/// Convert half-precision to IEEE single-precision. +/// Credit for this goes to [Jeroen van der Zijp](ftp://ftp.fox-toolkit.org/pub/fasthalffloatconversion.pdf). +/// \param value binary representation of half-precision value +/// \return single-precision value + inline float half2float_impl(uint16 value, float, true_type) { + typedef bits::type uint32; + /* uint32 bits = static_cast(value&0x8000) << 16; + int abs = value & 0x7FFF; + if(abs) + { + bits |= 0x38000000 << static_cast(abs>=0x7C00); + for(; abs<0x400; abs<<=1,bits-=0x800000) ; + bits += static_cast(abs) << 13; + } + */ + static const uint32 mantissa_table[2048] = {0x00000000, 0x33800000, 0x34000000, 0x34400000, 0x34800000, + 0x34A00000, + 0x34C00000, 0x34E00000, 0x35000000, 0x35100000, 0x35200000, + 0x35300000, 0x35400000, 0x35500000, 0x35600000, + 0x35700000, 0x35800000, 0x35880000, 0x35900000, 0x35980000, + 0x35A00000, 0x35A80000, 0x35B00000, 0x35B80000, + 0x35C00000, 0x35C80000, 0x35D00000, 0x35D80000, 0x35E00000, + 0x35E80000, 0x35F00000, 0x35F80000, 0x36000000, + 0x36040000, 0x36080000, 0x360C0000, 0x36100000, 0x36140000, + 0x36180000, 0x361C0000, 0x36200000, 0x36240000, + 0x36280000, 0x362C0000, 0x36300000, 0x36340000, 0x36380000, + 0x363C0000, 0x36400000, 0x36440000, 0x36480000, + 0x364C0000, 0x36500000, 0x36540000, 0x36580000, 0x365C0000, + 0x36600000, 0x36640000, 0x36680000, 0x366C0000, + 0x36700000, 0x36740000, 0x36780000, 0x367C0000, 0x36800000, + 0x36820000, 0x36840000, 0x36860000, 0x36880000, + 0x368A0000, 0x368C0000, 0x368E0000, 0x36900000, 0x36920000, + 0x36940000, 0x36960000, 0x36980000, 0x369A0000, + 0x369C0000, 0x369E0000, 0x36A00000, 0x36A20000, 0x36A40000, + 0x36A60000, 0x36A80000, 0x36AA0000, 0x36AC0000, + 0x36AE0000, 0x36B00000, 0x36B20000, 0x36B40000, 0x36B60000, + 0x36B80000, 0x36BA0000, 0x36BC0000, 0x36BE0000, + 0x36C00000, 0x36C20000, 0x36C40000, 0x36C60000, 0x36C80000, + 0x36CA0000, 0x36CC0000, 0x36CE0000, 0x36D00000, + 0x36D20000, 0x36D40000, 0x36D60000, 0x36D80000, 0x36DA0000, + 0x36DC0000, 0x36DE0000, 0x36E00000, 0x36E20000, + 0x36E40000, 0x36E60000, 0x36E80000, 0x36EA0000, 0x36EC0000, + 0x36EE0000, 0x36F00000, 0x36F20000, 0x36F40000, + 0x36F60000, 0x36F80000, 0x36FA0000, 0x36FC0000, 0x36FE0000, + 0x37000000, 0x37010000, 0x37020000, 0x37030000, + 0x37040000, 0x37050000, 0x37060000, 0x37070000, 0x37080000, + 0x37090000, 0x370A0000, 0x370B0000, 0x370C0000, + 0x370D0000, 0x370E0000, 0x370F0000, 0x37100000, 0x37110000, + 0x37120000, 0x37130000, 0x37140000, 0x37150000, + 0x37160000, 0x37170000, 0x37180000, 0x37190000, 0x371A0000, + 0x371B0000, 0x371C0000, 0x371D0000, 0x371E0000, + 0x371F0000, 0x37200000, 0x37210000, 0x37220000, 0x37230000, + 0x37240000, 0x37250000, 0x37260000, 0x37270000, + 0x37280000, 0x37290000, 0x372A0000, 0x372B0000, 0x372C0000, + 0x372D0000, 0x372E0000, 0x372F0000, 0x37300000, + 0x37310000, 0x37320000, 0x37330000, 0x37340000, 0x37350000, + 0x37360000, 0x37370000, 0x37380000, 0x37390000, + 0x373A0000, 0x373B0000, 0x373C0000, 0x373D0000, 0x373E0000, + 0x373F0000, 0x37400000, 0x37410000, 0x37420000, + 0x37430000, 0x37440000, 0x37450000, 0x37460000, 0x37470000, + 0x37480000, 0x37490000, 0x374A0000, 0x374B0000, + 0x374C0000, 0x374D0000, 0x374E0000, 0x374F0000, 0x37500000, + 0x37510000, 0x37520000, 0x37530000, 0x37540000, + 0x37550000, 0x37560000, 0x37570000, 0x37580000, 0x37590000, + 0x375A0000, 0x375B0000, 0x375C0000, 0x375D0000, + 0x375E0000, 0x375F0000, 0x37600000, 0x37610000, 0x37620000, + 0x37630000, 0x37640000, 0x37650000, 0x37660000, + 0x37670000, 0x37680000, 0x37690000, 0x376A0000, 0x376B0000, + 0x376C0000, 0x376D0000, 0x376E0000, 0x376F0000, + 0x37700000, 0x37710000, 0x37720000, 0x37730000, 0x37740000, + 0x37750000, 0x37760000, 0x37770000, 0x37780000, + 0x37790000, 0x377A0000, 0x377B0000, 0x377C0000, 0x377D0000, + 0x377E0000, 0x377F0000, 0x37800000, 0x37808000, + 0x37810000, 0x37818000, 0x37820000, 0x37828000, 0x37830000, + 0x37838000, 0x37840000, 0x37848000, 0x37850000, + 0x37858000, 0x37860000, 0x37868000, 0x37870000, 0x37878000, + 0x37880000, 0x37888000, 0x37890000, 0x37898000, + 0x378A0000, 0x378A8000, 0x378B0000, 0x378B8000, 0x378C0000, + 0x378C8000, 0x378D0000, 0x378D8000, 0x378E0000, + 0x378E8000, 0x378F0000, 0x378F8000, 0x37900000, 0x37908000, + 0x37910000, 0x37918000, 0x37920000, 0x37928000, + 0x37930000, 0x37938000, 0x37940000, 0x37948000, 0x37950000, + 0x37958000, 0x37960000, 0x37968000, 0x37970000, + 0x37978000, 0x37980000, 0x37988000, 0x37990000, 0x37998000, + 0x379A0000, 0x379A8000, 0x379B0000, 0x379B8000, + 0x379C0000, 0x379C8000, 0x379D0000, 0x379D8000, 0x379E0000, + 0x379E8000, 0x379F0000, 0x379F8000, 0x37A00000, + 0x37A08000, 0x37A10000, 0x37A18000, 0x37A20000, 0x37A28000, + 0x37A30000, 0x37A38000, 0x37A40000, 0x37A48000, + 0x37A50000, 0x37A58000, 0x37A60000, 0x37A68000, 0x37A70000, + 0x37A78000, 0x37A80000, 0x37A88000, 0x37A90000, + 0x37A98000, 0x37AA0000, 0x37AA8000, 0x37AB0000, 0x37AB8000, + 0x37AC0000, 0x37AC8000, 0x37AD0000, 0x37AD8000, + 0x37AE0000, 0x37AE8000, 0x37AF0000, 0x37AF8000, 0x37B00000, + 0x37B08000, 0x37B10000, 0x37B18000, 0x37B20000, + 0x37B28000, 0x37B30000, 0x37B38000, 0x37B40000, 0x37B48000, + 0x37B50000, 0x37B58000, 0x37B60000, 0x37B68000, + 0x37B70000, 0x37B78000, 0x37B80000, 0x37B88000, 0x37B90000, + 0x37B98000, 0x37BA0000, 0x37BA8000, 0x37BB0000, + 0x37BB8000, 0x37BC0000, 0x37BC8000, 0x37BD0000, 0x37BD8000, + 0x37BE0000, 0x37BE8000, 0x37BF0000, 0x37BF8000, + 0x37C00000, 0x37C08000, 0x37C10000, 0x37C18000, 0x37C20000, + 0x37C28000, 0x37C30000, 0x37C38000, 0x37C40000, + 0x37C48000, 0x37C50000, 0x37C58000, 0x37C60000, 0x37C68000, + 0x37C70000, 0x37C78000, 0x37C80000, 0x37C88000, + 0x37C90000, 0x37C98000, 0x37CA0000, 0x37CA8000, 0x37CB0000, + 0x37CB8000, 0x37CC0000, 0x37CC8000, 0x37CD0000, + 0x37CD8000, 0x37CE0000, 0x37CE8000, 0x37CF0000, 0x37CF8000, + 0x37D00000, 0x37D08000, 0x37D10000, 0x37D18000, + 0x37D20000, 0x37D28000, 0x37D30000, 0x37D38000, 0x37D40000, + 0x37D48000, 0x37D50000, 0x37D58000, 0x37D60000, + 0x37D68000, 0x37D70000, 0x37D78000, 0x37D80000, 0x37D88000, + 0x37D90000, 0x37D98000, 0x37DA0000, 0x37DA8000, + 0x37DB0000, 0x37DB8000, 0x37DC0000, 0x37DC8000, 0x37DD0000, + 0x37DD8000, 0x37DE0000, 0x37DE8000, 0x37DF0000, + 0x37DF8000, 0x37E00000, 0x37E08000, 0x37E10000, 0x37E18000, + 0x37E20000, 0x37E28000, 0x37E30000, 0x37E38000, + 0x37E40000, 0x37E48000, 0x37E50000, 0x37E58000, 0x37E60000, + 0x37E68000, 0x37E70000, 0x37E78000, 0x37E80000, + 0x37E88000, 0x37E90000, 0x37E98000, 0x37EA0000, 0x37EA8000, + 0x37EB0000, 0x37EB8000, 0x37EC0000, 0x37EC8000, + 0x37ED0000, 0x37ED8000, 0x37EE0000, 0x37EE8000, 0x37EF0000, + 0x37EF8000, 0x37F00000, 0x37F08000, 0x37F10000, + 0x37F18000, 0x37F20000, 0x37F28000, 0x37F30000, 0x37F38000, + 0x37F40000, 0x37F48000, 0x37F50000, 0x37F58000, + 0x37F60000, 0x37F68000, 0x37F70000, 0x37F78000, 0x37F80000, + 0x37F88000, 0x37F90000, 0x37F98000, 0x37FA0000, + 0x37FA8000, 0x37FB0000, 0x37FB8000, 0x37FC0000, 0x37FC8000, + 0x37FD0000, 0x37FD8000, 0x37FE0000, 0x37FE8000, + 0x37FF0000, 0x37FF8000, 0x38000000, 0x38004000, 0x38008000, + 0x3800C000, 0x38010000, 0x38014000, 0x38018000, + 0x3801C000, 0x38020000, 0x38024000, 0x38028000, 0x3802C000, + 0x38030000, 0x38034000, 0x38038000, 0x3803C000, + 0x38040000, 0x38044000, 0x38048000, 0x3804C000, 0x38050000, + 0x38054000, 0x38058000, 0x3805C000, 0x38060000, + 0x38064000, 0x38068000, 0x3806C000, 0x38070000, 0x38074000, + 0x38078000, 0x3807C000, 0x38080000, 0x38084000, + 0x38088000, 0x3808C000, 0x38090000, 0x38094000, 0x38098000, + 0x3809C000, 0x380A0000, 0x380A4000, 0x380A8000, + 0x380AC000, 0x380B0000, 0x380B4000, 0x380B8000, 0x380BC000, + 0x380C0000, 0x380C4000, 0x380C8000, 0x380CC000, + 0x380D0000, 0x380D4000, 0x380D8000, 0x380DC000, 0x380E0000, + 0x380E4000, 0x380E8000, 0x380EC000, 0x380F0000, + 0x380F4000, 0x380F8000, 0x380FC000, 0x38100000, 0x38104000, + 0x38108000, 0x3810C000, 0x38110000, 0x38114000, + 0x38118000, 0x3811C000, 0x38120000, 0x38124000, 0x38128000, + 0x3812C000, 0x38130000, 0x38134000, 0x38138000, + 0x3813C000, 0x38140000, 0x38144000, 0x38148000, 0x3814C000, + 0x38150000, 0x38154000, 0x38158000, 0x3815C000, + 0x38160000, 0x38164000, 0x38168000, 0x3816C000, 0x38170000, + 0x38174000, 0x38178000, 0x3817C000, 0x38180000, + 0x38184000, 0x38188000, 0x3818C000, 0x38190000, 0x38194000, + 0x38198000, 0x3819C000, 0x381A0000, 0x381A4000, + 0x381A8000, 0x381AC000, 0x381B0000, 0x381B4000, 0x381B8000, + 0x381BC000, 0x381C0000, 0x381C4000, 0x381C8000, + 0x381CC000, 0x381D0000, 0x381D4000, 0x381D8000, 0x381DC000, + 0x381E0000, 0x381E4000, 0x381E8000, 0x381EC000, + 0x381F0000, 0x381F4000, 0x381F8000, 0x381FC000, 0x38200000, + 0x38204000, 0x38208000, 0x3820C000, 0x38210000, + 0x38214000, 0x38218000, 0x3821C000, 0x38220000, 0x38224000, + 0x38228000, 0x3822C000, 0x38230000, 0x38234000, + 0x38238000, 0x3823C000, 0x38240000, 0x38244000, 0x38248000, + 0x3824C000, 0x38250000, 0x38254000, 0x38258000, + 0x3825C000, 0x38260000, 0x38264000, 0x38268000, 0x3826C000, + 0x38270000, 0x38274000, 0x38278000, 0x3827C000, + 0x38280000, 0x38284000, 0x38288000, 0x3828C000, 0x38290000, + 0x38294000, 0x38298000, 0x3829C000, 0x382A0000, + 0x382A4000, 0x382A8000, 0x382AC000, 0x382B0000, 0x382B4000, + 0x382B8000, 0x382BC000, 0x382C0000, 0x382C4000, + 0x382C8000, 0x382CC000, 0x382D0000, 0x382D4000, 0x382D8000, + 0x382DC000, 0x382E0000, 0x382E4000, 0x382E8000, + 0x382EC000, 0x382F0000, 0x382F4000, 0x382F8000, 0x382FC000, + 0x38300000, 0x38304000, 0x38308000, 0x3830C000, + 0x38310000, 0x38314000, 0x38318000, 0x3831C000, 0x38320000, + 0x38324000, 0x38328000, 0x3832C000, 0x38330000, + 0x38334000, 0x38338000, 0x3833C000, 0x38340000, 0x38344000, + 0x38348000, 0x3834C000, 0x38350000, 0x38354000, + 0x38358000, 0x3835C000, 0x38360000, 0x38364000, 0x38368000, + 0x3836C000, 0x38370000, 0x38374000, 0x38378000, + 0x3837C000, 0x38380000, 0x38384000, 0x38388000, 0x3838C000, + 0x38390000, 0x38394000, 0x38398000, 0x3839C000, + 0x383A0000, 0x383A4000, 0x383A8000, 0x383AC000, 0x383B0000, + 0x383B4000, 0x383B8000, 0x383BC000, 0x383C0000, + 0x383C4000, 0x383C8000, 0x383CC000, 0x383D0000, 0x383D4000, + 0x383D8000, 0x383DC000, 0x383E0000, 0x383E4000, + 0x383E8000, 0x383EC000, 0x383F0000, 0x383F4000, 0x383F8000, + 0x383FC000, 0x38400000, 0x38404000, 0x38408000, + 0x3840C000, 0x38410000, 0x38414000, 0x38418000, 0x3841C000, + 0x38420000, 0x38424000, 0x38428000, 0x3842C000, + 0x38430000, 0x38434000, 0x38438000, 0x3843C000, 0x38440000, + 0x38444000, 0x38448000, 0x3844C000, 0x38450000, + 0x38454000, 0x38458000, 0x3845C000, 0x38460000, 0x38464000, + 0x38468000, 0x3846C000, 0x38470000, 0x38474000, + 0x38478000, 0x3847C000, 0x38480000, 0x38484000, 0x38488000, + 0x3848C000, 0x38490000, 0x38494000, 0x38498000, + 0x3849C000, 0x384A0000, 0x384A4000, 0x384A8000, 0x384AC000, + 0x384B0000, 0x384B4000, 0x384B8000, 0x384BC000, + 0x384C0000, 0x384C4000, 0x384C8000, 0x384CC000, 0x384D0000, + 0x384D4000, 0x384D8000, 0x384DC000, 0x384E0000, + 0x384E4000, 0x384E8000, 0x384EC000, 0x384F0000, 0x384F4000, + 0x384F8000, 0x384FC000, 0x38500000, 0x38504000, + 0x38508000, 0x3850C000, 0x38510000, 0x38514000, 0x38518000, + 0x3851C000, 0x38520000, 0x38524000, 0x38528000, + 0x3852C000, 0x38530000, 0x38534000, 0x38538000, 0x3853C000, + 0x38540000, 0x38544000, 0x38548000, 0x3854C000, + 0x38550000, 0x38554000, 0x38558000, 0x3855C000, 0x38560000, + 0x38564000, 0x38568000, 0x3856C000, 0x38570000, + 0x38574000, 0x38578000, 0x3857C000, 0x38580000, 0x38584000, + 0x38588000, 0x3858C000, 0x38590000, 0x38594000, + 0x38598000, 0x3859C000, 0x385A0000, 0x385A4000, 0x385A8000, + 0x385AC000, 0x385B0000, 0x385B4000, 0x385B8000, + 0x385BC000, 0x385C0000, 0x385C4000, 0x385C8000, 0x385CC000, + 0x385D0000, 0x385D4000, 0x385D8000, 0x385DC000, + 0x385E0000, 0x385E4000, 0x385E8000, 0x385EC000, 0x385F0000, + 0x385F4000, 0x385F8000, 0x385FC000, 0x38600000, + 0x38604000, 0x38608000, 0x3860C000, 0x38610000, 0x38614000, + 0x38618000, 0x3861C000, 0x38620000, 0x38624000, + 0x38628000, 0x3862C000, 0x38630000, 0x38634000, 0x38638000, + 0x3863C000, 0x38640000, 0x38644000, 0x38648000, + 0x3864C000, 0x38650000, 0x38654000, 0x38658000, 0x3865C000, + 0x38660000, 0x38664000, 0x38668000, 0x3866C000, + 0x38670000, 0x38674000, 0x38678000, 0x3867C000, 0x38680000, + 0x38684000, 0x38688000, 0x3868C000, 0x38690000, + 0x38694000, 0x38698000, 0x3869C000, 0x386A0000, 0x386A4000, + 0x386A8000, 0x386AC000, 0x386B0000, 0x386B4000, + 0x386B8000, 0x386BC000, 0x386C0000, 0x386C4000, 0x386C8000, + 0x386CC000, 0x386D0000, 0x386D4000, 0x386D8000, + 0x386DC000, 0x386E0000, 0x386E4000, 0x386E8000, 0x386EC000, + 0x386F0000, 0x386F4000, 0x386F8000, 0x386FC000, + 0x38700000, 0x38704000, 0x38708000, 0x3870C000, 0x38710000, + 0x38714000, 0x38718000, 0x3871C000, 0x38720000, + 0x38724000, 0x38728000, 0x3872C000, 0x38730000, 0x38734000, + 0x38738000, 0x3873C000, 0x38740000, 0x38744000, + 0x38748000, 0x3874C000, 0x38750000, 0x38754000, 0x38758000, + 0x3875C000, 0x38760000, 0x38764000, 0x38768000, + 0x3876C000, 0x38770000, 0x38774000, 0x38778000, 0x3877C000, + 0x38780000, 0x38784000, 0x38788000, 0x3878C000, + 0x38790000, 0x38794000, 0x38798000, 0x3879C000, 0x387A0000, + 0x387A4000, 0x387A8000, 0x387AC000, 0x387B0000, + 0x387B4000, 0x387B8000, 0x387BC000, 0x387C0000, 0x387C4000, + 0x387C8000, 0x387CC000, 0x387D0000, 0x387D4000, + 0x387D8000, 0x387DC000, 0x387E0000, 0x387E4000, 0x387E8000, + 0x387EC000, 0x387F0000, 0x387F4000, 0x387F8000, + 0x387FC000, 0x38000000, 0x38002000, 0x38004000, 0x38006000, + 0x38008000, 0x3800A000, 0x3800C000, 0x3800E000, + 0x38010000, 0x38012000, 0x38014000, 0x38016000, 0x38018000, + 0x3801A000, 0x3801C000, 0x3801E000, 0x38020000, + 0x38022000, 0x38024000, 0x38026000, 0x38028000, 0x3802A000, + 0x3802C000, 0x3802E000, 0x38030000, 0x38032000, + 0x38034000, 0x38036000, 0x38038000, 0x3803A000, 0x3803C000, + 0x3803E000, 0x38040000, 0x38042000, 0x38044000, + 0x38046000, 0x38048000, 0x3804A000, 0x3804C000, 0x3804E000, + 0x38050000, 0x38052000, 0x38054000, 0x38056000, + 0x38058000, 0x3805A000, 0x3805C000, 0x3805E000, 0x38060000, + 0x38062000, 0x38064000, 0x38066000, 0x38068000, + 0x3806A000, 0x3806C000, 0x3806E000, 0x38070000, 0x38072000, + 0x38074000, 0x38076000, 0x38078000, 0x3807A000, + 0x3807C000, 0x3807E000, 0x38080000, 0x38082000, 0x38084000, + 0x38086000, 0x38088000, 0x3808A000, 0x3808C000, + 0x3808E000, 0x38090000, 0x38092000, 0x38094000, 0x38096000, + 0x38098000, 0x3809A000, 0x3809C000, 0x3809E000, + 0x380A0000, 0x380A2000, 0x380A4000, 0x380A6000, 0x380A8000, + 0x380AA000, 0x380AC000, 0x380AE000, 0x380B0000, + 0x380B2000, 0x380B4000, 0x380B6000, 0x380B8000, 0x380BA000, + 0x380BC000, 0x380BE000, 0x380C0000, 0x380C2000, + 0x380C4000, 0x380C6000, 0x380C8000, 0x380CA000, 0x380CC000, + 0x380CE000, 0x380D0000, 0x380D2000, 0x380D4000, + 0x380D6000, 0x380D8000, 0x380DA000, 0x380DC000, 0x380DE000, + 0x380E0000, 0x380E2000, 0x380E4000, 0x380E6000, + 0x380E8000, 0x380EA000, 0x380EC000, 0x380EE000, 0x380F0000, + 0x380F2000, 0x380F4000, 0x380F6000, 0x380F8000, + 0x380FA000, 0x380FC000, 0x380FE000, 0x38100000, 0x38102000, + 0x38104000, 0x38106000, 0x38108000, 0x3810A000, + 0x3810C000, 0x3810E000, 0x38110000, 0x38112000, 0x38114000, + 0x38116000, 0x38118000, 0x3811A000, 0x3811C000, + 0x3811E000, 0x38120000, 0x38122000, 0x38124000, 0x38126000, + 0x38128000, 0x3812A000, 0x3812C000, 0x3812E000, + 0x38130000, 0x38132000, 0x38134000, 0x38136000, 0x38138000, + 0x3813A000, 0x3813C000, 0x3813E000, 0x38140000, + 0x38142000, 0x38144000, 0x38146000, 0x38148000, 0x3814A000, + 0x3814C000, 0x3814E000, 0x38150000, 0x38152000, + 0x38154000, 0x38156000, 0x38158000, 0x3815A000, 0x3815C000, + 0x3815E000, 0x38160000, 0x38162000, 0x38164000, + 0x38166000, 0x38168000, 0x3816A000, 0x3816C000, 0x3816E000, + 0x38170000, 0x38172000, 0x38174000, 0x38176000, + 0x38178000, 0x3817A000, 0x3817C000, 0x3817E000, 0x38180000, + 0x38182000, 0x38184000, 0x38186000, 0x38188000, + 0x3818A000, 0x3818C000, 0x3818E000, 0x38190000, 0x38192000, + 0x38194000, 0x38196000, 0x38198000, 0x3819A000, + 0x3819C000, 0x3819E000, 0x381A0000, 0x381A2000, 0x381A4000, + 0x381A6000, 0x381A8000, 0x381AA000, 0x381AC000, + 0x381AE000, 0x381B0000, 0x381B2000, 0x381B4000, 0x381B6000, + 0x381B8000, 0x381BA000, 0x381BC000, 0x381BE000, + 0x381C0000, 0x381C2000, 0x381C4000, 0x381C6000, 0x381C8000, + 0x381CA000, 0x381CC000, 0x381CE000, 0x381D0000, + 0x381D2000, 0x381D4000, 0x381D6000, 0x381D8000, 0x381DA000, + 0x381DC000, 0x381DE000, 0x381E0000, 0x381E2000, + 0x381E4000, 0x381E6000, 0x381E8000, 0x381EA000, 0x381EC000, + 0x381EE000, 0x381F0000, 0x381F2000, 0x381F4000, + 0x381F6000, 0x381F8000, 0x381FA000, 0x381FC000, 0x381FE000, + 0x38200000, 0x38202000, 0x38204000, 0x38206000, + 0x38208000, 0x3820A000, 0x3820C000, 0x3820E000, 0x38210000, + 0x38212000, 0x38214000, 0x38216000, 0x38218000, + 0x3821A000, 0x3821C000, 0x3821E000, 0x38220000, 0x38222000, + 0x38224000, 0x38226000, 0x38228000, 0x3822A000, + 0x3822C000, 0x3822E000, 0x38230000, 0x38232000, 0x38234000, + 0x38236000, 0x38238000, 0x3823A000, 0x3823C000, + 0x3823E000, 0x38240000, 0x38242000, 0x38244000, 0x38246000, + 0x38248000, 0x3824A000, 0x3824C000, 0x3824E000, + 0x38250000, 0x38252000, 0x38254000, 0x38256000, 0x38258000, + 0x3825A000, 0x3825C000, 0x3825E000, 0x38260000, + 0x38262000, 0x38264000, 0x38266000, 0x38268000, 0x3826A000, + 0x3826C000, 0x3826E000, 0x38270000, 0x38272000, + 0x38274000, 0x38276000, 0x38278000, 0x3827A000, 0x3827C000, + 0x3827E000, 0x38280000, 0x38282000, 0x38284000, + 0x38286000, 0x38288000, 0x3828A000, 0x3828C000, 0x3828E000, + 0x38290000, 0x38292000, 0x38294000, 0x38296000, + 0x38298000, 0x3829A000, 0x3829C000, 0x3829E000, 0x382A0000, + 0x382A2000, 0x382A4000, 0x382A6000, 0x382A8000, + 0x382AA000, 0x382AC000, 0x382AE000, 0x382B0000, 0x382B2000, + 0x382B4000, 0x382B6000, 0x382B8000, 0x382BA000, + 0x382BC000, 0x382BE000, 0x382C0000, 0x382C2000, 0x382C4000, + 0x382C6000, 0x382C8000, 0x382CA000, 0x382CC000, + 0x382CE000, 0x382D0000, 0x382D2000, 0x382D4000, 0x382D6000, + 0x382D8000, 0x382DA000, 0x382DC000, 0x382DE000, + 0x382E0000, 0x382E2000, 0x382E4000, 0x382E6000, 0x382E8000, + 0x382EA000, 0x382EC000, 0x382EE000, 0x382F0000, + 0x382F2000, 0x382F4000, 0x382F6000, 0x382F8000, 0x382FA000, + 0x382FC000, 0x382FE000, 0x38300000, 0x38302000, + 0x38304000, 0x38306000, 0x38308000, 0x3830A000, 0x3830C000, + 0x3830E000, 0x38310000, 0x38312000, 0x38314000, + 0x38316000, 0x38318000, 0x3831A000, 0x3831C000, 0x3831E000, + 0x38320000, 0x38322000, 0x38324000, 0x38326000, + 0x38328000, 0x3832A000, 0x3832C000, 0x3832E000, 0x38330000, + 0x38332000, 0x38334000, 0x38336000, 0x38338000, + 0x3833A000, 0x3833C000, 0x3833E000, 0x38340000, 0x38342000, + 0x38344000, 0x38346000, 0x38348000, 0x3834A000, + 0x3834C000, 0x3834E000, 0x38350000, 0x38352000, 0x38354000, + 0x38356000, 0x38358000, 0x3835A000, 0x3835C000, + 0x3835E000, 0x38360000, 0x38362000, 0x38364000, 0x38366000, + 0x38368000, 0x3836A000, 0x3836C000, 0x3836E000, + 0x38370000, 0x38372000, 0x38374000, 0x38376000, 0x38378000, + 0x3837A000, 0x3837C000, 0x3837E000, 0x38380000, + 0x38382000, 0x38384000, 0x38386000, 0x38388000, 0x3838A000, + 0x3838C000, 0x3838E000, 0x38390000, 0x38392000, + 0x38394000, 0x38396000, 0x38398000, 0x3839A000, 0x3839C000, + 0x3839E000, 0x383A0000, 0x383A2000, 0x383A4000, + 0x383A6000, 0x383A8000, 0x383AA000, 0x383AC000, 0x383AE000, + 0x383B0000, 0x383B2000, 0x383B4000, 0x383B6000, + 0x383B8000, 0x383BA000, 0x383BC000, 0x383BE000, 0x383C0000, + 0x383C2000, 0x383C4000, 0x383C6000, 0x383C8000, + 0x383CA000, 0x383CC000, 0x383CE000, 0x383D0000, 0x383D2000, + 0x383D4000, 0x383D6000, 0x383D8000, 0x383DA000, + 0x383DC000, 0x383DE000, 0x383E0000, 0x383E2000, 0x383E4000, + 0x383E6000, 0x383E8000, 0x383EA000, 0x383EC000, + 0x383EE000, 0x383F0000, 0x383F2000, 0x383F4000, 0x383F6000, + 0x383F8000, 0x383FA000, 0x383FC000, 0x383FE000, + 0x38400000, 0x38402000, 0x38404000, 0x38406000, 0x38408000, + 0x3840A000, 0x3840C000, 0x3840E000, 0x38410000, + 0x38412000, 0x38414000, 0x38416000, 0x38418000, 0x3841A000, + 0x3841C000, 0x3841E000, 0x38420000, 0x38422000, + 0x38424000, 0x38426000, 0x38428000, 0x3842A000, 0x3842C000, + 0x3842E000, 0x38430000, 0x38432000, 0x38434000, + 0x38436000, 0x38438000, 0x3843A000, 0x3843C000, 0x3843E000, + 0x38440000, 0x38442000, 0x38444000, 0x38446000, + 0x38448000, 0x3844A000, 0x3844C000, 0x3844E000, 0x38450000, + 0x38452000, 0x38454000, 0x38456000, 0x38458000, + 0x3845A000, 0x3845C000, 0x3845E000, 0x38460000, 0x38462000, + 0x38464000, 0x38466000, 0x38468000, 0x3846A000, + 0x3846C000, 0x3846E000, 0x38470000, 0x38472000, 0x38474000, + 0x38476000, 0x38478000, 0x3847A000, 0x3847C000, + 0x3847E000, 0x38480000, 0x38482000, 0x38484000, 0x38486000, + 0x38488000, 0x3848A000, 0x3848C000, 0x3848E000, + 0x38490000, 0x38492000, 0x38494000, 0x38496000, 0x38498000, + 0x3849A000, 0x3849C000, 0x3849E000, 0x384A0000, + 0x384A2000, 0x384A4000, 0x384A6000, 0x384A8000, 0x384AA000, + 0x384AC000, 0x384AE000, 0x384B0000, 0x384B2000, + 0x384B4000, 0x384B6000, 0x384B8000, 0x384BA000, 0x384BC000, + 0x384BE000, 0x384C0000, 0x384C2000, 0x384C4000, + 0x384C6000, 0x384C8000, 0x384CA000, 0x384CC000, 0x384CE000, + 0x384D0000, 0x384D2000, 0x384D4000, 0x384D6000, + 0x384D8000, 0x384DA000, 0x384DC000, 0x384DE000, 0x384E0000, + 0x384E2000, 0x384E4000, 0x384E6000, 0x384E8000, + 0x384EA000, 0x384EC000, 0x384EE000, 0x384F0000, 0x384F2000, + 0x384F4000, 0x384F6000, 0x384F8000, 0x384FA000, + 0x384FC000, 0x384FE000, 0x38500000, 0x38502000, 0x38504000, + 0x38506000, 0x38508000, 0x3850A000, 0x3850C000, + 0x3850E000, 0x38510000, 0x38512000, 0x38514000, 0x38516000, + 0x38518000, 0x3851A000, 0x3851C000, 0x3851E000, + 0x38520000, 0x38522000, 0x38524000, 0x38526000, 0x38528000, + 0x3852A000, 0x3852C000, 0x3852E000, 0x38530000, + 0x38532000, 0x38534000, 0x38536000, 0x38538000, 0x3853A000, + 0x3853C000, 0x3853E000, 0x38540000, 0x38542000, + 0x38544000, 0x38546000, 0x38548000, 0x3854A000, 0x3854C000, + 0x3854E000, 0x38550000, 0x38552000, 0x38554000, + 0x38556000, 0x38558000, 0x3855A000, 0x3855C000, 0x3855E000, + 0x38560000, 0x38562000, 0x38564000, 0x38566000, + 0x38568000, 0x3856A000, 0x3856C000, 0x3856E000, 0x38570000, + 0x38572000, 0x38574000, 0x38576000, 0x38578000, + 0x3857A000, 0x3857C000, 0x3857E000, 0x38580000, 0x38582000, + 0x38584000, 0x38586000, 0x38588000, 0x3858A000, + 0x3858C000, 0x3858E000, 0x38590000, 0x38592000, 0x38594000, + 0x38596000, 0x38598000, 0x3859A000, 0x3859C000, + 0x3859E000, 0x385A0000, 0x385A2000, 0x385A4000, 0x385A6000, + 0x385A8000, 0x385AA000, 0x385AC000, 0x385AE000, + 0x385B0000, 0x385B2000, 0x385B4000, 0x385B6000, 0x385B8000, + 0x385BA000, 0x385BC000, 0x385BE000, 0x385C0000, + 0x385C2000, 0x385C4000, 0x385C6000, 0x385C8000, 0x385CA000, + 0x385CC000, 0x385CE000, 0x385D0000, 0x385D2000, + 0x385D4000, 0x385D6000, 0x385D8000, 0x385DA000, 0x385DC000, + 0x385DE000, 0x385E0000, 0x385E2000, 0x385E4000, + 0x385E6000, 0x385E8000, 0x385EA000, 0x385EC000, 0x385EE000, + 0x385F0000, 0x385F2000, 0x385F4000, 0x385F6000, + 0x385F8000, 0x385FA000, 0x385FC000, 0x385FE000, 0x38600000, + 0x38602000, 0x38604000, 0x38606000, 0x38608000, + 0x3860A000, 0x3860C000, 0x3860E000, 0x38610000, 0x38612000, + 0x38614000, 0x38616000, 0x38618000, 0x3861A000, + 0x3861C000, 0x3861E000, 0x38620000, 0x38622000, 0x38624000, + 0x38626000, 0x38628000, 0x3862A000, 0x3862C000, + 0x3862E000, 0x38630000, 0x38632000, 0x38634000, 0x38636000, + 0x38638000, 0x3863A000, 0x3863C000, 0x3863E000, + 0x38640000, 0x38642000, 0x38644000, 0x38646000, 0x38648000, + 0x3864A000, 0x3864C000, 0x3864E000, 0x38650000, + 0x38652000, 0x38654000, 0x38656000, 0x38658000, 0x3865A000, + 0x3865C000, 0x3865E000, 0x38660000, 0x38662000, + 0x38664000, 0x38666000, 0x38668000, 0x3866A000, 0x3866C000, + 0x3866E000, 0x38670000, 0x38672000, 0x38674000, + 0x38676000, 0x38678000, 0x3867A000, 0x3867C000, 0x3867E000, + 0x38680000, 0x38682000, 0x38684000, 0x38686000, + 0x38688000, 0x3868A000, 0x3868C000, 0x3868E000, 0x38690000, + 0x38692000, 0x38694000, 0x38696000, 0x38698000, + 0x3869A000, 0x3869C000, 0x3869E000, 0x386A0000, 0x386A2000, + 0x386A4000, 0x386A6000, 0x386A8000, 0x386AA000, + 0x386AC000, 0x386AE000, 0x386B0000, 0x386B2000, 0x386B4000, + 0x386B6000, 0x386B8000, 0x386BA000, 0x386BC000, + 0x386BE000, 0x386C0000, 0x386C2000, 0x386C4000, 0x386C6000, + 0x386C8000, 0x386CA000, 0x386CC000, 0x386CE000, + 0x386D0000, 0x386D2000, 0x386D4000, 0x386D6000, 0x386D8000, + 0x386DA000, 0x386DC000, 0x386DE000, 0x386E0000, + 0x386E2000, 0x386E4000, 0x386E6000, 0x386E8000, 0x386EA000, + 0x386EC000, 0x386EE000, 0x386F0000, 0x386F2000, + 0x386F4000, 0x386F6000, 0x386F8000, 0x386FA000, 0x386FC000, + 0x386FE000, 0x38700000, 0x38702000, 0x38704000, + 0x38706000, 0x38708000, 0x3870A000, 0x3870C000, 0x3870E000, + 0x38710000, 0x38712000, 0x38714000, 0x38716000, + 0x38718000, 0x3871A000, 0x3871C000, 0x3871E000, 0x38720000, + 0x38722000, 0x38724000, 0x38726000, 0x38728000, + 0x3872A000, 0x3872C000, 0x3872E000, 0x38730000, 0x38732000, + 0x38734000, 0x38736000, 0x38738000, 0x3873A000, + 0x3873C000, 0x3873E000, 0x38740000, 0x38742000, 0x38744000, + 0x38746000, 0x38748000, 0x3874A000, 0x3874C000, + 0x3874E000, 0x38750000, 0x38752000, 0x38754000, 0x38756000, + 0x38758000, 0x3875A000, 0x3875C000, 0x3875E000, + 0x38760000, 0x38762000, 0x38764000, 0x38766000, 0x38768000, + 0x3876A000, 0x3876C000, 0x3876E000, 0x38770000, + 0x38772000, 0x38774000, 0x38776000, 0x38778000, 0x3877A000, + 0x3877C000, 0x3877E000, 0x38780000, 0x38782000, + 0x38784000, 0x38786000, 0x38788000, 0x3878A000, 0x3878C000, + 0x3878E000, 0x38790000, 0x38792000, 0x38794000, + 0x38796000, 0x38798000, 0x3879A000, 0x3879C000, 0x3879E000, + 0x387A0000, 0x387A2000, 0x387A4000, 0x387A6000, + 0x387A8000, 0x387AA000, 0x387AC000, 0x387AE000, 0x387B0000, + 0x387B2000, 0x387B4000, 0x387B6000, 0x387B8000, + 0x387BA000, 0x387BC000, 0x387BE000, 0x387C0000, 0x387C2000, + 0x387C4000, 0x387C6000, 0x387C8000, 0x387CA000, + 0x387CC000, 0x387CE000, 0x387D0000, 0x387D2000, 0x387D4000, + 0x387D6000, 0x387D8000, 0x387DA000, 0x387DC000, + 0x387DE000, 0x387E0000, 0x387E2000, 0x387E4000, 0x387E6000, + 0x387E8000, 0x387EA000, 0x387EC000, 0x387EE000, + 0x387F0000, 0x387F2000, 0x387F4000, 0x387F6000, 0x387F8000, + 0x387FA000, 0x387FC000, 0x387FE000}; + static const uint32 exponent_table[64] = {0x00000000, 0x00800000, 0x01000000, 0x01800000, 0x02000000, + 0x02800000, + 0x03000000, 0x03800000, 0x04000000, 0x04800000, 0x05000000, + 0x05800000, 0x06000000, 0x06800000, 0x07000000, + 0x07800000, 0x08000000, 0x08800000, 0x09000000, 0x09800000, + 0x0A000000, 0x0A800000, 0x0B000000, 0x0B800000, + 0x0C000000, 0x0C800000, 0x0D000000, 0x0D800000, 0x0E000000, + 0x0E800000, 0x0F000000, 0x47800000, 0x80000000, + 0x80800000, 0x81000000, 0x81800000, 0x82000000, 0x82800000, + 0x83000000, 0x83800000, 0x84000000, 0x84800000, + 0x85000000, 0x85800000, 0x86000000, 0x86800000, 0x87000000, + 0x87800000, 0x88000000, 0x88800000, 0x89000000, + 0x89800000, 0x8A000000, 0x8A800000, 0x8B000000, 0x8B800000, + 0x8C000000, 0x8C800000, 0x8D000000, 0x8D800000, + 0x8E000000, 0x8E800000, 0x8F000000, 0xC7800000}; + static const unsigned short offset_table[64] = {0, 1024, 1024, 1024, 1024, 1024, 1024, 1024, 1024, 1024, + 1024, 1024, + 1024, 1024, 1024, 1024, 1024, 1024, 1024, 1024, 1024, 1024, + 1024, 1024, 1024, 1024, 1024, 1024, 1024, 1024, + 1024, 1024, 0, 1024, 1024, 1024, 1024, 1024, 1024, 1024, + 1024, 1024, 1024, 1024, 1024, 1024, 1024, 1024, 1024, + 1024, 1024, 1024, 1024, 1024, 1024, 1024, 1024, 1024, 1024, + 1024, 1024, 1024, 1024, 1024}; + uint32 bits = mantissa_table[offset_table[value >> 10] + (value & 0x3FF)] + exponent_table[value >> 10]; + // return *reinterpret_cast(&bits); //violating strict aliasing! + float out; + std::memcpy(&out, &bits, sizeof(float)); + return out; + } + +/// Convert half-precision to IEEE double-precision. +/// \param value binary representation of half-precision value +/// \return double-precision value + inline double half2float_impl(uint16 value, double, true_type) { + typedef bits::type uint32; + typedef bits::type uint64; + uint32 hi = static_cast(value & 0x8000) << 16; + int abs = value & 0x7FFF; + if (abs) { + hi |= 0x3F000000 << static_cast(abs >= 0x7C00); + for (; abs < 0x400; abs <<= 1, hi -= 0x100000); + hi += static_cast(abs) << 10; + } + uint64 bits = static_cast(hi) << 32; + // return *reinterpret_cast(&bits); //violating strict aliasing! + double out; + std::memcpy(&out, &bits, sizeof(double)); + return out; + } + +/// Convert half-precision to non-IEEE floating point. +/// \tparam T type to convert to (builtin integer type) +/// \param value binary representation of half-precision value +/// \return floating point value + template + T half2float_impl(uint16 value, T, ...) { + T out; + int abs = value & 0x7FFF; + if (abs > 0x7C00) + out = std::numeric_limits::has_quiet_NaN ? std::numeric_limits::quiet_NaN() : T(); + else if (abs == 0x7C00) + out = std::numeric_limits::has_infinity ? std::numeric_limits::infinity() + : std::numeric_limits::max(); + else if (abs > 0x3FF) + out = std::ldexp(static_cast((abs & 0x3FF) | 0x400), (abs >> 10) - 25); + else + out = std::ldexp(static_cast(abs), -24); + return (value & 0x8000) ? -out : out; + } + +/// Convert half-precision to floating point. +/// \tparam T type to convert to (builtin integer type) +/// \param value binary representation of half-precision value +/// \return floating point value + template + T half2float(uint16 value) { + return half2float_impl( + value, T(), + bool_type::is_iec559 && sizeof(typename bits::type) == sizeof(T)>()); + } + +/// Convert half-precision floating point to integer. +/// \tparam R rounding mode to use, `std::round_indeterminate` for fastest rounding +/// \tparam E `true` for round to even, `false` for round away from zero +/// \tparam T type to convert to (buitlin integer type with at least 16 bits precision, excluding any implicit sign +/// bits) \param value binary representation of half-precision value \return integral value + template + T half2int_impl(uint16 value) { +#if HALF_ENABLE_CPP11_STATIC_ASSERT && HALF_ENABLE_CPP11_TYPE_TRAITS + static_assert(std::is_integral::value, "half to int conversion only supports builtin integer types"); +#endif + uint32_t e = value & 0x7FFF; + if (e >= 0x7C00) + return (value & 0x8000) ? std::numeric_limits::min() : std::numeric_limits::max(); + if (e < 0x3800) { + if (R == std::round_toward_infinity) + return T(~(value >> 15) & (e != 0)); + else if (R == std::round_toward_neg_infinity) + return -T(value > 0x8000); + return T(); + } + uint32_t m = (value & 0x3FF) | 0x400; + e >>= 10; + if (e < 25) { + if (R == std::round_to_nearest) + m += (1 << (24 - e)) - (~(m >> (25 - e)) & E); + else if (R == std::round_toward_infinity) + m += ((value >> 15) - 1) & ((1 << (25 - e)) - 1U); + else if (R == std::round_toward_neg_infinity) + m += -(value >> 15) & ((1 << (25 - e)) - 1U); + m >>= 25 - e; + } else + m <<= e - 25; + return (value & 0x8000) ? -static_cast(m) : static_cast(m); + } + +/// Convert half-precision floating point to integer. +/// \tparam R rounding mode to use, `std::round_indeterminate` for fastest rounding +/// \tparam T type to convert to (buitlin integer type with at least 16 bits precision, excluding any implicit sign +/// bits) \param value binary representation of half-precision value \return integral value + template + T half2int(uint16 value) { + return half2int_impl(value); + } + +/// Convert half-precision floating point to integer using round-to-nearest-away-from-zero. +/// \tparam T type to convert to (buitlin integer type with at least 16 bits precision, excluding any implicit sign +/// bits) \param value binary representation of half-precision value \return integral value + template + T half2int_up(uint16 value) { + return half2int_impl(value); + } + +/// Round half-precision number to nearest integer value. +/// \tparam R rounding mode to use, `std::round_indeterminate` for fastest rounding +/// \tparam E `true` for round to even, `false` for round away from zero +/// \param value binary representation of half-precision value +/// \return half-precision bits for nearest integral value + template + uint16 round_half_impl(uint16 value) { + uint32_t e = value & 0x7FFF; + uint16 result = value; + if (e < 0x3C00) { + result &= 0x8000; + if (R == std::round_to_nearest) + result |= 0x3C00U & -(e >= (0x3800 + E)); + else if (R == std::round_toward_infinity) + result |= 0x3C00U & -(~(value >> 15) & (e != 0)); + else if (R == std::round_toward_neg_infinity) + result |= 0x3C00U & -(value > 0x8000); + } else if (e < 0x6400) { + e = 25 - (e >> 10); + uint32_t mask = (1 << e) - 1; + if (R == std::round_to_nearest) + result += (1 << (e - 1)) - (~(result >> e) & E); + else if (R == std::round_toward_infinity) + result += mask & ((value >> 15) - 1); + else if (R == std::round_toward_neg_infinity) + result += mask & -(value >> 15); + result &= ~mask; + } + return result; + } + +/// Round half-precision number to nearest integer value. +/// \tparam R rounding mode to use, `std::round_indeterminate` for fastest rounding +/// \param value binary representation of half-precision value +/// \return half-precision bits for nearest integral value + template + uint16 round_half(uint16 value) { + return round_half_impl(value); + } + +/// Round half-precision number to nearest integer value using round-to-nearest-away-from-zero. +/// \param value binary representation of half-precision value +/// \return half-precision bits for nearest integral value + inline uint16 round_half_up(uint16 value) { + return round_half_impl(value); + } +/// \} + + struct functions; + template + struct unary_specialized; + template + struct binary_specialized; + template + struct half_caster; + } // namespace detail + +/// Half-precision floating point type. +/// This class implements an IEEE-conformant half-precision floating point type with the usual arithmetic operators and +/// conversions. It is implicitly convertible to single-precision floating point, which makes artihmetic expressions and +/// functions with mixed-type operands to be of the most precise operand type. Additionally all arithmetic operations +/// (and many mathematical functions) are carried out in single-precision internally. All conversions from single- to +/// half-precision are done using the library's default rounding mode, but temporary results inside chained arithmetic +/// expressions are kept in single-precision as long as possible (while of course still maintaining a strong +/// half-precision type). +/// +/// According to the C++98/03 definition, the half type is not a POD type. But according to C++11's less strict and +/// extended definitions it is both a standard layout type and a trivially copyable type (even if not a POD type), which +/// means it can be standard-conformantly copied using raw binary copies. But in this context some more words about the +/// actual size of the type. Although the half is representing an IEEE 16-bit type, it does not neccessarily have to be +/// of exactly 16-bits size. But on any reasonable implementation the actual binary representation of this type will +/// most probably not ivolve any additional "magic" or padding beyond the simple binary representation of the underlying +/// 16-bit IEEE number, even if not strictly guaranteed by the standard. But even then it only has an actual size of 16 +/// bits if your C++ implementation supports an unsigned integer type of exactly 16 bits width. But this should be the +/// case on nearly any reasonable platform. +/// +/// So if your C++ implementation is not totally exotic or imposes special alignment requirements, it is a reasonable +/// assumption that the data of a half is just comprised of the 2 bytes of the underlying IEEE representation. + class half { + friend struct detail::functions; + friend struct detail::unary_specialized; + friend struct detail::binary_specialized; + template + friend + struct detail::half_caster; + + friend class std::numeric_limits; + +#if HALF_ENABLE_CPP11_HASH + friend struct std::hash; +#endif +#if HALF_ENABLE_CPP11_USER_LITERALS + + friend half literal::operator "" _h(long double); + +#endif + + public: + /// Default constructor. + /// This initializes the half to 0. Although this does not match the builtin types' default-initialization semantics + /// and may be less efficient than no initialization, it is needed to provide proper value-initialization semantics. + HALF_CONSTEXPR half() HALF_NOEXCEPT: data_() {} + + /// Copy constructor. + /// \tparam T type of concrete half expression + /// \param rhs half expression to copy from + half(detail::expr rhs) + : data_(detail::float2half(static_cast(rhs))) { + } + + /// Conversion constructor. + /// \param rhs float to convert + explicit half(float rhs) + : data_(detail::float2half(rhs)) { + } + + /// Conversion to single-precision. + /// \return single precision value representing expression value + operator float() const { + return detail::half2float(data_); + } + + /// Assignment operator. + /// \tparam T type of concrete half expression + /// \param rhs half expression to copy from + /// \return reference to this half + half &operator=(detail::expr rhs) { + return *this = static_cast(rhs); + } + + /// Arithmetic assignment. + /// \tparam T type of concrete half expression + /// \param rhs half expression to add + /// \return reference to this half + template + typename detail::enable::type operator+=(T rhs) { + return *this += static_cast(rhs); + } + + /// Arithmetic assignment. + /// \tparam T type of concrete half expression + /// \param rhs half expression to subtract + /// \return reference to this half + template + typename detail::enable::type operator-=(T rhs) { + return *this -= static_cast(rhs); + } + + /// Arithmetic assignment. + /// \tparam T type of concrete half expression + /// \param rhs half expression to multiply with + /// \return reference to this half + template + typename detail::enable::type operator*=(T rhs) { + return *this *= static_cast(rhs); + } + + /// Arithmetic assignment. + /// \tparam T type of concrete half expression + /// \param rhs half expression to divide by + /// \return reference to this half + template + typename detail::enable::type operator/=(T rhs) { + return *this /= static_cast(rhs); + } + + /// Assignment operator. + /// \param rhs single-precision value to copy from + /// \return reference to this half + half &operator=(float rhs) { + data_ = detail::float2half(rhs); + return *this; + } + + /// Arithmetic assignment. + /// \param rhs single-precision value to add + /// \return reference to this half + half &operator+=(float rhs) { + data_ = detail::float2half(detail::half2float(data_) + rhs); + return *this; + } + + /// Arithmetic assignment. + /// \param rhs single-precision value to subtract + /// \return reference to this half + half &operator-=(float rhs) { + data_ = detail::float2half(detail::half2float(data_) - rhs); + return *this; + } + + /// Arithmetic assignment. + /// \param rhs single-precision value to multiply with + /// \return reference to this half + half &operator*=(float rhs) { + data_ = detail::float2half(detail::half2float(data_) * rhs); + return *this; + } + + /// Arithmetic assignment. + /// \param rhs single-precision value to divide by + /// \return reference to this half + half &operator/=(float rhs) { + data_ = detail::float2half(detail::half2float(data_) / rhs); + return *this; + } + + /// Prefix increment. + /// \return incremented half value + half &operator++() { + return *this += 1.0f; + } + + /// Prefix decrement. + /// \return decremented half value + half &operator--() { + return *this -= 1.0f; + } + + /// Postfix increment. + /// \return non-incremented half value + half operator++(int) { + half out(*this); + ++*this; + return out; + } + + /// Postfix decrement. + /// \return non-decremented half value + half operator--(int) { + half out(*this); + --*this; + return out; + } + + private: + /// Rounding mode to use + static const std::float_round_style round_style = (std::float_round_style) (HALF_ROUND_STYLE); + + /// Constructor. + /// \param bits binary representation to set half to + HALF_CONSTEXPR half(detail::binary_t, detail::uint16 bits) HALF_NOEXCEPT: data_(bits) {} + + /// Internal binary representation + detail::uint16 data_; + }; + +#if HALF_ENABLE_CPP11_USER_LITERALS + namespace literal { +/// Half literal. +/// While this returns an actual half-precision value, half literals can unfortunately not be constant expressions due +/// to rather involved conversions. +/// \param value literal value +/// \return half with given value (if representable) + inline half operator "" _h(long double value) { + return half(detail::binary, detail::float2half(value)); + } + } // namespace literal +#endif + + namespace detail { +/// Wrapper implementing unspecialized half-precision functions. + struct functions { + /// Addition implementation. + /// \param x first operand + /// \param y second operand + /// \return Half-precision sum stored in single-precision + static expr plus(float x, float y) { + return expr(x + y); + } + + /// Subtraction implementation. + /// \param x first operand + /// \param y second operand + /// \return Half-precision difference stored in single-precision + static expr minus(float x, float y) { + return expr(x - y); + } + + /// Multiplication implementation. + /// \param x first operand + /// \param y second operand + /// \return Half-precision product stored in single-precision + static expr multiplies(float x, float y) { + return expr(x * y); + } + + /// Division implementation. + /// \param x first operand + /// \param y second operand + /// \return Half-precision quotient stored in single-precision + static expr divides(float x, float y) { + return expr(x / y); + } + + /// Output implementation. + /// \param out stream to write to + /// \param arg value to write + /// \return reference to stream + template + static std::basic_ostream &write(std::basic_ostream &out, float arg) { + return out << arg; + } + + /// Input implementation. + /// \param in stream to read from + /// \param arg half to read into + /// \return reference to stream + template + static std::basic_istream &read(std::basic_istream &in, half &arg) { + float f; + if (in >> f) + arg = f; + return in; + } + + /// Modulo implementation. + /// \param x first operand + /// \param y second operand + /// \return Half-precision division remainder stored in single-precision + static expr fmod(float x, float y) { + return expr(std::fmod(x, y)); + } + + /// Remainder implementation. + /// \param x first operand + /// \param y second operand + /// \return Half-precision division remainder stored in single-precision + static expr remainder(float x, float y) { +#if HALF_ENABLE_CPP11_CMATH + return expr(std::remainder(x, y)); +#else + if (builtin_isnan(x) || builtin_isnan(y)) + return expr(std::numeric_limits::quiet_NaN()); + float ax = std::fabs(x), ay = std::fabs(y); + if (ax >= 65536.0f || ay < std::ldexp(1.0f, -24)) + return expr(std::numeric_limits::quiet_NaN()); + if (ay >= 65536.0f) + return expr(x); + if (ax == ay) + return expr(builtin_signbit(x) ? -0.0f : 0.0f); + ax = std::fmod(ax, ay + ay); + float y2 = 0.5f * ay; + if (ax > y2) + { + ax -= ay; + if (ax >= y2) + ax -= ay; + } + return expr(builtin_signbit(x) ? -ax : ax); +#endif + } + + /// Remainder implementation. + /// \param x first operand + /// \param y second operand + /// \param quo address to store quotient bits at + /// \return Half-precision division remainder stored in single-precision + static expr remquo(float x, float y, int *quo) { +#if HALF_ENABLE_CPP11_CMATH + return expr(std::remquo(x, y, quo)); +#else + if (builtin_isnan(x) || builtin_isnan(y)) + return expr(std::numeric_limits::quiet_NaN()); + bool sign = builtin_signbit(x), qsign = static_cast(sign ^ builtin_signbit(y)); + float ax = std::fabs(x), ay = std::fabs(y); + if (ax >= 65536.0f || ay < std::ldexp(1.0f, -24)) + return expr(std::numeric_limits::quiet_NaN()); + if (ay >= 65536.0f) + return expr(x); + if (ax == ay) + return *quo = qsign ? -1 : 1, expr(sign ? -0.0f : 0.0f); + ax = std::fmod(ax, 8.0f * ay); + int cquo = 0; + if (ax >= 4.0f * ay) + { + ax -= 4.0f * ay; + cquo += 4; + } + if (ax >= 2.0f * ay) + { + ax -= 2.0f * ay; + cquo += 2; + } + float y2 = 0.5f * ay; + if (ax > y2) + { + ax -= ay; + ++cquo; + if (ax >= y2) + { + ax -= ay; + ++cquo; + } + } + return *quo = qsign ? -cquo : cquo, expr(sign ? -ax : ax); +#endif + } + + /// Positive difference implementation. + /// \param x first operand + /// \param y second operand + /// \return Positive difference stored in single-precision + static expr fdim(float x, float y) { +#if HALF_ENABLE_CPP11_CMATH + return expr(std::fdim(x, y)); +#else + return expr((x <= y) ? 0.0f : (x - y)); +#endif + } + + /// Fused multiply-add implementation. + /// \param x first operand + /// \param y second operand + /// \param z third operand + /// \return \a x * \a y + \a z stored in single-precision + static expr fma(float x, float y, float z) { +#if HALF_ENABLE_CPP11_CMATH && defined(FP_FAST_FMAF) + return expr(std::fma(x, y, z)); +#else + return expr(x * y + z); +#endif + } + + /// Get NaN. + /// \return Half-precision quiet NaN + static half nanh() { + return half(binary, 0x7FFF); + } + + /// Exponential implementation. + /// \param arg function argument + /// \return function value stored in single-preicision + static expr exp(float arg) { + return expr(std::exp(arg)); + } + + /// Exponential implementation. + /// \param arg function argument + /// \return function value stored in single-preicision + static expr expm1(float arg) { +#if HALF_ENABLE_CPP11_CMATH + return expr(std::expm1(arg)); +#else + return expr(static_cast(std::exp(static_cast(arg)) - 1.0)); +#endif + } + + /// Binary exponential implementation. + /// \param arg function argument + /// \return function value stored in single-preicision + static expr exp2(float arg) { +#if HALF_ENABLE_CPP11_CMATH + return expr(std::exp2(arg)); +#else + return expr(static_cast(std::exp(arg * 0.69314718055994530941723212145818))); +#endif + } + + /// Logarithm implementation. + /// \param arg function argument + /// \return function value stored in single-preicision + static expr log(float arg) { + return expr(std::log(arg)); + } + + /// Common logarithm implementation. + /// \param arg function argument + /// \return function value stored in single-preicision + static expr log10(float arg) { + return expr(std::log10(arg)); + } + + /// Logarithm implementation. + /// \param arg function argument + /// \return function value stored in single-preicision + static expr log1p(float arg) { +#if HALF_ENABLE_CPP11_CMATH + return expr(std::log1p(arg)); +#else + return expr(static_cast(std::log(1.0 + arg))); +#endif + } + + /// Binary logarithm implementation. + /// \param arg function argument + /// \return function value stored in single-preicision + static expr log2(float arg) { +#if HALF_ENABLE_CPP11_CMATH + return expr(std::log2(arg)); +#else + return expr(static_cast(std::log(static_cast(arg)) * 1.4426950408889634073599246810019)); +#endif + } + + /// Square root implementation. + /// \param arg function argument + /// \return function value stored in single-preicision + static expr sqrt(float arg) { + return expr(std::sqrt(arg)); + } + + /// Cubic root implementation. + /// \param arg function argument + /// \return function value stored in single-preicision + static expr cbrt(float arg) { +#if HALF_ENABLE_CPP11_CMATH + return expr(std::cbrt(arg)); +#else + if (builtin_isnan(arg) || builtin_isinf(arg)) + return expr(arg); + return expr(builtin_signbit(arg) ? -static_cast(std::pow(-static_cast(arg), 1.0 / 3.0)) + : static_cast(std::pow(static_cast(arg), 1.0 / 3.0))); +#endif + } + + /// Hypotenuse implementation. + /// \param x first argument + /// \param y second argument + /// \return function value stored in single-preicision + static expr hypot(float x, float y) { +#if HALF_ENABLE_CPP11_CMATH + return expr(std::hypot(x, y)); +#else + return expr((builtin_isinf(x) || builtin_isinf(y)) + ? std::numeric_limits::infinity() + : static_cast(std::sqrt(static_cast(x) * x + static_cast(y) * y))); +#endif + } + + /// Power implementation. + /// \param base value to exponentiate + /// \param exp power to expontiate to + /// \return function value stored in single-preicision + static expr pow(float base, float exp) { + return expr(std::pow(base, exp)); + } + + /// Sine implementation. + /// \param arg function argument + /// \return function value stored in single-preicision + static expr sin(float arg) { + return expr(std::sin(arg)); + } + + /// Cosine implementation. + /// \param arg function argument + /// \return function value stored in single-preicision + static expr cos(float arg) { + return expr(std::cos(arg)); + } + + /// Tan implementation. + /// \param arg function argument + /// \return function value stored in single-preicision + static expr tan(float arg) { + return expr(std::tan(arg)); + } + + /// Arc sine implementation. + /// \param arg function argument + /// \return function value stored in single-preicision + static expr asin(float arg) { + return expr(std::asin(arg)); + } + + /// Arc cosine implementation. + /// \param arg function argument + /// \return function value stored in single-preicision + static expr acos(float arg) { + return expr(std::acos(arg)); + } + + /// Arc tangent implementation. + /// \param arg function argument + /// \return function value stored in single-preicision + static expr atan(float arg) { + return expr(std::atan(arg)); + } + + /// Arc tangent implementation. + /// \param x first argument + /// \param y second argument + /// \return function value stored in single-preicision + static expr atan2(float x, float y) { + return expr(std::atan2(x, y)); + } + + /// Hyperbolic sine implementation. + /// \param arg function argument + /// \return function value stored in single-preicision + static expr sinh(float arg) { + return expr(std::sinh(arg)); + } + + /// Hyperbolic cosine implementation. + /// \param arg function argument + /// \return function value stored in single-preicision + static expr cosh(float arg) { + return expr(std::cosh(arg)); + } + + /// Hyperbolic tangent implementation. + /// \param arg function argument + /// \return function value stored in single-preicision + static expr tanh(float arg) { + return expr(std::tanh(arg)); + } + + /// Hyperbolic area sine implementation. + /// \param arg function argument + /// \return function value stored in single-preicision + static expr asinh(float arg) { +#if HALF_ENABLE_CPP11_CMATH + return expr(std::asinh(arg)); +#else + return expr((arg == -std::numeric_limits::infinity()) + ? arg + : static_cast(std::log(arg + std::sqrt(arg * arg + 1.0)))); +#endif + } + + /// Hyperbolic area cosine implementation. + /// \param arg function argument + /// \return function value stored in single-preicision + static expr acosh(float arg) { +#if HALF_ENABLE_CPP11_CMATH + return expr(std::acosh(arg)); +#else + return expr((arg < -1.0f) ? std::numeric_limits::quiet_NaN() + : static_cast(std::log(arg + std::sqrt(arg * arg - 1.0)))); +#endif + } + + /// Hyperbolic area tangent implementation. + /// \param arg function argument + /// \return function value stored in single-preicision + static expr atanh(float arg) { +#if HALF_ENABLE_CPP11_CMATH + return expr(std::atanh(arg)); +#else + return expr(static_cast(0.5 * std::log((1.0 + arg) / (1.0 - arg)))); +#endif + } + + /// Error function implementation. + /// \param arg function argument + /// \return function value stored in single-preicision + static expr erf(float arg) { +#if HALF_ENABLE_CPP11_CMATH + return expr(std::erf(arg)); +#else + return expr(static_cast(erf(static_cast(arg)))); +#endif + } + + /// Complementary implementation. + /// \param arg function argument + /// \return function value stored in single-preicision + static expr erfc(float arg) { +#if HALF_ENABLE_CPP11_CMATH + return expr(std::erfc(arg)); +#else + return expr(static_cast(1.0 - erf(static_cast(arg)))); +#endif + } + + /// Gamma logarithm implementation. + /// \param arg function argument + /// \return function value stored in single-preicision + static expr lgamma(float arg) { +#if HALF_ENABLE_CPP11_CMATH + return expr(std::lgamma(arg)); +#else + if (builtin_isinf(arg)) + return expr(std::numeric_limits::infinity()); + if (arg < 0.0f) + { + float i, f = std::modf(-arg, &i); + if (f == 0.0f) + return expr(std::numeric_limits::infinity()); + return expr(static_cast(1.1447298858494001741434273513531 + - std::log(std::abs(std::sin(3.1415926535897932384626433832795 * f))) - lgamma(1.0 - arg))); + } + return expr(static_cast(lgamma(static_cast(arg)))); +#endif + } + + /// Gamma implementation. + /// \param arg function argument + /// \return function value stored in single-preicision + static expr tgamma(float arg) { +#if HALF_ENABLE_CPP11_CMATH + return expr(std::tgamma(arg)); +#else + if (arg == 0.0f) + return builtin_signbit(arg) ? expr(-std::numeric_limits::infinity()) + : expr(std::numeric_limits::infinity()); + if (arg < 0.0f) + { + float i, f = std::modf(-arg, &i); + if (f == 0.0f) + return expr(std::numeric_limits::quiet_NaN()); + double value = 3.1415926535897932384626433832795 + / (std::sin(3.1415926535897932384626433832795 * f) * std::exp(lgamma(1.0 - arg))); + return expr(static_cast((std::fmod(i, 2.0f) == 0.0f) ? -value : value)); + } + if (builtin_isinf(arg)) + return expr(arg); + return expr(static_cast(std::exp(lgamma(static_cast(arg))))); +#endif + } + + /// Floor implementation. + /// \param arg value to round + /// \return rounded value + static half floor(half arg) { + return half(binary, round_half(arg.data_)); + } + + /// Ceiling implementation. + /// \param arg value to round + /// \return rounded value + static half ceil(half arg) { + return half(binary, round_half(arg.data_)); + } + + /// Truncation implementation. + /// \param arg value to round + /// \return rounded value + static half trunc(half arg) { + return half(binary, round_half(arg.data_)); + } + + /// Nearest integer implementation. + /// \param arg value to round + /// \return rounded value + static half round(half arg) { + return half(binary, round_half_up(arg.data_)); + } + + /// Nearest integer implementation. + /// \param arg value to round + /// \return rounded value + static long lround(half arg) { + return detail::half2int_up(arg.data_); + } + + /// Nearest integer implementation. + /// \param arg value to round + /// \return rounded value + static half rint(half arg) { + return half(binary, round_half(arg.data_)); + } + + /// Nearest integer implementation. + /// \param arg value to round + /// \return rounded value + static long lrint(half arg) { + return detail::half2int(arg.data_); + } + +#if HALF_ENABLE_CPP11_LONG_LONG + + /// Nearest integer implementation. + /// \param arg value to round + /// \return rounded value + static long long llround(half arg) { + return detail::half2int_up(arg.data_); + } + + /// Nearest integer implementation. + /// \param arg value to round + /// \return rounded value + static long long llrint(half arg) { + return detail::half2int(arg.data_); + } + +#endif + + /// Decompression implementation. + /// \param arg number to decompress + /// \param exp address to store exponent at + /// \return normalized significant + static half frexp(half arg, int *exp) { + int m = arg.data_ & 0x7FFF, e = -14; + if (m >= 0x7C00 || !m) + return *exp = 0, arg; + for (; m < 0x400; m <<= 1, --e); + return *exp = e + (m >> 10), half(binary, (arg.data_ & 0x8000) | 0x3800 | (m & 0x3FF)); + } + + /// Decompression implementation. + /// \param arg number to decompress + /// \param iptr address to store integer part at + /// \return fractional part + static half modf(half arg, half *iptr) { + uint32_t e = arg.data_ & 0x7FFF; + if (e >= 0x6400) + return *iptr = arg, half(binary, arg.data_ & (0x8000U | -(e > 0x7C00))); + if (e < 0x3C00) + return iptr->data_ = arg.data_ & 0x8000, arg; + e >>= 10; + uint32_t mask = (1 << (25 - e)) - 1, m = arg.data_ & mask; + iptr->data_ = arg.data_ & ~mask; + if (!m) + return half(binary, arg.data_ & 0x8000); + for (; m < 0x400; m <<= 1, --e); + return half(binary, static_cast((arg.data_ & 0x8000) | (e << 10) | (m & 0x3FF))); + } + + /// Scaling implementation. + /// \param arg number to scale + /// \param exp power of two to scale by + /// \return scaled number + static half scalbln(half arg, long exp) { + uint32_t m = arg.data_ & 0x7FFF; + if (m >= 0x7C00 || !m) + return arg; + for (; m < 0x400; m <<= 1, --exp); + exp += m >> 10; + uint16 value = arg.data_ & 0x8000; + if (exp > 30) { + if (half::round_style == std::round_toward_zero) + value |= 0x7BFF; + else if (half::round_style == std::round_toward_infinity) + value |= 0x7C00 - (value >> 15); + else if (half::round_style == std::round_toward_neg_infinity) + value |= 0x7BFF + (value >> 15); + else + value |= 0x7C00; + } else if (exp > 0) + value |= (exp << 10) | (m & 0x3FF); + else if (exp > -11) { + m = (m & 0x3FF) | 0x400; + if (half::round_style == std::round_to_nearest) { + m += 1 << -exp; +#if HALF_ROUND_TIES_TO_EVEN + m -= (m >> (1 - exp)) & 1; +#endif + } else if (half::round_style == std::round_toward_infinity) + m += ((value >> 15) - 1) & ((1 << (1 - exp)) - 1U); + else if (half::round_style == std::round_toward_neg_infinity) + m += -(value >> 15) & ((1 << (1 - exp)) - 1U); + value |= m >> (1 - exp); + } else if (half::round_style == std::round_toward_infinity) + value -= (value >> 15) - 1; + else if (half::round_style == std::round_toward_neg_infinity) + value += value >> 15; + return half(binary, value); + } + + /// Exponent implementation. + /// \param arg number to query + /// \return floating point exponent + static int ilogb(half arg) { + int abs = arg.data_ & 0x7FFF; + if (!abs) + return FP_ILOGB0; + if (abs < 0x7C00) { + int exp = (abs >> 10) - 15; + if (abs < 0x400) + for (; abs < 0x200; abs <<= 1, --exp); + return exp; + } + if (abs > 0x7C00) + return FP_ILOGBNAN; + return INT_MAX; + } + + /// Exponent implementation. + /// \param arg number to query + /// \return floating point exponent + static half logb(half arg) { + int abs = arg.data_ & 0x7FFF; + if (!abs) + return half(binary, 0xFC00); + if (abs < 0x7C00) { + int exp = (abs >> 10) - 15; + if (abs < 0x400) + for (; abs < 0x200; abs <<= 1, --exp); + uint16 bits = (exp < 0) << 15; + if (exp) { + uint32_t m = std::abs(exp) << 6, e = 18; + for (; m < 0x400; m <<= 1, --e); + bits |= (e << 10) + m; + } + return half(binary, bits); + } + if (abs > 0x7C00) + return arg; + return half(binary, 0x7C00); + } + + /// Enumeration implementation. + /// \param from number to increase/decrease + /// \param to direction to enumerate into + /// \return next representable number + static half nextafter(half from, half to) { + uint16 fabs = from.data_ & 0x7FFF, tabs = to.data_ & 0x7FFF; + if (fabs > 0x7C00) + return from; + if (tabs > 0x7C00 || from.data_ == to.data_ || !(fabs | tabs)) + return to; + if (!fabs) + return half(binary, (to.data_ & 0x8000) + 1); + bool lt = ((fabs == from.data_) ? static_cast(fabs) : -static_cast(fabs)) + < ((tabs == to.data_) ? static_cast(tabs) : -static_cast(tabs)); + return half(binary, from.data_ + (((from.data_ >> 15) ^ static_cast(lt)) << 1) - 1); + } + + /// Enumeration implementation. + /// \param from number to increase/decrease + /// \param to direction to enumerate into + /// \return next representable number + static half nexttoward(half from, long double to) { + if (isnan(from)) + return from; + long double lfrom = static_cast(from); + if (builtin_isnan(to) || lfrom == to) + return half(static_cast(to)); + if (!(from.data_ & 0x7FFF)) + return half(binary, (static_cast(builtin_signbit(to)) << 15) + 1); + return half(binary, from.data_ + (((from.data_ >> 15) ^ static_cast(lfrom < to)) << 1) - 1); + } + + /// Sign implementation + /// \param x first operand + /// \param y second operand + /// \return composed value + static half copysign(half x, half y) { + return half(binary, x.data_ ^ ((x.data_ ^ y.data_) & 0x8000)); + } + + /// Classification implementation. + /// \param arg value to classify + /// \retval true if infinite number + /// \retval false else + static int fpclassify(half arg) { + uint32_t abs = arg.data_ & 0x7FFF; + return abs + ? ((abs > 0x3FF) ? ((abs >= 0x7C00) ? ((abs > 0x7C00) ? FP_NAN : FP_INFINITE) : FP_NORMAL) + : FP_SUBNORMAL) + : FP_ZERO; + } + + /// Classification implementation. + /// \param arg value to classify + /// \retval true if finite number + /// \retval false else + static bool isfinite(half arg) { + return (arg.data_ & 0x7C00) != 0x7C00; + } + + /// Classification implementation. + /// \param arg value to classify + /// \retval true if infinite number + /// \retval false else + static bool isinf(half arg) { + return (arg.data_ & 0x7FFF) == 0x7C00; + } + + /// Classification implementation. + /// \param arg value to classify + /// \retval true if not a number + /// \retval false else + static bool isnan(half arg) { + return (arg.data_ & 0x7FFF) > 0x7C00; + } + + /// Classification implementation. + /// \param arg value to classify + /// \retval true if normal number + /// \retval false else + static bool isnormal(half arg) { + return ((arg.data_ & 0x7C00) != 0) & ((arg.data_ & 0x7C00) != 0x7C00); + } + + /// Sign bit implementation. + /// \param arg value to check + /// \retval true if signed + /// \retval false if unsigned + static bool signbit(half arg) { + return (arg.data_ & 0x8000) != 0; + } + + /// Comparison implementation. + /// \param x first operand + /// \param y second operand + /// \retval true if operands equal + /// \retval false else + static bool isequal(half x, half y) { + return (x.data_ == y.data_ || !((x.data_ | y.data_) & 0x7FFF)) && !isnan(x); + } + + /// Comparison implementation. + /// \param x first operand + /// \param y second operand + /// \retval true if operands not equal + /// \retval false else + static bool isnotequal(half x, half y) { + return (x.data_ != y.data_ && ((x.data_ | y.data_) & 0x7FFF)) || isnan(x); + } + + /// Comparison implementation. + /// \param x first operand + /// \param y second operand + /// \retval true if \a x > \a y + /// \retval false else + static bool isgreater(half x, half y) { + int xabs = x.data_ & 0x7FFF, yabs = y.data_ & 0x7FFF; + return xabs <= 0x7C00 && yabs <= 0x7C00 + && (((xabs == x.data_) ? xabs : -xabs) > ((yabs == y.data_) ? yabs : -yabs)); + } + + /// Comparison implementation. + /// \param x first operand + /// \param y second operand + /// \retval true if \a x >= \a y + /// \retval false else + static bool isgreaterequal(half x, half y) { + int xabs = x.data_ & 0x7FFF, yabs = y.data_ & 0x7FFF; + return xabs <= 0x7C00 && yabs <= 0x7C00 + && (((xabs == x.data_) ? xabs : -xabs) >= ((yabs == y.data_) ? yabs : -yabs)); + } + + /// Comparison implementation. + /// \param x first operand + /// \param y second operand + /// \retval true if \a x < \a y + /// \retval false else + static bool isless(half x, half y) { + int xabs = x.data_ & 0x7FFF, yabs = y.data_ & 0x7FFF; + return xabs <= 0x7C00 && yabs <= 0x7C00 + && (((xabs == x.data_) ? xabs : -xabs) < ((yabs == y.data_) ? yabs : -yabs)); + } + + /// Comparison implementation. + /// \param x first operand + /// \param y second operand + /// \retval true if \a x <= \a y + /// \retval false else + static bool islessequal(half x, half y) { + int xabs = x.data_ & 0x7FFF, yabs = y.data_ & 0x7FFF; + return xabs <= 0x7C00 && yabs <= 0x7C00 + && (((xabs == x.data_) ? xabs : -xabs) <= ((yabs == y.data_) ? yabs : -yabs)); + } + + /// Comparison implementation. + /// \param x first operand + /// \param y second operand + /// \retval true if either \a x > \a y nor \a x < \a y + /// \retval false else + static bool islessgreater(half x, half y) { + int xabs = x.data_ & 0x7FFF, yabs = y.data_ & 0x7FFF; + if (xabs > 0x7C00 || yabs > 0x7C00) + return false; + int a = (xabs == x.data_) ? xabs : -xabs, b = (yabs == y.data_) ? yabs : -yabs; + return a < b || a > b; + } + + /// Comparison implementation. + /// \param x first operand + /// \param y second operand + /// \retval true if operand unordered + /// \retval false else + static bool isunordered(half x, half y) { + return isnan(x) || isnan(y); + } + + private: + static double erf(double arg) { + if (builtin_isinf(arg)) + return (arg < 0.0) ? -1.0 : 1.0; + double x2 = arg * arg, ax2 = 0.147 * x2, + value = std::sqrt( + 1.0 - std::exp(-x2 * (1.2732395447351626861510701069801 + ax2) / (1.0 + ax2))); + return builtin_signbit(arg) ? -value : value; + } + + static double lgamma(double arg) { + double v = 1.0; + for (; arg < 8.0; ++arg) + v *= arg; + double w = 1.0 / (arg * arg); + return (((((((-0.02955065359477124183006535947712 * w + 0.00641025641025641025641025641026) * w + + -0.00191752691752691752691752691753) + * w + + 8.4175084175084175084175084175084e-4) + * w + + -5.952380952380952380952380952381e-4) + * w + + 7.9365079365079365079365079365079e-4) + * w + + -0.00277777777777777777777777777778) + * w + + 0.08333333333333333333333333333333) + / arg + + 0.91893853320467274178032973640562 - std::log(v) - arg + (arg - 0.5) * std::log(arg); + } + }; + +/// Wrapper for unary half-precision functions needing specialization for individual argument types. +/// \tparam T argument type + template + struct unary_specialized { + /// Negation implementation. + /// \param arg value to negate + /// \return negated value + static HALF_CONSTEXPR half negate(half arg) { + return half(binary, arg.data_ ^ 0x8000); + } + + /// Absolute value implementation. + /// \param arg function argument + /// \return absolute value + static half fabs(half arg) { + return half(binary, arg.data_ & 0x7FFF); + } + }; + + template<> + struct unary_specialized { + static HALF_CONSTEXPR expr negate(float arg) { + return expr(-arg); + } + + static expr fabs(float arg) { + return expr(std::fabs(arg)); + } + }; + +/// Wrapper for binary half-precision functions needing specialization for individual argument types. +/// \tparam T first argument type +/// \tparam U first argument type + template + struct binary_specialized { + /// Minimum implementation. + /// \param x first operand + /// \param y second operand + /// \return minimum value + static expr fmin(float x, float y) { +#if HALF_ENABLE_CPP11_CMATH + return expr(std::fmin(x, y)); +#else + if (builtin_isnan(x)) + return expr(y); + if (builtin_isnan(y)) + return expr(x); + return expr(std::min(x, y)); +#endif + } + + /// Maximum implementation. + /// \param x first operand + /// \param y second operand + /// \return maximum value + static expr fmax(float x, float y) { +#if HALF_ENABLE_CPP11_CMATH + return expr(std::fmax(x, y)); +#else + if (builtin_isnan(x)) + return expr(y); + if (builtin_isnan(y)) + return expr(x); + return expr(std::max(x, y)); +#endif + } + }; + + template<> + struct binary_specialized { + static half fmin(half x, half y) { + int xabs = x.data_ & 0x7FFF, yabs = y.data_ & 0x7FFF; + if (xabs > 0x7C00) + return y; + if (yabs > 0x7C00) + return x; + return (((xabs == x.data_) ? xabs : -xabs) > ((yabs == y.data_) ? yabs : -yabs)) ? y : x; + } + + static half fmax(half x, half y) { + int xabs = x.data_ & 0x7FFF, yabs = y.data_ & 0x7FFF; + if (xabs > 0x7C00) + return y; + if (yabs > 0x7C00) + return x; + return (((xabs == x.data_) ? xabs : -xabs) < ((yabs == y.data_) ? yabs : -yabs)) ? y : x; + } + }; + +/// Helper class for half casts. +/// This class template has to be specialized for all valid cast argument to define an appropriate static `cast` member +/// function and a corresponding `type` member denoting its return type. +/// \tparam T destination type +/// \tparam U source type +/// \tparam R rounding mode to use + template + struct half_caster { + }; + + template + struct half_caster { +#if HALF_ENABLE_CPP11_STATIC_ASSERT && HALF_ENABLE_CPP11_TYPE_TRAITS + static_assert(std::is_arithmetic::value, "half_cast from non-arithmetic type unsupported"); +#endif + + static half cast(U arg) { + return cast_impl(arg, is_float()); + }; + + private: + static half cast_impl(U arg, true_type) { + return half(binary, float2half(arg)); + } + + static half cast_impl(U arg, false_type) { + return half(binary, int2half(arg)); + } + }; + + template + struct half_caster { +#if HALF_ENABLE_CPP11_STATIC_ASSERT && HALF_ENABLE_CPP11_TYPE_TRAITS + static_assert(std::is_arithmetic::value, "half_cast to non-arithmetic type unsupported"); +#endif + + static T cast(half arg) { + return cast_impl(arg, is_float()); + } + + private: + static T cast_impl(half arg, true_type) { + return half2float(arg.data_); + } + + static T cast_impl(half arg, false_type) { + return half2int(arg.data_); + } + }; + + template + struct half_caster { +#if HALF_ENABLE_CPP11_STATIC_ASSERT && HALF_ENABLE_CPP11_TYPE_TRAITS + static_assert(std::is_arithmetic::value, "half_cast to non-arithmetic type unsupported"); +#endif + + static T cast(expr arg) { + return cast_impl(arg, is_float()); + } + + private: + static T cast_impl(float arg, true_type) { + return static_cast(arg); + } + + static T cast_impl(half arg, false_type) { + return half2int(arg.data_); + } + }; + + template + struct half_caster { + static half cast(half arg) { + return arg; + } + }; + + template + struct half_caster : half_caster { + }; + +/// \name Comparison operators +/// \{ + +/// Comparison for equality. +/// \param x first operand +/// \param y second operand +/// \retval true if operands equal +/// \retval false else + template + typename enable::type operator==(T x, U y) { + return functions::isequal(x, y); + } + +/// Comparison for inequality. +/// \param x first operand +/// \param y second operand +/// \retval true if operands not equal +/// \retval false else + template + typename enable::type operator!=(T x, U y) { + return functions::isnotequal(x, y); + } + +/// Comparison for less than. +/// \param x first operand +/// \param y second operand +/// \retval true if \a x less than \a y +/// \retval false else + template + typename enable::type operator<(T x, U y) { + return functions::isless(x, y); + } + +/// Comparison for greater than. +/// \param x first operand +/// \param y second operand +/// \retval true if \a x greater than \a y +/// \retval false else + template + typename enable::type operator>(T x, U y) { + return functions::isgreater(x, y); + } + +/// Comparison for less equal. +/// \param x first operand +/// \param y second operand +/// \retval true if \a x less equal \a y +/// \retval false else + template + typename enable::type operator<=(T x, U y) { + return functions::islessequal(x, y); + } + +/// Comparison for greater equal. +/// \param x first operand +/// \param y second operand +/// \retval true if \a x greater equal \a y +/// \retval false else + template + typename enable::type operator>=(T x, U y) { + return functions::isgreaterequal(x, y); + } + +/// \} +/// \name Arithmetic operators +/// \{ + +/// Add halfs. +/// \param x left operand +/// \param y right operand +/// \return sum of half expressions + template + typename enable::type operator+(T x, U y) { + return functions::plus(x, y); + } + +/// Subtract halfs. +/// \param x left operand +/// \param y right operand +/// \return difference of half expressions + template + typename enable::type operator-(T x, U y) { + return functions::minus(x, y); + } + +/// Multiply halfs. +/// \param x left operand +/// \param y right operand +/// \return product of half expressions + template + typename enable::type operator*(T x, U y) { + return functions::multiplies(x, y); + } + +/// Divide halfs. +/// \param x left operand +/// \param y right operand +/// \return quotient of half expressions + template + typename enable::type operator/(T x, U y) { + return functions::divides(x, y); + } + +/// Identity. +/// \param arg operand +/// \return uncahnged operand + template + HALF_CONSTEXPR typename enable::type operator+(T arg) { + return arg; + } + +/// Negation. +/// \param arg operand +/// \return negated operand + template + HALF_CONSTEXPR typename enable::type operator-(T arg) { + return unary_specialized::negate(arg); + } + +/// \} +/// \name Input and output +/// \{ + +/// Output operator. +/// \param out output stream to write into +/// \param arg half expression to write +/// \return reference to output stream + template + typename enable &, T>::type + operator<<(std::basic_ostream &out, T arg) { + return functions::write(out, arg); + } + +/// Input operator. +/// \param in input stream to read from +/// \param arg half to read into +/// \return reference to input stream + template + std::basic_istream &operator>>(std::basic_istream &in, half &arg) { + return functions::read(in, arg); + } + +/// \} +/// \name Basic mathematical operations +/// \{ + +/// Absolute value. +/// \param arg operand +/// \return absolute value of \a arg +// template typename enable::type abs(T arg) { return unary_specialized::fabs(arg); } + inline half abs(half arg) { + return unary_specialized::fabs(arg); + } + + inline expr abs(expr arg) { + return unary_specialized::fabs(arg); + } + +/// Absolute value. +/// \param arg operand +/// \return absolute value of \a arg +// template typename enable::type fabs(T arg) { return unary_specialized::fabs(arg); } + inline half fabs(half arg) { + return unary_specialized::fabs(arg); + } + + inline expr fabs(expr arg) { + return unary_specialized::fabs(arg); + } + +/// Remainder of division. +/// \param x first operand +/// \param y second operand +/// \return remainder of floating point division. +// template typename enable::type fmod(T x, U y) { return functions::fmod(x, y); } + inline expr fmod(half x, half y) { + return functions::fmod(x, y); + } + + inline expr fmod(half x, expr y) { + return functions::fmod(x, y); + } + + inline expr fmod(expr x, half y) { + return functions::fmod(x, y); + } + + inline expr fmod(expr x, expr y) { + return functions::fmod(x, y); + } + +/// Remainder of division. +/// \param x first operand +/// \param y second operand +/// \return remainder of floating point division. +// template typename enable::type remainder(T x, U y) { return +// functions::remainder(x, y); } + inline expr remainder(half x, half y) { + return functions::remainder(x, y); + } + + inline expr remainder(half x, expr y) { + return functions::remainder(x, y); + } + + inline expr remainder(expr x, half y) { + return functions::remainder(x, y); + } + + inline expr remainder(expr x, expr y) { + return functions::remainder(x, y); + } + +/// Remainder of division. +/// \param x first operand +/// \param y second operand +/// \param quo address to store some bits of quotient at +/// \return remainder of floating point division. +// template typename enable::type remquo(T x, U y, int *quo) { return +// functions::remquo(x, y, quo); } + inline expr remquo(half x, half y, int *quo) { + return functions::remquo(x, y, quo); + } + + inline expr remquo(half x, expr y, int *quo) { + return functions::remquo(x, y, quo); + } + + inline expr remquo(expr x, half y, int *quo) { + return functions::remquo(x, y, quo); + } + + inline expr remquo(expr x, expr y, int *quo) { + return functions::remquo(x, y, quo); + } + +/// Fused multiply add. +/// \param x first operand +/// \param y second operand +/// \param z third operand +/// \return ( \a x * \a y ) + \a z rounded as one operation. +// template typename enable::type fma(T x, U y, V z) { return +// functions::fma(x, y, z); } + inline expr fma(half x, half y, half z) { + return functions::fma(x, y, z); + } + + inline expr fma(half x, half y, expr z) { + return functions::fma(x, y, z); + } + + inline expr fma(half x, expr y, half z) { + return functions::fma(x, y, z); + } + + inline expr fma(half x, expr y, expr z) { + return functions::fma(x, y, z); + } + + inline expr fma(expr x, half y, half z) { + return functions::fma(x, y, z); + } + + inline expr fma(expr x, half y, expr z) { + return functions::fma(x, y, z); + } + + inline expr fma(expr x, expr y, half z) { + return functions::fma(x, y, z); + } + + inline expr fma(expr x, expr y, expr z) { + return functions::fma(x, y, z); + } + +/// Maximum of half expressions. +/// \param x first operand +/// \param y second operand +/// \return maximum of operands +// template typename result::type fmax(T x, U y) { return +// binary_specialized::fmax(x, y); } + inline half fmax(half x, half y) { + return binary_specialized::fmax(x, y); + } + + inline expr fmax(half x, expr y) { + return binary_specialized::fmax(x, y); + } + + inline expr fmax(expr x, half y) { + return binary_specialized::fmax(x, y); + } + + inline expr fmax(expr x, expr y) { + return binary_specialized::fmax(x, y); + } + +/// Minimum of half expressions. +/// \param x first operand +/// \param y second operand +/// \return minimum of operands +// template typename result::type fmin(T x, U y) { return +// binary_specialized::fmin(x, y); } + inline half fmin(half x, half y) { + return binary_specialized::fmin(x, y); + } + + inline expr fmin(half x, expr y) { + return binary_specialized::fmin(x, y); + } + + inline expr fmin(expr x, half y) { + return binary_specialized::fmin(x, y); + } + + inline expr fmin(expr x, expr y) { + return binary_specialized::fmin(x, y); + } + +/// Positive difference. +/// \param x first operand +/// \param y second operand +/// \return \a x - \a y or 0 if difference negative +// template typename enable::type fdim(T x, U y) { return functions::fdim(x, y); } + inline expr fdim(half x, half y) { + return functions::fdim(x, y); + } + + inline expr fdim(half x, expr y) { + return functions::fdim(x, y); + } + + inline expr fdim(expr x, half y) { + return functions::fdim(x, y); + } + + inline expr fdim(expr x, expr y) { + return functions::fdim(x, y); + } + +/// Get NaN value. +/// \return quiet NaN + inline half nanh(const char *) { + return functions::nanh(); + } + +/// \} +/// \name Exponential functions +/// \{ + +/// Exponential function. +/// \param arg function argument +/// \return e raised to \a arg +// template typename enable::type exp(T arg) { return functions::exp(arg); } + inline expr exp(half arg) { + return functions::exp(arg); + } + + inline expr exp(expr arg) { + return functions::exp(arg); + } + +/// Exponential minus one. +/// \param arg function argument +/// \return e raised to \a arg subtracted by 1 +// template typename enable::type expm1(T arg) { return functions::expm1(arg); } + inline expr expm1(half arg) { + return functions::expm1(arg); + } + + inline expr expm1(expr arg) { + return functions::expm1(arg); + } + +/// Binary exponential. +/// \param arg function argument +/// \return 2 raised to \a arg +// template typename enable::type exp2(T arg) { return functions::exp2(arg); } + inline expr exp2(half arg) { + return functions::exp2(arg); + } + + inline expr exp2(expr arg) { + return functions::exp2(arg); + } + +/// Natural logorithm. +/// \param arg function argument +/// \return logarithm of \a arg to base e +// template typename enable::type log(T arg) { return functions::log(arg); } + inline expr log(half arg) { + return functions::log(arg); + } + + inline expr log(expr arg) { + return functions::log(arg); + } + +/// Common logorithm. +/// \param arg function argument +/// \return logarithm of \a arg to base 10 +// template typename enable::type log10(T arg) { return functions::log10(arg); } + inline expr log10(half arg) { + return functions::log10(arg); + } + + inline expr log10(expr arg) { + return functions::log10(arg); + } + +/// Natural logorithm. +/// \param arg function argument +/// \return logarithm of \a arg plus 1 to base e +// template typename enable::type log1p(T arg) { return functions::log1p(arg); } + inline expr log1p(half arg) { + return functions::log1p(arg); + } + + inline expr log1p(expr arg) { + return functions::log1p(arg); + } + +/// Binary logorithm. +/// \param arg function argument +/// \return logarithm of \a arg to base 2 +// template typename enable::type log2(T arg) { return functions::log2(arg); } + inline expr log2(half arg) { + return functions::log2(arg); + } + + inline expr log2(expr arg) { + return functions::log2(arg); + } + +/// \} +/// \name Power functions +/// \{ + +/// Square root. +/// \param arg function argument +/// \return square root of \a arg +// template typename enable::type sqrt(T arg) { return functions::sqrt(arg); } + inline expr sqrt(half arg) { + return functions::sqrt(arg); + } + + inline expr sqrt(expr arg) { + return functions::sqrt(arg); + } + +/// Cubic root. +/// \param arg function argument +/// \return cubic root of \a arg +// template typename enable::type cbrt(T arg) { return functions::cbrt(arg); } + inline expr cbrt(half arg) { + return functions::cbrt(arg); + } + + inline expr cbrt(expr arg) { + return functions::cbrt(arg); + } + +/// Hypotenuse function. +/// \param x first argument +/// \param y second argument +/// \return square root of sum of squares without internal over- or underflows +// template typename enable::type hypot(T x, U y) { return functions::hypot(x, y); +//} + inline expr hypot(half x, half y) { + return functions::hypot(x, y); + } + + inline expr hypot(half x, expr y) { + return functions::hypot(x, y); + } + + inline expr hypot(expr x, half y) { + return functions::hypot(x, y); + } + + inline expr hypot(expr x, expr y) { + return functions::hypot(x, y); + } + +/// Power function. +/// \param base first argument +/// \param exp second argument +/// \return \a base raised to \a exp +// template typename enable::type pow(T base, U exp) { return functions::pow(base, +// exp); } + inline expr pow(half base, half exp) { + return functions::pow(base, exp); + } + + inline expr pow(half base, expr exp) { + return functions::pow(base, exp); + } + + inline expr pow(expr base, half exp) { + return functions::pow(base, exp); + } + + inline expr pow(expr base, expr exp) { + return functions::pow(base, exp); + } + +/// \} +/// \name Trigonometric functions +/// \{ + +/// Sine function. +/// \param arg function argument +/// \return sine value of \a arg +// template typename enable::type sin(T arg) { return functions::sin(arg); } + inline expr sin(half arg) { + return functions::sin(arg); + } + + inline expr sin(expr arg) { + return functions::sin(arg); + } + +/// Cosine function. +/// \param arg function argument +/// \return cosine value of \a arg +// template typename enable::type cos(T arg) { return functions::cos(arg); } + inline expr cos(half arg) { + return functions::cos(arg); + } + + inline expr cos(expr arg) { + return functions::cos(arg); + } + +/// Tangent function. +/// \param arg function argument +/// \return tangent value of \a arg +// template typename enable::type tan(T arg) { return functions::tan(arg); } + inline expr tan(half arg) { + return functions::tan(arg); + } + + inline expr tan(expr arg) { + return functions::tan(arg); + } + +/// Arc sine. +/// \param arg function argument +/// \return arc sine value of \a arg +// template typename enable::type asin(T arg) { return functions::asin(arg); } + inline expr asin(half arg) { + return functions::asin(arg); + } + + inline expr asin(expr arg) { + return functions::asin(arg); + } + +/// Arc cosine function. +/// \param arg function argument +/// \return arc cosine value of \a arg +// template typename enable::type acos(T arg) { return functions::acos(arg); } + inline expr acos(half arg) { + return functions::acos(arg); + } + + inline expr acos(expr arg) { + return functions::acos(arg); + } + +/// Arc tangent function. +/// \param arg function argument +/// \return arc tangent value of \a arg +// template typename enable::type atan(T arg) { return functions::atan(arg); } + inline expr atan(half arg) { + return functions::atan(arg); + } + + inline expr atan(expr arg) { + return functions::atan(arg); + } + +/// Arc tangent function. +/// \param x first argument +/// \param y second argument +/// \return arc tangent value +// template typename enable::type atan2(T x, U y) { return functions::atan2(x, y); +//} + inline expr atan2(half x, half y) { + return functions::atan2(x, y); + } + + inline expr atan2(half x, expr y) { + return functions::atan2(x, y); + } + + inline expr atan2(expr x, half y) { + return functions::atan2(x, y); + } + + inline expr atan2(expr x, expr y) { + return functions::atan2(x, y); + } + +/// \} +/// \name Hyperbolic functions +/// \{ + +/// Hyperbolic sine. +/// \param arg function argument +/// \return hyperbolic sine value of \a arg +// template typename enable::type sinh(T arg) { return functions::sinh(arg); } + inline expr sinh(half arg) { + return functions::sinh(arg); + } + + inline expr sinh(expr arg) { + return functions::sinh(arg); + } + +/// Hyperbolic cosine. +/// \param arg function argument +/// \return hyperbolic cosine value of \a arg +// template typename enable::type cosh(T arg) { return functions::cosh(arg); } + inline expr cosh(half arg) { + return functions::cosh(arg); + } + + inline expr cosh(expr arg) { + return functions::cosh(arg); + } + +/// Hyperbolic tangent. +/// \param arg function argument +/// \return hyperbolic tangent value of \a arg +// template typename enable::type tanh(T arg) { return functions::tanh(arg); } + inline expr tanh(half arg) { + return functions::tanh(arg); + } + + inline expr tanh(expr arg) { + return functions::tanh(arg); + } + +/// Hyperbolic area sine. +/// \param arg function argument +/// \return area sine value of \a arg +// template typename enable::type asinh(T arg) { return functions::asinh(arg); } + inline expr asinh(half arg) { + return functions::asinh(arg); + } + + inline expr asinh(expr arg) { + return functions::asinh(arg); + } + +/// Hyperbolic area cosine. +/// \param arg function argument +/// \return area cosine value of \a arg +// template typename enable::type acosh(T arg) { return functions::acosh(arg); } + inline expr acosh(half arg) { + return functions::acosh(arg); + } + + inline expr acosh(expr arg) { + return functions::acosh(arg); + } + +/// Hyperbolic area tangent. +/// \param arg function argument +/// \return area tangent value of \a arg +// template typename enable::type atanh(T arg) { return functions::atanh(arg); } + inline expr atanh(half arg) { + return functions::atanh(arg); + } + + inline expr atanh(expr arg) { + return functions::atanh(arg); + } + +/// \} +/// \name Error and gamma functions +/// \{ + +/// Error function. +/// \param arg function argument +/// \return error function value of \a arg +// template typename enable::type erf(T arg) { return functions::erf(arg); } + inline expr erf(half arg) { + return functions::erf(arg); + } + + inline expr erf(expr arg) { + return functions::erf(arg); + } + +/// Complementary error function. +/// \param arg function argument +/// \return 1 minus error function value of \a arg +// template typename enable::type erfc(T arg) { return functions::erfc(arg); } + inline expr erfc(half arg) { + return functions::erfc(arg); + } + + inline expr erfc(expr arg) { + return functions::erfc(arg); + } + +/// Natural logarithm of gamma function. +/// \param arg function argument +/// \return natural logarith of gamma function for \a arg +// template typename enable::type lgamma(T arg) { return functions::lgamma(arg); } + inline expr lgamma(half arg) { + return functions::lgamma(arg); + } + + inline expr lgamma(expr arg) { + return functions::lgamma(arg); + } + +/// Gamma function. +/// \param arg function argument +/// \return gamma function value of \a arg +// template typename enable::type tgamma(T arg) { return functions::tgamma(arg); } + inline expr tgamma(half arg) { + return functions::tgamma(arg); + } + + inline expr tgamma(expr arg) { + return functions::tgamma(arg); + } + +/// \} +/// \name Rounding +/// \{ + +/// Nearest integer not less than half value. +/// \param arg half to round +/// \return nearest integer not less than \a arg +// template typename enable::type ceil(T arg) { return functions::ceil(arg); } + inline half ceil(half arg) { + return functions::ceil(arg); + } + + inline half ceil(expr arg) { + return functions::ceil(arg); + } + +/// Nearest integer not greater than half value. +/// \param arg half to round +/// \return nearest integer not greater than \a arg +// template typename enable::type floor(T arg) { return functions::floor(arg); } + inline half floor(half arg) { + return functions::floor(arg); + } + + inline half floor(expr arg) { + return functions::floor(arg); + } + +/// Nearest integer not greater in magnitude than half value. +/// \param arg half to round +/// \return nearest integer not greater in magnitude than \a arg +// template typename enable::type trunc(T arg) { return functions::trunc(arg); } + inline half trunc(half arg) { + return functions::trunc(arg); + } + + inline half trunc(expr arg) { + return functions::trunc(arg); + } + +/// Nearest integer. +/// \param arg half to round +/// \return nearest integer, rounded away from zero in half-way cases +// template typename enable::type round(T arg) { return functions::round(arg); } + inline half round(half arg) { + return functions::round(arg); + } + + inline half round(expr arg) { + return functions::round(arg); + } + +/// Nearest integer. +/// \param arg half to round +/// \return nearest integer, rounded away from zero in half-way cases +// template typename enable::type lround(T arg) { return functions::lround(arg); } + inline long lround(half arg) { + return functions::lround(arg); + } + + inline long lround(expr arg) { + return functions::lround(arg); + } + +/// Nearest integer using half's internal rounding mode. +/// \param arg half expression to round +/// \return nearest integer using default rounding mode +// template typename enable::type nearbyint(T arg) { return functions::nearbyint(arg); } + inline half nearbyint(half arg) { + return functions::rint(arg); + } + + inline half nearbyint(expr arg) { + return functions::rint(arg); + } + +/// Nearest integer using half's internal rounding mode. +/// \param arg half expression to round +/// \return nearest integer using default rounding mode +// template typename enable::type rint(T arg) { return functions::rint(arg); } + inline half rint(half arg) { + return functions::rint(arg); + } + + inline half rint(expr arg) { + return functions::rint(arg); + } + +/// Nearest integer using half's internal rounding mode. +/// \param arg half expression to round +/// \return nearest integer using default rounding mode +// template typename enable::type lrint(T arg) { return functions::lrint(arg); } + inline long lrint(half arg) { + return functions::lrint(arg); + } + + inline long lrint(expr arg) { + return functions::lrint(arg); + } + +#if HALF_ENABLE_CPP11_LONG_LONG + +/// Nearest integer. +/// \param arg half to round +/// \return nearest integer, rounded away from zero in half-way cases +// template typename enable::type llround(T arg) { return functions::llround(arg); } + inline long long llround(half arg) { + return functions::llround(arg); + } + + inline long long llround(expr arg) { + return functions::llround(arg); + } + +/// Nearest integer using half's internal rounding mode. +/// \param arg half expression to round +/// \return nearest integer using default rounding mode +// template typename enable::type llrint(T arg) { return functions::llrint(arg); } + inline long long llrint(half arg) { + return functions::llrint(arg); + } + + inline long long llrint(expr arg) { + return functions::llrint(arg); + } + +#endif + +/// \} +/// \name Floating point manipulation +/// \{ + +/// Decompress floating point number. +/// \param arg number to decompress +/// \param exp address to store exponent at +/// \return significant in range [0.5, 1) +// template typename enable::type frexp(T arg, int *exp) { return functions::frexp(arg, exp); } + inline half frexp(half arg, int *exp) { + return functions::frexp(arg, exp); + } + + inline half frexp(expr arg, int *exp) { + return functions::frexp(arg, exp); + } + +/// Multiply by power of two. +/// \param arg number to modify +/// \param exp power of two to multiply with +/// \return \a arg multplied by 2 raised to \a exp +// template typename enable::type ldexp(T arg, int exp) { return functions::scalbln(arg, exp); +//} + inline half ldexp(half arg, int exp) { + return functions::scalbln(arg, exp); + } + + inline half ldexp(expr arg, int exp) { + return functions::scalbln(arg, exp); + } + +/// Extract integer and fractional parts. +/// \param arg number to decompress +/// \param iptr address to store integer part at +/// \return fractional part +// template typename enable::type modf(T arg, half *iptr) { return functions::modf(arg, iptr); +//} + inline half modf(half arg, half *iptr) { + return functions::modf(arg, iptr); + } + + inline half modf(expr arg, half *iptr) { + return functions::modf(arg, iptr); + } + +/// Multiply by power of two. +/// \param arg number to modify +/// \param exp power of two to multiply with +/// \return \a arg multplied by 2 raised to \a exp +// template typename enable::type scalbn(T arg, int exp) { return functions::scalbln(arg, exp); +//} + inline half scalbn(half arg, int exp) { + return functions::scalbln(arg, exp); + } + + inline half scalbn(expr arg, int exp) { + return functions::scalbln(arg, exp); + } + +/// Multiply by power of two. +/// \param arg number to modify +/// \param exp power of two to multiply with +/// \return \a arg multplied by 2 raised to \a exp +// template typename enable::type scalbln(T arg, long exp) { return functions::scalbln(arg, +// exp); +//} + inline half scalbln(half arg, long exp) { + return functions::scalbln(arg, exp); + } + + inline half scalbln(expr arg, long exp) { + return functions::scalbln(arg, exp); + } + +/// Extract exponent. +/// \param arg number to query +/// \return floating point exponent +/// \retval FP_ILOGB0 for zero +/// \retval FP_ILOGBNAN for NaN +/// \retval MAX_INT for infinity +// template typename enable::type ilogb(T arg) { return functions::ilogb(arg); } + inline int ilogb(half arg) { + return functions::ilogb(arg); + } + + inline int ilogb(expr arg) { + return functions::ilogb(arg); + } + +/// Extract exponent. +/// \param arg number to query +/// \return floating point exponent +// template typename enable::type logb(T arg) { return functions::logb(arg); } + inline half logb(half arg) { + return functions::logb(arg); + } + + inline half logb(expr arg) { + return functions::logb(arg); + } + +/// Next representable value. +/// \param from value to compute next representable value for +/// \param to direction towards which to compute next value +/// \return next representable value after \a from in direction towards \a to +// template typename enable::type nextafter(T from, U to) { return +// functions::nextafter(from, to); } + inline half nextafter(half from, half to) { + return functions::nextafter(from, to); + } + + inline half nextafter(half from, expr to) { + return functions::nextafter(from, to); + } + + inline half nextafter(expr from, half to) { + return functions::nextafter(from, to); + } + + inline half nextafter(expr from, expr to) { + return functions::nextafter(from, to); + } + +/// Next representable value. +/// \param from value to compute next representable value for +/// \param to direction towards which to compute next value +/// \return next representable value after \a from in direction towards \a to +// template typename enable::type nexttoward(T from, long double to) { return +// functions::nexttoward(from, to); } + inline half nexttoward(half from, long double to) { + return functions::nexttoward(from, to); + } + + inline half nexttoward(expr from, long double to) { + return functions::nexttoward(from, to); + } + +/// Take sign. +/// \param x value to change sign for +/// \param y value to take sign from +/// \return value equal to \a x in magnitude and to \a y in sign +// template typename enable::type copysign(T x, U y) { return +// functions::copysign(x, y); } + inline half copysign(half x, half y) { + return functions::copysign(x, y); + } + + inline half copysign(half x, expr y) { + return functions::copysign(x, y); + } + + inline half copysign(expr x, half y) { + return functions::copysign(x, y); + } + + inline half copysign(expr x, expr y) { + return functions::copysign(x, y); + } + +/// \} +/// \name Floating point classification +/// \{ + +/// Classify floating point value. +/// \param arg number to classify +/// \retval FP_ZERO for positive and negative zero +/// \retval FP_SUBNORMAL for subnormal numbers +/// \retval FP_INFINITY for positive and negative infinity +/// \retval FP_NAN for NaNs +/// \retval FP_NORMAL for all other (normal) values +// template typename enable::type fpclassify(T arg) { return functions::fpclassify(arg); } + inline int fpclassify(half arg) { + return functions::fpclassify(arg); + } + + inline int fpclassify(expr arg) { + return functions::fpclassify(arg); + } + +/// Check if finite number. +/// \param arg number to check +/// \retval true if neither infinity nor NaN +/// \retval false else +// template typename enable::type isfinite(T arg) { return functions::isfinite(arg); } + inline bool isfinite(half arg) { + return functions::isfinite(arg); + } + + inline bool isfinite(expr arg) { + return functions::isfinite(arg); + } + +/// Check for infinity. +/// \param arg number to check +/// \retval true for positive or negative infinity +/// \retval false else +// template typename enable::type isinf(T arg) { return functions::isinf(arg); } + inline bool isinf(half arg) { + return functions::isinf(arg); + } + + inline bool isinf(expr arg) { + return functions::isinf(arg); + } + +/// Check for NaN. +/// \param arg number to check +/// \retval true for NaNs +/// \retval false else +// template typename enable::type isnan(T arg) { return functions::isnan(arg); } + inline bool isnan(half arg) { + return functions::isnan(arg); + } + + inline bool isnan(expr arg) { + return functions::isnan(arg); + } + +/// Check if normal number. +/// \param arg number to check +/// \retval true if normal number +/// \retval false if either subnormal, zero, infinity or NaN +// template typename enable::type isnormal(T arg) { return functions::isnormal(arg); } + inline bool isnormal(half arg) { + return functions::isnormal(arg); + } + + inline bool isnormal(expr arg) { + return functions::isnormal(arg); + } + +/// Check sign. +/// \param arg number to check +/// \retval true for negative number +/// \retval false for positive number +// template typename enable::type signbit(T arg) { return functions::signbit(arg); } + inline bool signbit(half arg) { + return functions::signbit(arg); + } + + inline bool signbit(expr arg) { + return functions::signbit(arg); + } + +/// \} +/// \name Comparison +/// \{ + +/// Comparison for greater than. +/// \param x first operand +/// \param y second operand +/// \retval true if \a x greater than \a y +/// \retval false else +// template typename enable::type isgreater(T x, U y) { return +// functions::isgreater(x, y); } + inline bool isgreater(half x, half y) { + return functions::isgreater(x, y); + } + + inline bool isgreater(half x, expr y) { + return functions::isgreater(x, y); + } + + inline bool isgreater(expr x, half y) { + return functions::isgreater(x, y); + } + + inline bool isgreater(expr x, expr y) { + return functions::isgreater(x, y); + } + +/// Comparison for greater equal. +/// \param x first operand +/// \param y second operand +/// \retval true if \a x greater equal \a y +/// \retval false else +// template typename enable::type isgreaterequal(T x, U y) { return +// functions::isgreaterequal(x, y); } + inline bool isgreaterequal(half x, half y) { + return functions::isgreaterequal(x, y); + } + + inline bool isgreaterequal(half x, expr y) { + return functions::isgreaterequal(x, y); + } + + inline bool isgreaterequal(expr x, half y) { + return functions::isgreaterequal(x, y); + } + + inline bool isgreaterequal(expr x, expr y) { + return functions::isgreaterequal(x, y); + } + +/// Comparison for less than. +/// \param x first operand +/// \param y second operand +/// \retval true if \a x less than \a y +/// \retval false else +// template typename enable::type isless(T x, U y) { return functions::isless(x, +// y); +//} + inline bool isless(half x, half y) { + return functions::isless(x, y); + } + + inline bool isless(half x, expr y) { + return functions::isless(x, y); + } + + inline bool isless(expr x, half y) { + return functions::isless(x, y); + } + + inline bool isless(expr x, expr y) { + return functions::isless(x, y); + } + +/// Comparison for less equal. +/// \param x first operand +/// \param y second operand +/// \retval true if \a x less equal \a y +/// \retval false else +// template typename enable::type islessequal(T x, U y) { return +// functions::islessequal(x, y); } + inline bool islessequal(half x, half y) { + return functions::islessequal(x, y); + } + + inline bool islessequal(half x, expr y) { + return functions::islessequal(x, y); + } + + inline bool islessequal(expr x, half y) { + return functions::islessequal(x, y); + } + + inline bool islessequal(expr x, expr y) { + return functions::islessequal(x, y); + } + +/// Comarison for less or greater. +/// \param x first operand +/// \param y second operand +/// \retval true if either less or greater +/// \retval false else +// template typename enable::type islessgreater(T x, U y) { return +// functions::islessgreater(x, y); } + inline bool islessgreater(half x, half y) { + return functions::islessgreater(x, y); + } + + inline bool islessgreater(half x, expr y) { + return functions::islessgreater(x, y); + } + + inline bool islessgreater(expr x, half y) { + return functions::islessgreater(x, y); + } + + inline bool islessgreater(expr x, expr y) { + return functions::islessgreater(x, y); + } + +/// Check if unordered. +/// \param x first operand +/// \param y second operand +/// \retval true if unordered (one or two NaN operands) +/// \retval false else +// template typename enable::type isunordered(T x, U y) { return +// functions::isunordered(x, y); } + inline bool isunordered(half x, half y) { + return functions::isunordered(x, y); + } + + inline bool isunordered(half x, expr y) { + return functions::isunordered(x, y); + } + + inline bool isunordered(expr x, half y) { + return functions::isunordered(x, y); + } + + inline bool isunordered(expr x, expr y) { + return functions::isunordered(x, y); + } + +/// \name Casting +/// \{ + +/// Cast to or from half-precision floating point number. +/// This casts between [half](\ref half_float::half) and any built-in arithmetic type. The values are converted +/// directly using the given rounding mode, without any roundtrip over `float` that a `static_cast` would otherwise do. +/// It uses the default rounding mode. +/// +/// Using this cast with neither of the two types being a [half](\ref half_float::half) or with any of the two types +/// not being a built-in arithmetic type (apart from [half](\ref half_float::half), of course) results in a compiler +/// error and casting between [half](\ref half_float::half)s is just a no-op. +/// \tparam T destination type (half or built-in arithmetic type) +/// \tparam U source type (half or built-in arithmetic type) +/// \param arg value to cast +/// \return \a arg converted to destination type + template + T half_cast(U arg) { + return half_caster::cast(arg); + } + +/// Cast to or from half-precision floating point number. +/// This casts between [half](\ref half_float::half) and any built-in arithmetic type. The values are converted +/// directly using the given rounding mode, without any roundtrip over `float` that a `static_cast` would otherwise do. +/// +/// Using this cast with neither of the two types being a [half](\ref half_float::half) or with any of the two types +/// not being a built-in arithmetic type (apart from [half](\ref half_float::half), of course) results in a compiler +/// error and casting between [half](\ref half_float::half)s is just a no-op. +/// \tparam T destination type (half or built-in arithmetic type) +/// \tparam R rounding mode to use. +/// \tparam U source type (half or built-in arithmetic type) +/// \param arg value to cast +/// \return \a arg converted to destination type + template + T half_cast(U arg) { + return half_caster::cast(arg); + } +/// \} + } // namespace detail + + using detail::operator==; + using detail::operator!=; + using detail::operator<; + using detail::operator>; + using detail::operator<=; + using detail::operator>=; + using detail::operator+; + using detail::operator-; + using detail::operator*; + using detail::operator/; + using detail::operator<<; + using detail::operator>>; + + using detail::abs; + using detail::acos; + using detail::acosh; + using detail::asin; + using detail::asinh; + using detail::atan; + using detail::atan2; + using detail::atanh; + using detail::cbrt; + using detail::ceil; + using detail::cos; + using detail::cosh; + using detail::erf; + using detail::erfc; + using detail::exp; + using detail::exp2; + using detail::expm1; + using detail::fabs; + using detail::fdim; + using detail::floor; + using detail::fma; + using detail::fmax; + using detail::fmin; + using detail::fmod; + using detail::hypot; + using detail::lgamma; + using detail::log; + using detail::log10; + using detail::log1p; + using detail::log2; + using detail::lrint; + using detail::lround; + using detail::nanh; + using detail::nearbyint; + using detail::pow; + using detail::remainder; + using detail::remquo; + using detail::rint; + using detail::round; + using detail::sin; + using detail::sinh; + using detail::sqrt; + using detail::tan; + using detail::tanh; + using detail::tgamma; + using detail::trunc; +#if HALF_ENABLE_CPP11_LONG_LONG + using detail::llrint; + using detail::llround; +#endif + using detail::copysign; + using detail::fpclassify; + using detail::frexp; + using detail::ilogb; + using detail::isfinite; + using detail::isgreater; + using detail::isgreaterequal; + using detail::isinf; + using detail::isless; + using detail::islessequal; + using detail::islessgreater; + using detail::isnan; + using detail::isnormal; + using detail::isunordered; + using detail::ldexp; + using detail::logb; + using detail::modf; + using detail::nextafter; + using detail::nexttoward; + using detail::scalbln; + using detail::scalbn; + using detail::signbit; + + using detail::half_cast; +} // namespace half_float + +/// Extensions to the C++ standard library. +namespace std { +/// Numeric limits for half-precision floats. +/// Because of the underlying single-precision implementation of many operations, it inherits some properties from +/// `std::numeric_limits`. + template<> + class numeric_limits : public numeric_limits { + public: + /// Supports signed values. + static HALF_CONSTEXPR_CONST bool is_signed = true; + + /// Is not exact. + static HALF_CONSTEXPR_CONST bool is_exact = false; + + /// Doesn't provide modulo arithmetic. + static HALF_CONSTEXPR_CONST bool is_modulo = false; + + /// IEEE conformant. + static HALF_CONSTEXPR_CONST bool is_iec559 = true; + + /// Supports infinity. + static HALF_CONSTEXPR_CONST bool has_infinity = true; + + /// Supports quiet NaNs. + static HALF_CONSTEXPR_CONST bool has_quiet_NaN = true; + + /// Supports subnormal values. + static HALF_CONSTEXPR_CONST float_denorm_style has_denorm = denorm_present; + + /// Rounding mode. + /// Due to the mix of internal single-precision computations (using the rounding mode of the underlying + /// single-precision implementation) with the rounding mode of the single-to-half conversions, the actual rounding + /// mode might be `std::round_indeterminate` if the default half-precision rounding mode doesn't match the + /// single-precision rounding mode. + static HALF_CONSTEXPR_CONST float_round_style round_style + = (std::numeric_limits::round_style == half_float::half::round_style) + ? half_float::half::round_style + : round_indeterminate; + + /// Significant digits. + static HALF_CONSTEXPR_CONST int digits = 11; + + /// Significant decimal digits. + static HALF_CONSTEXPR_CONST int digits10 = 3; + + /// Required decimal digits to represent all possible values. + static HALF_CONSTEXPR_CONST int max_digits10 = 5; + + /// Number base. + static HALF_CONSTEXPR_CONST int radix = 2; + + /// One more than smallest exponent. + static HALF_CONSTEXPR_CONST int min_exponent = -13; + + /// Smallest normalized representable power of 10. + static HALF_CONSTEXPR_CONST int min_exponent10 = -4; + + /// One more than largest exponent + static HALF_CONSTEXPR_CONST int max_exponent = 16; + + /// Largest finitely representable power of 10. + static HALF_CONSTEXPR_CONST int max_exponent10 = 4; + + /// Smallest positive normal value. + static HALF_CONSTEXPR half_float::half min() HALF_NOTHROW { + return half_float::half(half_float::detail::binary, 0x0400); + } + + /// Smallest finite value. + static HALF_CONSTEXPR half_float::half lowest() HALF_NOTHROW { + return half_float::half(half_float::detail::binary, 0xFBFF); + } + + /// Largest finite value. + static HALF_CONSTEXPR half_float::half max() HALF_NOTHROW { + return half_float::half(half_float::detail::binary, 0x7BFF); + } + + /// Difference between one and next representable value. + static HALF_CONSTEXPR half_float::half epsilon() HALF_NOTHROW { + return half_float::half(half_float::detail::binary, 0x1400); + } + + /// Maximum rounding error. + static HALF_CONSTEXPR half_float::half round_error() HALF_NOTHROW { + return half_float::half(half_float::detail::binary, + (round_style == std::round_to_nearest) ? 0x3800 : 0x3C00); + } + + /// Positive infinity. + static HALF_CONSTEXPR half_float::half infinity() HALF_NOTHROW { + return half_float::half(half_float::detail::binary, 0x7C00); + } + + /// Quiet NaN. + static HALF_CONSTEXPR half_float::half quiet_NaN() HALF_NOTHROW { + return half_float::half(half_float::detail::binary, 0x7FFF); + } + + /// Signalling NaN. + static HALF_CONSTEXPR half_float::half signaling_NaN() HALF_NOTHROW { + return half_float::half(half_float::detail::binary, 0x7DFF); + } + + /// Smallest positive subnormal value. + static HALF_CONSTEXPR half_float::half denorm_min() HALF_NOTHROW { + return half_float::half(half_float::detail::binary, 0x0001); + } + }; + +#if HALF_ENABLE_CPP11_HASH + +/// Hash function for half-precision floats. +/// This is only defined if C++11 `std::hash` is supported and enabled. + template<> + struct hash //: unary_function + { + /// Type of function argument. + typedef half_float::half argument_type; + + /// Function return type. + typedef size_t result_type; + + /// Compute hash function. + /// \param arg half to hash + /// \return hash value + result_type operator()(argument_type arg) const { + return hash()(static_cast(arg.data_) & -(arg.data_ != 0x8000)); + } + }; + +#endif +} // namespace std + +#undef HALF_CONSTEXPR +#undef HALF_CONSTEXPR_CONST +#undef HALF_NOEXCEPT +#undef HALF_NOTHROW +#ifdef HALF_POP_WARNINGS + #pragma warning(pop) +#undef HALF_POP_WARNINGS +#endif + +#endif diff --git a/tensorrt_utils/include/tensorrt_utils/logger.h b/tensorrt_utils/include/tensorrt_utils/logger.h new file mode 100644 index 00000000..7a6c8a10 --- /dev/null +++ b/tensorrt_utils/include/tensorrt_utils/logger.h @@ -0,0 +1,36 @@ +/* + * Copyright (c) 2021, NVIDIA CORPORATION. All rights reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef LOGGER_H +#define LOGGER_H + +#include "logging.h" + +class TensorRTErrorRecorder; + +extern TensorRTErrorRecorder gRecorder; +namespace tensorrt_log { + extern Logger gLogger; + extern LogStreamConsumer gLogVerbose; + extern LogStreamConsumer gLogInfo; + extern LogStreamConsumer gLogWarning; + extern LogStreamConsumer gLogError; + extern LogStreamConsumer gLogFatal; + + void setReportableSeverity(Logger::Severity severity); +} // namespace tensorrt_log + +#endif // LOGGER_H diff --git a/tensorrt_utils/include/tensorrt_utils/logging.h b/tensorrt_utils/include/tensorrt_utils/logging.h new file mode 100644 index 00000000..815abfd7 --- /dev/null +++ b/tensorrt_utils/include/tensorrt_utils/logging.h @@ -0,0 +1,477 @@ +/* + * Copyright (c) 2021, NVIDIA CORPORATION. All rights reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef TENSORRT_LOGGING_H +#define TENSORRT_LOGGING_H + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace tensorrt_log { + + using Severity = nvinfer1::ILogger::Severity; + + class LogStreamConsumerBuffer : public std::stringbuf { + public: + LogStreamConsumerBuffer(std::ostream &stream, const std::string &prefix, bool shouldLog) + : mOutput(stream), mPrefix(prefix), mShouldLog(shouldLog) { + } + + LogStreamConsumerBuffer(LogStreamConsumerBuffer &&other) + : mOutput(other.mOutput), mPrefix(other.mPrefix), mShouldLog(other.mShouldLog) { + } + + ~LogStreamConsumerBuffer() { + // std::streambuf::pbase() gives a pointer to the beginning of the buffered part of the output sequence + // std::streambuf::pptr() gives a pointer to the current position of the output sequence + // if the pointer to the beginning is not equal to the pointer to the current position, + // call putOutput() to log the output to the stream + if (pbase() != pptr()) { + putOutput(); + } + } + + // synchronizes the stream buffer and returns 0 on success + // synchronizing the stream buffer consists of inserting the buffer contents into the stream, + // resetting the buffer and flushing the stream + virtual int sync() { + putOutput(); + return 0; + } + + void putOutput() { + if (mShouldLog) { + // prepend timestamp + std::time_t timestamp = std::time(nullptr); + tm *tm_local = std::localtime(×tamp); + std::cout << "["; + std::cout << std::setw(2) << std::setfill('0') << 1 + tm_local->tm_mon << "/"; + std::cout << std::setw(2) << std::setfill('0') << tm_local->tm_mday << "/"; + std::cout << std::setw(4) << std::setfill('0') << 1900 + tm_local->tm_year << "-"; + std::cout << std::setw(2) << std::setfill('0') << tm_local->tm_hour << ":"; + std::cout << std::setw(2) << std::setfill('0') << tm_local->tm_min << ":"; + std::cout << std::setw(2) << std::setfill('0') << tm_local->tm_sec << "] "; + // std::stringbuf::str() gets the string contents of the buffer + // insert the buffer contents pre-appended by the appropriate prefix into the stream + mOutput << mPrefix << str(); + } + // set the buffer to empty + str(""); + // flush the stream + mOutput.flush(); + } + + void setShouldLog(bool shouldLog) { + mShouldLog = shouldLog; + } + + private: + std::ostream &mOutput; + std::string mPrefix; + bool mShouldLog; + }; + +//! +//! \class LogStreamConsumerBase +//! \brief Convenience object used to initialize LogStreamConsumerBuffer before std::ostream in LogStreamConsumer +//! + class LogStreamConsumerBase { + public: + LogStreamConsumerBase(std::ostream &stream, const std::string &prefix, bool shouldLog) + : mBuffer(stream, prefix, shouldLog) { + } + + protected: + LogStreamConsumerBuffer mBuffer; + }; + +//! +//! \class LogStreamConsumer +//! \brief Convenience object used to facilitate use of C++ stream syntax when logging messages. +//! Order of base classes is LogStreamConsumerBase and then std::ostream. +//! This is because the LogStreamConsumerBase class is used to initialize the LogStreamConsumerBuffer member field +//! in LogStreamConsumer and then the address of the buffer is passed to std::ostream. +//! This is necessary to prevent the address of an uninitialized buffer from being passed to std::ostream. +//! Please do not change the order of the parent classes. +//! + class LogStreamConsumer : protected LogStreamConsumerBase, public std::ostream { + public: + //! \brief Creates a LogStreamConsumer which logs messages with level severity. + //! Reportable severity determines if the messages are severe enough to be logged. + LogStreamConsumer(Severity reportableSeverity, Severity severity) + : LogStreamConsumerBase(severityOstream(severity), severityPrefix(severity), + severity <= reportableSeverity), + std::ostream(&mBuffer) // links the stream buffer with the stream + , mShouldLog(severity <= reportableSeverity), mSeverity(severity) { + } + + LogStreamConsumer(LogStreamConsumer &&other) + : LogStreamConsumerBase(severityOstream(other.mSeverity), severityPrefix(other.mSeverity), + other.mShouldLog), + std::ostream(&mBuffer) // links the stream buffer with the stream + , mShouldLog(other.mShouldLog), mSeverity(other.mSeverity) { + } + + void setReportableSeverity(Severity reportableSeverity) { + mShouldLog = mSeverity <= reportableSeverity; + mBuffer.setShouldLog(mShouldLog); + } + + private: + static std::ostream &severityOstream(Severity severity) { + return severity >= Severity::kINFO ? std::cout : std::cerr; + } + + static std::string severityPrefix(Severity severity) { + switch (severity) { + case Severity::kINTERNAL_ERROR: + return "[F] "; + case Severity::kERROR: + return "[E] "; + case Severity::kWARNING: + return "[W] "; + case Severity::kINFO: + return "[I] "; + case Severity::kVERBOSE: + return "[V] "; + default: + assert(0); + return ""; + } + } + + bool mShouldLog; + Severity mSeverity; + }; + +//! \class Logger +//! +//! \brief Class which manages logging of TensorRT tools and samples +//! +//! \details This class provides a common interface for TensorRT tools and samples to log information to the console, +//! and supports logging two types of messages: +//! +//! - Debugging messages with an associated severity (info, warning, error, or internal error/fatal) +//! - Test pass/fail messages +//! +//! The advantage of having all samples use this class for logging as opposed to emitting directly to stdout/stderr is +//! that the logic for controlling the verbosity and formatting of sample output is centralized in one location. +//! +//! In the future, this class could be extended to support dumping test results to a file in some standard format +//! (for example, JUnit XML), and providing additional metadata (e.g. timing the duration of a test run). +//! +//! TODO: For backwards compatibility with existing samples, this class inherits directly from the nvinfer1::ILogger +//! interface, which is problematic since there isn't a clean separation between messages coming from the TensorRT +//! library and messages coming from the sample. +//! +//! In the future (once all samples are updated to use Logger::getTRTLogger() to access the ILogger) we can refactor the +//! class to eliminate the inheritance and instead make the nvinfer1::ILogger implementation a member of the Logger +//! object. + + class Logger : public nvinfer1::ILogger { + public: + Logger(Severity severity = Severity::kWARNING) + : mReportableSeverity(severity) { + } + + //! + //! \enum TestResult + //! \brief Represents the state of a given test + //! + enum class TestResult { + kRUNNING, //!< The test is running + kPASSED, //!< The test passed + kFAILED, //!< The test failed + kWAIVED //!< The test was waived + }; + + //! + //! \brief Forward-compatible method for retrieving the nvinfer::ILogger associated with this Logger + //! \return The nvinfer1::ILogger associated with this Logger + //! + //! TODO Once all samples are updated to use this method to register the logger with TensorRT, + //! we can eliminate the inheritance of Logger from ILogger + //! + nvinfer1::ILogger &getTRTLogger() noexcept { + return *this; + } + + //! + //! \brief Implementation of the nvinfer1::ILogger::log() virtual method + //! + //! Note samples should not be calling this function directly; it will eventually go away once we eliminate the + //! inheritance from nvinfer1::ILogger + //! + void log(Severity severity, const char *msg) noexcept override { + LogStreamConsumer(mReportableSeverity, severity) << "[TRT] " << std::string(msg) << std::endl; + } + + //! + //! \brief Method for controlling the verbosity of logging output + //! + //! \param severity The logger will only emit messages that have severity of this level or higher. + //! + void setReportableSeverity(Severity severity) { + mReportableSeverity = severity; + } + + //! + //! \brief Opaque handle that holds logging information for a particular test + //! + //! This object is an opaque handle to information used by the Logger to print test results. + //! The sample must call Logger::defineTest() in order to obtain a TestAtom that can be used + //! with Logger::reportTest{Start,End}(). + //! + class TestAtom { + public: + TestAtom(TestAtom &&) = default; + + private: + friend class Logger; + + TestAtom(bool started, const std::string &name, const std::string &cmdline) + : mStarted(started), mName(name), mCmdline(cmdline) { + } + + bool mStarted; + std::string mName; + std::string mCmdline; + }; + + //! + //! \brief Define a test for logging + //! + //! \param[in] name The name of the test. This should be a string starting with + //! "TensorRT" and containing dot-separated strings containing + //! the characters [A-Za-z0-9_]. + //! For example, "TensorRT.sample_googlenet" + //! \param[in] cmdline The command line used to reproduce the test + // + //! \return a TestAtom that can be used in Logger::reportTest{Start,End}(). + //! + static TestAtom defineTest(const std::string &name, const std::string &cmdline) { + return TestAtom(false, name, cmdline); + } + + //! + //! \brief A convenience overloaded version of defineTest() that accepts an array of command-line arguments + //! as input + //! + //! \param[in] name The name of the test + //! \param[in] argc The number of command-line arguments + //! \param[in] argv The array of command-line arguments (given as C strings) + //! + //! \return a TestAtom that can be used in Logger::reportTest{Start,End}(). + static TestAtom defineTest(const std::string &name, int argc, char const *const *argv) { + // Append TensorRT version as info + const std::string vname = name + " [TensorRT v" + std::to_string(NV_TENSORRT_VERSION) + "]"; + auto cmdline = genCmdlineString(argc, argv); + return defineTest(vname, cmdline); + } + + //! + //! \brief Report that a test has started. + //! + //! \pre reportTestStart() has not been called yet for the given testAtom + //! + //! \param[in] testAtom The handle to the test that has started + //! + static void reportTestStart(TestAtom &testAtom) { + reportTestResult(testAtom, TestResult::kRUNNING); + assert(!testAtom.mStarted); + testAtom.mStarted = true; + } + + //! + //! \brief Report that a test has ended. + //! + //! \pre reportTestStart() has been called for the given testAtom + //! + //! \param[in] testAtom The handle to the test that has ended + //! \param[in] result The result of the test. Should be one of TestResult::kPASSED, + //! TestResult::kFAILED, TestResult::kWAIVED + //! + static void reportTestEnd(const TestAtom &testAtom, TestResult result) { + assert(result != TestResult::kRUNNING); + assert(testAtom.mStarted); + reportTestResult(testAtom, result); + } + + static int reportPass(const TestAtom &testAtom) { + reportTestEnd(testAtom, TestResult::kPASSED); + return EXIT_SUCCESS; + } + + static int reportFail(const TestAtom &testAtom) { + reportTestEnd(testAtom, TestResult::kFAILED); + return EXIT_FAILURE; + } + + static int reportWaive(const TestAtom &testAtom) { + reportTestEnd(testAtom, TestResult::kWAIVED); + return EXIT_SUCCESS; + } + + static int reportTest(const TestAtom &testAtom, bool pass) { + return pass ? reportPass(testAtom) : reportFail(testAtom); + } + + Severity getReportableSeverity() const { + return mReportableSeverity; + } + + private: + //! + //! \brief returns an appropriate string for prefixing a log message with the given severity + //! + static const char *severityPrefix(Severity severity) { + switch (severity) { + case Severity::kINTERNAL_ERROR: + return "[F] "; + case Severity::kERROR: + return "[E] "; + case Severity::kWARNING: + return "[W] "; + case Severity::kINFO: + return "[I] "; + case Severity::kVERBOSE: + return "[V] "; + default: + assert(0); + return ""; + } + } + + //! + //! \brief returns an appropriate string for prefixing a test result message with the given result + //! + static const char *testResultString(TestResult result) { + switch (result) { + case TestResult::kRUNNING: + return "RUNNING"; + case TestResult::kPASSED: + return "PASSED"; + case TestResult::kFAILED: + return "FAILED"; + case TestResult::kWAIVED: + return "WAIVED"; + default: + assert(0); + return ""; + } + } + + //! + //! \brief returns an appropriate output stream (cout or cerr) to use with the given severity + //! + static std::ostream &severityOstream(Severity severity) { + return severity >= Severity::kINFO ? std::cout : std::cerr; + } + + //! + //! \brief method that implements logging test results + //! + static void reportTestResult(const TestAtom &testAtom, TestResult result) { + severityOstream(Severity::kINFO) << "&&&& " << testResultString(result) << " " << testAtom.mName << " # " + << testAtom.mCmdline << std::endl; + } + + //! + //! \brief generate a command line string from the given (argc, argv) values + //! + static std::string genCmdlineString(int argc, char const *const *argv) { + std::stringstream ss; + for (int i = 0; i < argc; i++) { + if (i > 0) { + ss << " "; + } + ss << argv[i]; + } + return ss.str(); + } + + Severity mReportableSeverity; + }; + + namespace { + +//! +//! \brief produces a LogStreamConsumer object that can be used to log messages of severity kVERBOSE +//! +//! Example usage: +//! +//! LOG_VERBOSE(logger) << "hello world" << std::endl; +//! + inline LogStreamConsumer LOG_VERBOSE(const Logger &logger) { + return LogStreamConsumer(logger.getReportableSeverity(), Severity::kVERBOSE); + } + +//! +//! \brief produces a LogStreamConsumer object that can be used to log messages of severity kINFO +//! +//! Example usage: +//! +//! LOG_INFO(logger) << "hello world" << std::endl; +//! + inline LogStreamConsumer LOG_INFO(const Logger &logger) { + return LogStreamConsumer(logger.getReportableSeverity(), Severity::kINFO); + } + +//! +//! \brief produces a LogStreamConsumer object that can be used to log messages of severity kWARNING +//! +//! Example usage: +//! +//! LOG_WARN(logger) << "hello world" << std::endl; +//! + inline LogStreamConsumer LOG_WARN(const Logger &logger) { + return LogStreamConsumer(logger.getReportableSeverity(), Severity::kWARNING); + } + +//! +//! \brief produces a LogStreamConsumer object that can be used to log messages of severity kERROR +//! +//! Example usage: +//! +//! LOG_ERROR(logger) << "hello world" << std::endl; +//! + inline LogStreamConsumer LOG_ERROR(const Logger &logger) { + return LogStreamConsumer(logger.getReportableSeverity(), Severity::kERROR); + } + +//! +//! \brief produces a LogStreamConsumer object that can be used to log messages of severity kINTERNAL_ERROR +//! ("fatal" severity) +//! +//! Example usage: +//! +//! LOG_FATAL(logger) << "hello world" << std::endl; +//! + inline LogStreamConsumer LOG_FATAL(const Logger &logger) { + return LogStreamConsumer(logger.getReportableSeverity(), Severity::kINTERNAL_ERROR); + } + + } // anonymous namespace + +} // namespace tensorrt_log + +#endif // TENSORRT_LOGGING_H diff --git a/tensorrt_utils/src/logger.cpp b/tensorrt_utils/src/logger.cpp new file mode 100644 index 00000000..e4699d78 --- /dev/null +++ b/tensorrt_utils/src/logger.cpp @@ -0,0 +1,38 @@ +/* + * Copyright (c) 2021, NVIDIA CORPORATION. All rights reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "logger.h" +#include "error_recorder.h" +#include "logging.h" + +TensorRTErrorRecorder gRecorder; +namespace tensorrt_log { + Logger gLogger{Logger::Severity::kINFO}; + LogStreamConsumer gLogVerbose{LOG_VERBOSE(gLogger)}; + LogStreamConsumer gLogInfo{LOG_INFO(gLogger)}; + LogStreamConsumer gLogWarning{LOG_WARN(gLogger)}; + LogStreamConsumer gLogError{LOG_ERROR(gLogger)}; + LogStreamConsumer gLogFatal{LOG_FATAL(gLogger)}; + + void setReportableSeverity(Logger::Severity severity) { + gLogger.setReportableSeverity(severity); + gLogVerbose.setReportableSeverity(severity); + gLogInfo.setReportableSeverity(severity); + gLogWarning.setReportableSeverity(severity); + gLogError.setReportableSeverity(severity); + gLogFatal.setReportableSeverity(severity); + } +} // namespace sample