diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..3d7c418 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,74 @@ +cmake_minimum_required(VERSION 2.8.12) + +# Project name +project(gv_tools) + +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -MP") +add_compile_options(-MP) + +add_definitions(-DGLOG_NO_ABBREVIATED_SEVERITIES) +add_definitions(-DNOMINMAX) + +if(NOT WIN32) + string(ASCII 27 Esc) + set(ColourReset "${Esc}[m") + set(Red "${Esc}[31m") +endif() + +if(DEFINED CATKIN_DEVEL_PREFIX) + message("${Red}!!!!!!!ROS workspace detected, building ROS node...!!!!!!!${ColourReset}") +else() + message("${Red}!!!!!!!Building non-ROS libraries and examples...!!!!!!! ${ColourReset}") +endif() + +find_package(OpenCV) + if(NOT OpenCV_FOUND) + message(FATAL_ERROR "OpenCV not found.") + endif() +find_package(Eigen3 REQUIRED) +message(STATUS "OPENCV: " ${OpenCV_VERSION} ) + +include_directories( + ./ + ./include + ${EIGEN3_INCLUDE_DIR} + ${OpenCV_INCLUDE_DIRS} +) +file(GLOB_RECURSE SRC_FILE_LIST "src/*.cpp" "src/*.cc" "src/*.c" "camodocal/*.cpp" "camodocal/*.cc" "camodocal/*.c") +file(GLOB_RECURSE HEARDER_FILE_LIST "include/*.h" "include/*.hpp") + +list(APPEND LIBS + ${OpenCV_LIBS} +) +add_library(gv_tools SHARED ${SRC_FILE_LIST}) +add_executable(track_dataset example/track_dataset.cpp) +target_link_libraries(track_dataset gv_tools ${LIBS}) + +if(DEFINED CATKIN_DEVEL_PREFIX) + find_package (catkin REQUIRED COMPONENTS + roscpp + rospy + std_msgs + cv_bridge + image_transport + tf + sensor_msgs + ) + catkin_package ( + CATKIN_DEPENDS roscpp rospy std_msgs cv_bridge image_transport tf sensor_msgs + LIBRARIES gv_tools + ) + include_directories( + ${catkin_INCLUDE_DIRS} + ) + add_executable (ground_tracker_node + ros/ground_tracker_node.cpp + ) + target_link_libraries(ground_tracker_node + ${LIBS} ${catkin_LIBRARIES} gv_tools + ) +endif() + + + + diff --git a/README.md b/README.md index d8d2a2b..f0d23dd 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,109 @@ -# gv_tools +# Ground-vision toolkit +A toolkit for ground feature processing, inverse perspective mapping (IPM) and so on, which is applied in our preprint [Ground-VIO](https://arxiv.org/abs/2306.08341). +

-Ground-Vision Toolkit for ground feature processing, inverse perspective mapping (IPM) and so on. +
+ +
-# Timeline -- [ ] Upload code and test data sequences. *Deadline: July 15th*. +## News +- [2023/07/11] - Initial release of code and dataset. + +## Introduction +This toolkit fully utilizes the **camera-ground geometry** for accurate ground feature tracking. In the preprint, we verify that the estimation of vehicle states, the calibration of camera-ground geometry and stable feature tracking could be leveraged in a monocular visual-inertial estimator. + +
+ +
+
+ +
+ + +## Dataset +This repo also provides the urban road visual-inertial dataset used in [Ground-VIO](https://arxiv.org/abs/2306.08341). + +The dataset contains both [Carla](https://github.com/carla-simulator/carla) simulated data sequences (S-A, S-B) and real-world data squences (R-A, R-B, R-C, R-D, R-E and R-F). The detail information is listed below. For real-world data sequences, the GT poses are obtained from the forward-and-backward smoothed solution of PPK/tactical-grade IMU integration + +### Simulation Data (Carla) + | Sequence | Date | Length |Sensors |Features |Image | + | :---: | :---: | :---: |:---: |:---: |:---: | + | S-A |-| 110 s| IMU/Camera/Semantic |Urban road|| + | S-B |-| 135 s| IMU/Camera/Semantic |Urban road|| + +### Real-World Data + | Sequence | Date| Length |Sensors |Features |Image | + | :---: | :---: | :---: |:---: |:---: |:---: | + | R-A |2022/10/12| 180 s|IMU/Camera |Urban road| | + | R-B |2022/10/12| 180 s|IMU/Camera |Urban road| | + | R-C |2022/10/12| 180 s|IMU/Camera |Urban road| | + | R-D |2022/10/12| 180 s|IMU/Camera |Urban road| | + | R-E |2022/10/12| 270 s|IMU/Camera |Highway | | + | R-F |2022/10/12| 270 s|IMU/Camera |Highway | | + +Dataset is available at [OneDrive](https://whueducn-my.sharepoint.com/:f:/g/personal/2015301610143_whu_edu_cn/Evbp_Tf3GGdHtjI890ZfuUsByA0VF6DuMAJS7bqsiOUz-g?e=r3NQI9). + +## Dependencies +The dependencies include **Eigen** and **OpenCV**. We use the **[camodocal](https://github.com/hengli/camodocal)** project to handle camera models, while we modify it to a minimal version which doesn't need **Ceres**. + +## Getting Started +The project could be built either **with** or **without ROS**. + + +### Building with ROS + +Follow the steps to build the project in a ROS workspace +```Bash +mkdir catkin_ws +mkdir catkin_ws/src +cd catkin_ws/src +git clone https://github.com/GREAT-WHU/gv_tools +cd .. +catkin_make +``` + +To run the ground tracker node, following +```Bash +source devel/setup.bash +roslaunch gv_tools track_carla_example.launch +``` +and a rviz viewer would be simultaneously launched. + +Then play the data bag in another terminal +```Bash +rosbag play s_a.bag +``` + +### Building without ROS +To build the project just as a plain CMake project +```Bash +git clone https://github.com/GREAT-WHU/gv_tools +cd gv_tools +mkdir build && cd build +cmake .. +make -j8 +``` +Run the example *track_dataset* following +```Bash +./build/track_dataset ./config/realworld/tracker.yaml DATASET_DIR/data_r_a/cam0/ DATASET_DIR/data_r_a/stamp.txt DATASET_DIR/data_r_a/gt_pose.txt +``` + +## Acknowledgements +The toolkit is developed by GREAT (GNSS+ REsearch, Application and Teaching) Group, School of Geodesy and Geomatics, Wuhan University. + +![image](pics/whu.png) + +Thanks to [VINS-Fusion](https://github.com/HKUST-Aerial-Robotics/VINS-Fusion) for excellent open-source codes. Thanks to [msckf-vio](https://github.com/KumarRobotics/msckf_vio) for inspring grid-based feature extraction method. + +## Credit / Licensing + +```txt +@article{zhou2023groundvio, + title={Ground-VIO: Monocular Visual-Inertial Odometry with Online Calibration of Camera-Ground Geometric Parameters}, + author={Yuxuan Zhou, Xingxing Li, Shengyu Li, Xuanbin Wang, Zhiheng Shen}, + journal={arXiv preprint arXiv:arXiv:2306.08341}, + year={2023} +} +``` + +The codebase and documentation is licensed under the [GNU General Public License v3 (GPL-3)](https://www.gnu.org/licenses/gpl-3.0.txt). \ No newline at end of file diff --git a/camodocal/camera_models/Camera.cc b/camodocal/camera_models/Camera.cc new file mode 100644 index 0000000..492ca69 --- /dev/null +++ b/camodocal/camera_models/Camera.cc @@ -0,0 +1,252 @@ +#include "../camera_models/Camera.h" +#include "../camera_models/ScaramuzzaCamera.h" + +#include + +namespace camodocal +{ + +Camera::Parameters::Parameters(ModelType modelType) + : m_modelType(modelType) + , m_imageWidth(0) + , m_imageHeight(0) +{ + switch (modelType) + { + case KANNALA_BRANDT: + m_nIntrinsics = 8; + break; + case PINHOLE: + m_nIntrinsics = 8; + break; + case SCARAMUZZA: + m_nIntrinsics = SCARAMUZZA_CAMERA_NUM_PARAMS; + break; + case MEI: + default: + 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) + { + case KANNALA_BRANDT: + m_nIntrinsics = 8; + break; + case PINHOLE: + m_nIntrinsics = 8; + break; + case SCARAMUZZA: + m_nIntrinsics = SCARAMUZZA_CAMERA_NUM_PARAMS; + break; + case MEI: + default: + m_nIntrinsics = 9; + } +} + +Camera::ModelType& +Camera::Parameters::modelType(void) +{ + return m_modelType; +} + +std::string& +Camera::Parameters::cameraName(void) +{ + return m_cameraName; +} + +int& +Camera::Parameters::imageWidth(void) +{ + return m_imageWidth; +} + +int& +Camera::Parameters::imageHeight(void) +{ + return m_imageHeight; +} + +Camera::ModelType +Camera::Parameters::modelType(void) const +{ + return m_modelType; +} + +const std::string& +Camera::Parameters::cameraName(void) const +{ + return m_cameraName; +} + +int +Camera::Parameters::imageWidth(void) const +{ + return m_imageWidth; +} + +int +Camera::Parameters::imageHeight(void) const +{ + return m_imageHeight; +} + +int +Camera::Parameters::nIntrinsics(void) const +{ + return m_nIntrinsics; +} + +cv::Mat& +Camera::mask(void) +{ + 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); + } + + // 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; + + spaceToPlane(P1, p1); + spaceToPlane(P2, p2); + + 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(); + } + + for (int i = 0; i < imageCount; ++i) + { + size_t pointCount = imagePoints.at(i).size(); + + pointsSoFar += pointCount; + + std::vector estImagePoints; + projectPoints(objectPoints.at(i), rvecs.at(i), tvecs.at(i), + estImagePoints); + + 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)); + } + + if (computePerViewErrors) + { + perViewErrors.at(i) = err / pointCount; + } + + 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; + + Eigen::Vector2d p; + spaceToPlane(P_cam, p); + + 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()); + + //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::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); + + // Rotate and translate + Eigen::Vector3d P; + P << objectPoint.x, objectPoint.y, objectPoint.z; + + P = R * P + t; + + Eigen::Vector2d p; + spaceToPlane(P, p); + + imagePoints.push_back(cv::Point2f(p(0), p(1))); + } +} + +} diff --git a/camodocal/camera_models/Camera.h b/camodocal/camera_models/Camera.h new file mode 100644 index 0000000..6322252 --- /dev/null +++ b/camodocal/camera_models/Camera.h @@ -0,0 +1,149 @@ +#ifndef CAMERA_H +#define CAMERA_H + +//#include +#include +#include +#include +#include + +namespace camodocal +{ + +class Camera +{ + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + enum ModelType + { + KANNALA_BRANDT, + MEI, + PINHOLE, + PINHOLE_FULL, + SCARAMUZZA + }; + + class Parameters + { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + Parameters( ModelType modelType ); + + Parameters( ModelType modelType, const std::string& cameraName, int w, int h ); + + ModelType& modelType( void ); + std::string& cameraName( void ); + int& imageWidth( void ); + int& imageHeight( void ); + + ModelType modelType( void ) const; + const std::string& cameraName( void ) const; + int imageWidth( void ) const; + int imageHeight( void ) const; + + int nIntrinsics( void ) const; + + virtual bool readFromYamlFile( const std::string& filename ) = 0; + virtual void writeToYamlFile( const std::string& filename ) const = 0; + + protected: + ModelType m_modelType; + int m_nIntrinsics; + std::string m_cameraName; + int m_imageWidth; + int m_imageHeight; + }; + + virtual ModelType modelType( void ) const = 0; + virtual const std::string& cameraName( void ) const = 0; + virtual int imageWidth( void ) const = 0; + virtual int imageHeight( void ) const = 0; + + virtual cv::Mat& mask( void ); + virtual const cv::Mat& mask( void ) const; + + virtual void estimateIntrinsics( const cv::Size& boardSize, + const std::vector< std::vector< cv::Point3f > >& objectPoints, + const std::vector< std::vector< cv::Point2f > >& imagePoints ) + = 0; + virtual void estimateExtrinsics( const std::vector< cv::Point3f >& objectPoints, + const std::vector< cv::Point2f >& imagePoints, + cv::Mat& rvec, + cv::Mat& tvec ) const; + + // Lift points from the image plane to the sphere + virtual void liftSphere( const Eigen::Vector2d& p, Eigen::Vector3d& P ) const = 0; + //%output P + + // Lift points from the image plane to the projective space + virtual void liftProjective( const Eigen::Vector2d& p, Eigen::Vector3d& P ) const = 0; + //%output P + + // Projects 3D points to the image plane (Pi function) + virtual void spaceToPlane( const Eigen::Vector3d& P, Eigen::Vector2d& p ) const = 0; + //%output p + + // Projects 3D points to the image plane (Pi function) + // and calculates jacobian + // virtual void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, + // Eigen::Matrix& J) const = 0; + //%output p + //%output J + + virtual void undistToPlane( const Eigen::Vector2d& p_u, Eigen::Vector2d& p ) const = 0; + //%output p + + // virtual void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) + // const = 0; + virtual cv::Mat initUndistortRectifyMap( cv::Mat& map1, + cv::Mat& map2, + float fx = -1.0f, + float fy = -1.0f, + cv::Size imageSize = cv::Size( 0, 0 ), + float cx = -1.0f, + float cy = -1.0f, + cv::Mat rmat = cv::Mat::eye( 3, 3, CV_32F ) ) const = 0; + + virtual int parameterCount( void ) const = 0; + + virtual void readParameters( const std::vector< double >& parameters ) = 0; + virtual void writeParameters( std::vector< double >& parameters ) const = 0; + + virtual void writeParametersToYamlFile( const std::string& filename ) const = 0; + + virtual std::string parametersToString( void ) const = 0; + + /** + * \brief Calculates the reprojection distance between points + * + * \param P1 first 3D point coordinates + * \param P2 second 3D point coordinates + * \return euclidean distance in the plane + */ + double reprojectionDist( const Eigen::Vector3d& P1, const Eigen::Vector3d& P2 ) const; + + double reprojectionError( const std::vector< std::vector< cv::Point3f > >& objectPoints, + const std::vector< std::vector< cv::Point2f > >& imagePoints, + const std::vector< cv::Mat >& rvecs, + const std::vector< cv::Mat >& tvecs, + cv::OutputArray perViewErrors = cv::noArray( ) ) const; + + double reprojectionError( const Eigen::Vector3d& P, + const Eigen::Quaterniond& camera_q, + const Eigen::Vector3d& camera_t, + const Eigen::Vector2d& observed_p ) const; + + void projectPoints( const std::vector< cv::Point3f >& objectPoints, + const cv::Mat& rvec, + const cv::Mat& tvec, + std::vector< cv::Point2f >& imagePoints ) const; + + protected: + cv::Mat m_mask; +}; + +typedef std::shared_ptr< Camera > CameraPtr; +typedef std::shared_ptr< const Camera > CameraConstPtr; +} + +#endif diff --git a/camodocal/camera_models/CameraFactory.cc b/camodocal/camera_models/CameraFactory.cc new file mode 100644 index 0000000..3848795 --- /dev/null +++ b/camodocal/camera_models/CameraFactory.cc @@ -0,0 +1,191 @@ +#include "../camera_models/CameraFactory.h" + +//#include + +#include "../camera_models/CataCamera.h" +#include "../camera_models/EquidistantCamera.h" +#include "../camera_models/PinholeCamera.h" +#include "../camera_models/PinholeFullCamera.h" +#include "../camera_models/ScaramuzzaCamera.h" + + +namespace camodocal +{ + +std::shared_ptr< CameraFactory > CameraFactory::m_instance; + +CameraFactory::CameraFactory( ) {} + +std::shared_ptr< CameraFactory > +CameraFactory::instance( void ) +{ + if ( m_instance.get( ) == 0 ) + { + m_instance.reset( new CameraFactory ); + } + + 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::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; + + std::transform(sModelType.begin(), sModelType.end(), sModelType.begin(), ::tolower); + + if ( sModelType== "kannala_brandt" ) + { + modelType = Camera::KANNALA_BRANDT; + } + else if ( sModelType=="mei" ) + { + modelType = Camera::MEI; + } + else if ( sModelType=="scaramuzza" ) + { + modelType = Camera::SCARAMUZZA; + } + else if ( sModelType=="pinhole" ) + { + modelType = Camera::PINHOLE; + } + else if ( sModelType=="PINHOLE_FULL" ) + { + modelType = Camera::PINHOLE_FULL; + } + else + { + std::cerr << "# ERROR: Unknown camera model: " << sModelType << std::endl; + return CameraPtr( ); + } + } + + 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::MEI: + default: + { + CataCameraPtr camera( new CataCamera ); + + CataCamera::Parameters params = camera->getParameters( ); + params.readFromYamlFile( filename ); + camera->setParameters( params ); + return camera; + } + } + + return CameraPtr( ); +} +} diff --git a/camodocal/camera_models/CameraFactory.h b/camodocal/camera_models/CameraFactory.h new file mode 100644 index 0000000..db726ec --- /dev/null +++ b/camodocal/camera_models/CameraFactory.h @@ -0,0 +1,33 @@ +#ifndef CAMERAFACTORY_H +#define CAMERAFACTORY_H + +#include +#include +#include + +#include "../camera_models/Camera.h" + +namespace camodocal +{ + +class CameraFactory +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + CameraFactory(); + + static std::shared_ptr instance(void); + + CameraPtr generateCamera(Camera::ModelType modelType, + const std::string& cameraName, + cv::Size imageSize) const; + + CameraPtr generateCameraFromYamlFile(const std::string& filename); + +private: + static std::shared_ptr m_instance; +}; + +} + +#endif diff --git a/camodocal/camera_models/CataCamera.cc b/camodocal/camera_models/CataCamera.cc new file mode 100644 index 0000000..100979c --- /dev/null +++ b/camodocal/camera_models/CataCamera.cc @@ -0,0 +1,1005 @@ +#include "../camera_models/CataCamera.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "../gpl/gpl.h" + +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, + 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) +{ +} + +double& +CataCamera::Parameters::xi(void) +{ + return m_xi; +} + +double& +CataCamera::Parameters::k1(void) +{ + return m_k1; +} + +double& +CataCamera::Parameters::k2(void) +{ + return m_k2; +} + +double& +CataCamera::Parameters::p1(void) +{ + return m_p1; +} + +double& +CataCamera::Parameters::p2(void) +{ + return m_p2; +} + +double& +CataCamera::Parameters::gamma1(void) +{ + return m_gamma1; +} + +double& +CataCamera::Parameters::gamma2(void) +{ + return m_gamma2; +} + +double& +CataCamera::Parameters::u0(void) +{ + return m_u0; +} + +double& +CataCamera::Parameters::v0(void) +{ + return m_v0; +} + +double +CataCamera::Parameters::xi(void) const +{ + return m_xi; +} + +double +CataCamera::Parameters::k1(void) const +{ + return m_k1; +} + +double +CataCamera::Parameters::k2(void) const +{ + return m_k2; +} + +double +CataCamera::Parameters::p1(void) const +{ + return m_p1; +} + +double +CataCamera::Parameters::p2(void) const +{ + return m_p2; +} + +double +CataCamera::Parameters::gamma1(void) const +{ + return m_gamma1; +} + +double +CataCamera::Parameters::gamma2(void) const +{ + return m_gamma2; +} + +double +CataCamera::Parameters::u0(void) const +{ + return m_u0; +} + +double +CataCamera::Parameters::v0(void) const +{ + return m_v0; +} + +bool +CataCamera::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 (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"]); + + 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["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; +} + +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(); +} + +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; +} + +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; +} + +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; + } + + // 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(); +} + +const std::string& +CataCamera::cameraName(void) const +{ + return mParameters.cameraName(); +} + +int +CataCamera::imageWidth(void) const +{ + return mParameters.imageWidth(); +} + +int +CataCamera::imageHeight(void) const +{ + return mParameters.imageHeight(); +} + +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 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()); + + 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); + + 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; + } + } + } + + 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); +} + +/** + * \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; + } +} + +/** + * \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)); + } +} + + +/** + * \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; + + // 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; + } + + // Apply generalised projection matrix + p << mParameters.gamma1() * p_d(0) + mParameters.u0(), + mParameters.gamma2() * p_d(1) + mParameters.v0(); +} + +#if 0 +/** + * \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 +CataCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, + Eigen::Matrix& J) const +{ + double xi = mParameters.xi(); + + 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) + xi * norm); + p_u << inv_denom * P(0), inv_denom * P(1); + + // Calculate jacobian + inv_denom = inv_denom * inv_denom / norm; + double dudx = inv_denom * (norm * P(2) + xi * (P(1) * P(1) + P(2) * P(2))); + double dvdx = -inv_denom * xi * P(0) * P(1); + double dudy = dvdx; + double dvdy = inv_denom * (norm * P(2) + xi * (P(0) * P(0) + P(2) * P(2))); + inv_denom = inv_denom * (-xi * P(2) - norm); // reuse variable + 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 gamma1 = mParameters.gamma1(); + double gamma2 = mParameters.gamma2(); + + // Make the product of the jacobians + // and add projection matrix jacobian + inv_denom = gamma1 * (dudx * dxdmx + dvdx * dxdmy); // reuse + dvdx = gamma2 * (dudx * dydmx + dvdx * dydmy); + dudx = inv_denom; + + inv_denom = gamma1 * (dudy * dxdmx + dvdy * dxdmy); // reuse + dvdy = gamma2 * (dudy * dydmx + dvdy * dydmy); + dudy = inv_denom; + + inv_denom = gamma1 * (dudz * dxdmx + dvdz * dxdmy); // reuse + dvdz = gamma2 * (dudz * dydmx + dvdz * dydmy); + dudz = inv_denom; + + // Apply generalised projection matrix + p << gamma1 * p_d(0) + mParameters.u0(), + gamma2 * p_d(1) + mParameters.v0(); + + J << dudx, dudy, dudz, + dvdx, dvdy, dvdz; +} +#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; + + 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(); +} + +/** + * \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); +} + +/** + * \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::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 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::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::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); + + 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.gamma1(); + K_rect(1,1) = mParameters.gamma2(); + } + + 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; + + 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); + } + } + + 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; +} + +int +CataCamera::parameterCount(void) const +{ + return 9; +} + +const CataCamera::Parameters& +CataCamera::getParameters(void) const +{ + return mParameters; +} + +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; + } + + 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; + } + + 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); + + 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::writeParametersToYamlFile(const std::string& filename) const +{ + mParameters.writeToYamlFile(filename); +} + +std::string +CataCamera::parametersToString(void) const +{ + std::ostringstream oss; + oss << mParameters; + + return oss.str(); +} + +} diff --git a/camodocal/camera_models/CataCamera.h b/camodocal/camera_models/CataCamera.h new file mode 100644 index 0000000..1a7881f --- /dev/null +++ b/camodocal/camera_models/CataCamera.h @@ -0,0 +1,154 @@ +#ifndef CATACAMERA_H +#define CATACAMERA_H + +#include +#include + +#include "Camera.h" + +namespace camodocal +{ + +/** + * C. Mei, and P. Rives, Single View Point Omnidirectional Camera Calibration + * from Planar Grids, ICRA 2007 + */ + +class CataCamera: public Camera +{ +public: + class Parameters: public Camera::Parameters + { + public: + 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); + + double& xi(void); + double& k1(void); + double& k2(void); + double& p1(void); + double& p2(void); + double& gamma1(void); + double& gamma2(void); + double& u0(void); + double& v0(void); + + double xi(void) const; + double k1(void) const; + double k2(void) const; + double p1(void) const; + double p2(void) const; + double gamma1(void) const; + double gamma2(void) const; + double u0(void) const; + double v0(void) const; + + bool readFromYamlFile(const std::string& filename); + void writeToYamlFile(const std::string& filename) const; + + Parameters& operator=(const Parameters& other); + friend std::ostream& operator<< (std::ostream& out, const Parameters& params); + + private: + double m_xi; + double m_k1; + double m_k2; + double m_p1; + double m_p2; + double m_gamma1; + double m_gamma2; + double m_u0; + double m_v0; + }; + + CataCamera(); + + /** + * \brief Constructor from the projection model parameters + */ + 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); + /** + * \brief Constructor from the projection model parameters + */ + CataCamera(const Parameters& params); + + Camera::ModelType modelType(void) const; + const std::string& cameraName(void) const; + int imageWidth(void) const; + int imageHeight(void) const; + + void estimateIntrinsics(const cv::Size& boardSize, + const std::vector< std::vector >& objectPoints, + const std::vector< std::vector >& imagePoints); + + // Lift points from the image plane to the sphere + void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; + //%output P + + // Lift points from the image plane to the projective space + void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; + //%output P + + // Projects 3D points to the image plane (Pi function) + void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const; + //%output p + + // Projects 3D points to the image plane (Pi function) + // and calculates jacobian + void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, + Eigen::Matrix& J) const; + //%output p + //%output J + + void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const; + //%output p + + template + static void spaceToPlane(const T* const params, + const T* const q, const T* const t, + const Eigen::Matrix& P, + Eigen::Matrix& p); + + void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const; + void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u, + Eigen::Matrix2d& J) const; + + void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const; + cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, + float fx = -1.0f, float fy = -1.0f, + cv::Size imageSize = cv::Size(0, 0), + float cx = -1.0f, float cy = -1.0f, + cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const; + + int parameterCount(void) const; + + const Parameters& getParameters(void) const; + void setParameters(const Parameters& parameters); + + void readParameters(const std::vector& parameterVec); + void writeParameters(std::vector& parameterVec) const; + + void writeParametersToYamlFile(const std::string& filename) const; + + std::string parametersToString(void) const; + +private: + Parameters mParameters; + + double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23; + bool m_noDistortion; +}; + +typedef std::shared_ptr CataCameraPtr; +typedef std::shared_ptr CataCameraConstPtr; + +} + +#endif diff --git a/camodocal/camera_models/EquidistantCamera.cc b/camodocal/camera_models/EquidistantCamera.cc new file mode 100644 index 0000000..0713845 --- /dev/null +++ b/camodocal/camera_models/EquidistantCamera.cc @@ -0,0 +1,820 @@ +#include "../camera_models/EquidistantCamera.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "../gpl/gpl.h" + +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) +{ + +} + +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::k3(void) +{ + return m_k3; +} + +double& +EquidistantCamera::Parameters::k4(void) +{ + return m_k4; +} + +double& +EquidistantCamera::Parameters::k5(void) +{ + return m_k5; +} + +double& +EquidistantCamera::Parameters::mu(void) +{ + return m_mu; +} + +double& +EquidistantCamera::Parameters::mv(void) +{ + return m_mv; +} + +double& +EquidistantCamera::Parameters::u0(void) +{ + return m_u0; +} + +double& +EquidistantCamera::Parameters::v0(void) +{ + return m_v0; +} + +double +EquidistantCamera::Parameters::k2(void) const +{ + return m_k2; +} + +double +EquidistantCamera::Parameters::k3(void) const +{ + return m_k3; +} + +double +EquidistantCamera::Parameters::k4(void) const +{ + return m_k4; +} + +double +EquidistantCamera::Parameters::k5(void) const +{ + return m_k5; +} + +double +EquidistantCamera::Parameters::mu(void) const +{ + return m_mu; +} + +double +EquidistantCamera::Parameters::mv(void) const +{ + return m_mv; +} + +double +EquidistantCamera::Parameters::u0(void) const +{ + return m_u0; +} + +double +EquidistantCamera::Parameters::v0(void) const +{ + return m_v0; +} + +bool +EquidistantCamera::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 (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; +} + +EquidistantCamera::EquidistantCamera() + : 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(); +} + +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< 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); + std::vector 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 (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; + } + } + } + } + + if (f0 <= 0.0 && minReprojErr >= std::numeric_limits::max()) + { + std::cout << "[" << params.cameraName() << "] " + << "# INFO: kannala-Brandt model fails with given data. " << std::endl; + + return; + } + + params.mu() = f0; + params.mv() = f0; + + 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 +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); +} + +/** + * \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)); + + 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 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(); +} + +/** + * \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); + } + } + + 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 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.mu(); + K_rect(1,1) = mParameters.mv(); + } + + 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; + + 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); + } + } + + 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; +} + +int +EquidistantCamera::parameterCount(void) const +{ + return 8; +} + +const EquidistantCamera::Parameters& +EquidistantCamera::getParameters(void) const +{ + return mParameters; +} + +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(); +} + +void +EquidistantCamera::readParameters(const std::vector& parameterVec) +{ + if (parameterVec.size() != parameterCount()) + { + return; + } + + 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); + + 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::writeParametersToYamlFile(const std::string& filename) const +{ + mParameters.writeToYamlFile(filename); +} + +std::string +EquidistantCamera::parametersToString(void) const +{ + std::ostringstream oss; + oss << mParameters; + + 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); + } + + 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 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); + } + + if (thetas.empty()) + { + theta = p_u_norm; + } + else + { + theta = *std::min_element(thetas.begin(), thetas.end()); + } + } +} + +} diff --git a/camodocal/camera_models/EquidistantCamera.h b/camodocal/camera_models/EquidistantCamera.h new file mode 100644 index 0000000..347d441 --- /dev/null +++ b/camodocal/camera_models/EquidistantCamera.h @@ -0,0 +1,171 @@ +#ifndef EQUIDISTANTCAMERA_H +#define EQUIDISTANTCAMERA_H + +#include +#include + +#include "Camera.h" + +namespace camodocal +{ + +/** + * J. Kannala, and S. Brandt, A Generic Camera Model and Calibration Method + * for Conventional, Wide-Angle, and Fish-Eye Lenses, PAMI 2006 + */ + +class EquidistantCamera: public Camera +{ +public: + class Parameters: public Camera::Parameters + { + public: + 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); + + double& k2(void); + double& k3(void); + double& k4(void); + double& k5(void); + double& mu(void); + double& mv(void); + double& u0(void); + double& v0(void); + + double k2(void) const; + double k3(void) const; + double k4(void) const; + double k5(void) const; + double mu(void) const; + double mv(void) const; + double u0(void) const; + double v0(void) const; + + bool readFromYamlFile(const std::string& filename); + void writeToYamlFile(const std::string& filename) const; + + Parameters& operator=(const Parameters& other); + friend std::ostream& operator<< (std::ostream& out, const Parameters& params); + + private: + // projection + double m_k2; + double m_k3; + double m_k4; + double m_k5; + + double m_mu; + double m_mv; + double m_u0; + double m_v0; + }; + + EquidistantCamera(); + + /** + * \brief Constructor from the projection model parameters + */ + 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); + /** + * \brief Constructor from the projection model parameters + */ + EquidistantCamera(const Parameters& params); + + Camera::ModelType modelType(void) const; + const std::string& cameraName(void) const; + int imageWidth(void) const; + int imageHeight(void) const; + + void estimateIntrinsics(const cv::Size& boardSize, + const std::vector< std::vector >& objectPoints, + const std::vector< std::vector >& imagePoints); + + // Lift points from the image plane to the sphere + virtual void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; + //%output P + + // Lift points from the image plane to the projective space + void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; + //%output P + + // Projects 3D points to the image plane (Pi function) + void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const; + //%output p + + // Projects 3D points to the image plane (Pi function) + // and calculates jacobian + void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, + Eigen::Matrix& J) const; + //%output p + //%output J + + void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const; + //%output p + + template + static void spaceToPlane(const T* const params, + const T* const q, const T* const t, + const Eigen::Matrix& P, + Eigen::Matrix& p); + + void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const; + cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, + float fx = -1.0f, float fy = -1.0f, + cv::Size imageSize = cv::Size(0, 0), + float cx = -1.0f, float cy = -1.0f, + cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const; + + int parameterCount(void) const; + + const Parameters& getParameters(void) const; + void setParameters(const Parameters& parameters); + + void readParameters(const std::vector& parameterVec); + void writeParameters(std::vector& parameterVec) const; + + void writeParametersToYamlFile(const std::string& filename) const; + + std::string parametersToString(void) const; + +private: + template + static T r(T k2, T k3, T k4, T k5, T theta); + + + void fitOddPoly(const std::vector& x, const std::vector& y, + int n, std::vector& coeffs) const; + + void backprojectSymmetric(const Eigen::Vector2d& p_u, + double& theta, double& phi) const; + + Parameters mParameters; + + double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23; +}; + +typedef std::shared_ptr EquidistantCameraPtr; +typedef std::shared_ptr EquidistantCameraConstPtr; + +template +T +EquidistantCamera::r(T k2, T k3, T k4, T k5, T theta) +{ + // k1 = 1 + return theta + + k2 * theta * theta * theta + + k3 * theta * theta * theta * theta * theta + + k4 * theta * theta * theta * theta * theta * theta * theta + + k5 * theta * theta * theta * theta * theta * theta * theta * theta * theta; +} + +} + +#endif diff --git a/camodocal/camera_models/PinholeCamera.cc b/camodocal/camera_models/PinholeCamera.cc new file mode 100644 index 0000000..13a1af2 --- /dev/null +++ b/camodocal/camera_models/PinholeCamera.cc @@ -0,0 +1,881 @@ +#include "../camera_models/PinholeCamera.h" + +#include +#include +#include +#include +#include +#include +#include + +#include "../gpl/gpl.h" + +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, + 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) +{ +} + +double& +PinholeCamera::Parameters::k1(void) +{ + return m_k1; +} + +double& +PinholeCamera::Parameters::k2(void) +{ + return m_k2; +} + +double& +PinholeCamera::Parameters::p1(void) +{ + return m_p1; +} + +double& +PinholeCamera::Parameters::p2(void) +{ + return m_p2; +} + +double& +PinholeCamera::Parameters::fx(void) +{ + return m_fx; +} + +double& +PinholeCamera::Parameters::fy(void) +{ + return m_fy; +} + +double& +PinholeCamera::Parameters::cx(void) +{ + return m_cx; +} + +double& +PinholeCamera::Parameters::cy(void) +{ + return m_cy; +} + +double +PinholeCamera::Parameters::k1(void) const +{ + return m_k1; +} + +double +PinholeCamera::Parameters::k2(void) const +{ + return m_k2; +} + +double +PinholeCamera::Parameters::p1(void) const +{ + return m_p1; +} + +double +PinholeCamera::Parameters::p2(void) const +{ + return m_p2; +} + +double +PinholeCamera::Parameters::fx(void) const +{ + return m_fx; +} + +double +PinholeCamera::Parameters::fy(void) const +{ + return m_fy; +} + +double +PinholeCamera::Parameters::cx(void) const +{ + return m_cx; +} + +double +PinholeCamera::Parameters::cy(void) const +{ + return m_cy; +} + +bool +PinholeCamera::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 (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; +} + +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; +} + +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::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(); +} + +Camera::ModelType +PinholeCamera::modelType(void) const +{ + return mParameters.modelType(); +} + +const std::string& +PinholeCamera::cameraName(void) const +{ + return mParameters.cameraName(); +} + +int +PinholeCamera::imageWidth(void) const +{ + return mParameters.imageWidth(); +} + +int +PinholeCamera::imageHeight(void) const +{ + return mParameters.imageHeight(); +} + +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 + + 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& 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]; + } + + 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))); + + 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 +PinholeCamera::liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const +{ + liftProjective(p, P); + + P.normalize(); +} + +/** + * \brief Lifts a point from the image plane to its projective ray + * + * \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); + } + } + } + + // 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; + + // 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; + } + + // Apply generalised projection matrix + p << mParameters.fx() * p_d(0) + mParameters.cx(), + mParameters.fy() * p_d(1) + mParameters.cy(); +} + +#if 0 +/** + * \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 +PinholeCamera::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; +} +#endif + +/** + * \brief Projects an undistorted 2D point p_u to the image plane + * + * \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; + + 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(); +} + +/** + * \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 +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); +} + +/** + * \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 +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::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); + + Eigen::Matrix3f R, R_inv; + cv::cv2eigen(rmat, R); + R_inv = R.inverse(); + + // 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 (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(); + + 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::Vector2d p; + spaceToPlane(uo.cast(), p); + + mapX.at(v,u) = p(0); + mapY.at(v,u) = p(1); + } + } + + 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; +} + +int +PinholeCamera::parameterCount(void) const +{ + return 8; +} + +const PinholeCamera::Parameters& +PinholeCamera::getParameters(void) const +{ + return mParameters; +} + +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; + } + + 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; + } + + 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); + + 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::writeParametersToYamlFile(const std::string& filename) const +{ + mParameters.writeToYamlFile(filename); +} + +std::string +PinholeCamera::parametersToString(void) const +{ + std::ostringstream oss; + oss << mParameters; + + return oss.str(); +} + +} diff --git a/camodocal/camera_models/PinholeCamera.h b/camodocal/camera_models/PinholeCamera.h new file mode 100644 index 0000000..a292280 --- /dev/null +++ b/camodocal/camera_models/PinholeCamera.h @@ -0,0 +1,146 @@ +#ifndef PINHOLECAMERA_H +#define PINHOLECAMERA_H + +#include +#include + +#include "Camera.h" + +namespace camodocal +{ + +class PinholeCamera: public Camera +{ +public: + class Parameters: public Camera::Parameters + { + public: + 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); + + double& k1(void); + double& k2(void); + double& p1(void); + double& p2(void); + double& fx(void); + double& fy(void); + double& cx(void); + double& cy(void); + + double xi(void) const; + double k1(void) const; + double k2(void) const; + double p1(void) const; + double p2(void) const; + double fx(void) const; + double fy(void) const; + double cx(void) const; + double cy(void) const; + + bool readFromYamlFile(const std::string& filename); + void writeToYamlFile(const std::string& filename) const; + + Parameters& operator=(const Parameters& other); + friend std::ostream& operator<< (std::ostream& out, const Parameters& params); + + private: + double m_k1; + double m_k2; + double m_p1; + double m_p2; + double m_fx; + double m_fy; + double m_cx; + double m_cy; + }; + + PinholeCamera(); + + /** + * \brief Constructor from the projection model parameters + */ + 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); + /** + * \brief Constructor from the projection model parameters + */ + PinholeCamera(const Parameters& params); + + Camera::ModelType modelType(void) const; + const std::string& cameraName(void) const; + int imageWidth(void) const; + int imageHeight(void) const; + + void estimateIntrinsics(const cv::Size& boardSize, + const std::vector< std::vector >& objectPoints, + const std::vector< std::vector >& imagePoints); + + // Lift points from the image plane to the sphere + virtual void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; + //%output P + + // Lift points from the image plane to the projective space + void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; + //%output P + + // Projects 3D points to the image plane (Pi function) + void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const; + //%output p + + // Projects 3D points to the image plane (Pi function) + // and calculates jacobian + void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, + Eigen::Matrix& J) const; + //%output p + //%output J + + void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const; + //%output p + + template + static void spaceToPlane(const T* const params, + const T* const q, const T* const t, + const Eigen::Matrix& P, + Eigen::Matrix& p); + + void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const; + void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u, + Eigen::Matrix2d& J) const; + + void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const; + cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, + float fx = -1.0f, float fy = -1.0f, + cv::Size imageSize = cv::Size(0, 0), + float cx = -1.0f, float cy = -1.0f, + cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const; + + int parameterCount(void) const; + + const Parameters& getParameters(void) const; + void setParameters(const Parameters& parameters); + + void readParameters(const std::vector& parameterVec); + void writeParameters(std::vector& parameterVec) const; + + void writeParametersToYamlFile(const std::string& filename) const; + + std::string parametersToString(void) const; + +private: + Parameters mParameters; + + double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23; + bool m_noDistortion; +}; + +typedef std::shared_ptr PinholeCameraPtr; +typedef std::shared_ptr PinholeCameraConstPtr; + +} + +#endif diff --git a/camodocal/camera_models/PinholeFullCamera.cc b/camodocal/camera_models/PinholeFullCamera.cc new file mode 100644 index 0000000..a6dcec3 --- /dev/null +++ b/camodocal/camera_models/PinholeFullCamera.cc @@ -0,0 +1,1000 @@ +#include "../camera_models/PinholeFullCamera.h" + +#include +#include +#include +#include +#include +#include +#include + +#include "../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 ) +{ +} + +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::k1( void ) +{ + return m_k1; +} + +double& +PinholeFullCamera::Parameters::k2( void ) +{ + return m_k2; +} + +double& +PinholeFullCamera::Parameters::k3( ) +{ + return m_k3; +} + +double& +PinholeFullCamera::Parameters::k4( ) +{ + return m_k4; +} + +double& +PinholeFullCamera::Parameters::k5( ) +{ + return m_k5; +} + +double& +PinholeFullCamera::Parameters::k6( ) +{ + return m_k6; +} + +double& +PinholeFullCamera::Parameters::p1( void ) +{ + return m_p1; +} + +double& +PinholeFullCamera::Parameters::p2( void ) +{ + return m_p2; +} + +double& +PinholeFullCamera::Parameters::fx( void ) +{ + return m_fx; +} + +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::k1( void ) const +{ + return m_k1; +} + +double +PinholeFullCamera::Parameters::k2( void ) const +{ + return m_k2; +} + +double +PinholeFullCamera::Parameters::p1( void ) const +{ + return m_p1; +} + +double +PinholeFullCamera::Parameters::p2( void ) const +{ + return m_p2; +} + +double +PinholeFullCamera::Parameters::fx( void ) const +{ + return m_fx; +} + +double +PinholeFullCamera::Parameters::fy( void ) const +{ + return m_fy; +} + +double +PinholeFullCamera::Parameters::cx( void ) const +{ + return m_cx; +} + +double +PinholeFullCamera::Parameters::cy( void ) const +{ + return m_cy; +} + +double +PinholeFullCamera::Parameters::k3( ) const +{ + return m_k3; +} + +double +PinholeFullCamera::Parameters::k4( ) const +{ + return m_k4; +} + +double +PinholeFullCamera::Parameters::k5( ) const +{ + return m_k5; +} + +double +PinholeFullCamera::Parameters::k6( ) const +{ + return m_k6; +} + +bool +PinholeFullCamera::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 ( sModelType.compare( "PINHOLE_FULL" ) != 0 ) + { + return false; + } + } + + 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; +} + +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< 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]; + } + + 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< double >( 0 ) ) ); + params.fy( ) = sqrt( fabs( 1.0 / f.at< double >( 1 ) ) ); + + 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 +PinholeFullCamera::liftSphere( const Eigen::Vector2d& p, Eigen::Vector3d& P ) const +{ + liftProjective( p, P ); + + P.normalize( ); +} + +/** + * \brief Lifts a point from the image plane to its projective ray + * + * \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 ) ); + } + } + + 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 +} + +/** + * \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 +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 ); + + 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( ); +} + +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; +} + +/** + * \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 +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; +} + +/** + * \brief Projects an undistorted 2D point p_u to the image plane + * + * \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; + } + + // Apply generalised projection matrix + p << mParameters.fx( ) * p_d( 0 ) + mParameters.cx( ), + mParameters.fy( ) * p_d( 1 ) + mParameters.cy( ); +} + +/** + * \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 +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; +} + +/** + * \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 +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 ); + } + } + + 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 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( ); + + // 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 ( 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( ); + + 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::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::Mat K_rect_cv; + cv::eigen2cv( K_rect, K_rect_cv ); + return K_rect_cv; +} + +int +PinholeFullCamera::parameterCount( void ) const +{ + return 12; +} + +const PinholeFullCamera::Parameters& +PinholeFullCamera::getParameters( void ) const +{ + return mParameters; +} + +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; + } + + 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; + } + + 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 ); +} + +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::writeParametersToYamlFile( const std::string& filename ) const +{ + mParameters.writeToYamlFile( filename ); +} + +std::string +PinholeFullCamera::parametersToString( void ) const +{ + std::ostringstream oss; + oss << mParameters; + + return oss.str( ); +} +} diff --git a/camodocal/camera_models/PinholeFullCamera.h b/camodocal/camera_models/PinholeFullCamera.h new file mode 100644 index 0000000..61a5716 --- /dev/null +++ b/camodocal/camera_models/PinholeFullCamera.h @@ -0,0 +1,215 @@ +#ifndef PinholeFullCAMERA_H +#define PinholeFullCAMERA_H + +#include +#include + +#include "Camera.h" + +namespace camodocal +{ + +class PinholeFullCamera : public Camera +{ + public: + class Parameters : public Camera::Parameters + { + public: + 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 ); + + double& k1( void ); + double& k2( void ); + double& k3( void ); + double& k4( void ); + double& k5( void ); + double& k6( void ); + double& p1( void ); + double& p2( void ); + double& fx( void ); + double& fy( void ); + double& cx( void ); + double& cy( void ); + + double xi( void ) const; + double k1( void ) const; + double k2( void ) const; + double k3( void ) const; + double k4( void ) const; + double k5( void ) const; + double k6( void ) const; + double p1( void ) const; + double p2( void ) const; + double fx( void ) const; + double fy( void ) const; + double cx( void ) const; + double cy( void ) const; + + bool readFromYamlFile( const std::string& filename ); + void writeToYamlFile( const std::string& filename ) const; + + Parameters& operator=( const Parameters& other ); + friend std::ostream& operator<<( std::ostream& out, const Parameters& params ); + + private: + double m_k1; + double m_k2; + double m_p1; + double m_p2; + double m_fx; + double m_fy; + double m_cx; + double m_cy; + double m_k3; + double m_k4; + double m_k5; + double m_k6; + }; + + PinholeFullCamera( ); + + /** + * \brief Constructor from the projection model parameters + */ + 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 ); + /** + * \brief Constructor from the projection model parameters + */ + PinholeFullCamera( const Parameters& params ); + + Camera::ModelType modelType( void ) const; + const std::string& cameraName( void ) const; + int imageWidth( void ) const; + int imageHeight( void ) const; + cv::Size imageSize( ) const { return cv::Size( imageWidth( ), imageHeight( ) ); } + cv::Point2f getPrinciple( ) const + { + return cv::Point2f( mParameters.cx( ), mParameters.cy( ) ); + } + + void estimateIntrinsics( const cv::Size& boardSize, + const std::vector< std::vector< cv::Point3f > >& objectPoints, + const std::vector< std::vector< cv::Point2f > >& imagePoints ); + + void setInitIntrinsics( const std::vector< std::vector< cv::Point3f > >& objectPoints, + const std::vector< std::vector< cv::Point2f > >& imagePoints ) + { + Parameters params = getParameters( ); + + params.k1( ) = 0.0; + params.k2( ) = 0.0; + params.k3( ) = 0.0; + params.k4( ) = 0.0; + params.k5( ) = 0.0; + params.k6( ) = 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; + params.fx( ) = 1200; + params.fy( ) = 1200; + + setParameters( params ); + } + + // Lift points from the image plane to the sphere + virtual void liftSphere( const Eigen::Vector2d& p, Eigen::Vector3d& P ) const; + //%output P + + // Lift points from the image plane to the projective space + void liftProjective( const Eigen::Vector2d& p, Eigen::Vector3d& P ) const; + //%output P + + void liftProjective( const Eigen::Vector2d& p, Eigen::Vector3d& P, float image_scale ) const; + + // Projects 3D points to the image plane (Pi function) + void spaceToPlane( const Eigen::Vector3d& P, Eigen::Vector2d& p ) const; + //%output p + + void spaceToPlane( const Eigen::Vector3d& P, Eigen::Vector2d& p, float image_scalse ) const; + + // Projects 3D points to the image plane (Pi function) + // and calculates jacobian + void spaceToPlane( const Eigen::Vector3d& P, Eigen::Vector2d& p, Eigen::Matrix< double, 2, 3 >& J ) const; + //%output p + //%output J + + void undistToPlane( const Eigen::Vector2d& p_u, Eigen::Vector2d& p ) const; + //%output p + + template< typename T > + static void spaceToPlane( const T* const params, + const T* const q, + const T* const t, + const Eigen::Matrix< T, 3, 1 >& P, + Eigen::Matrix< T, 2, 1 >& p ); + + void distortion( const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u ) const; + void distortion( const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u, Eigen::Matrix2d& J ) const; + + void initUndistortMap( cv::Mat& map1, cv::Mat& map2, double fScale = 1.0 ) const; + cv::Mat initUndistortRectifyMap( cv::Mat& map1, + cv::Mat& map2, + float fx = -1.0f, + float fy = -1.0f, + cv::Size imageSize = cv::Size( 0, 0 ), + float cx = -1.0f, + float cy = -1.0f, + cv::Mat rmat = cv::Mat::eye( 3, 3, CV_32F ) ) const; + + int parameterCount( void ) const; + + const Parameters& getParameters( void ) const; + void setParameters( const Parameters& parameters ); + + void readParameters( const std::vector< double >& parameterVec ); + void writeParameters( std::vector< double >& parameterVec ) const; + + void writeParametersToYamlFile( const std::string& filename ) const; + + std::string parametersToString( void ) const; + + private: + Parameters mParameters; + + double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23; + bool m_noDistortion; +}; + +typedef std::shared_ptr< PinholeFullCamera > PinholeFullCameraPtr; +typedef std::shared_ptr< const PinholeFullCamera > PinholeFullCameraConstPtr; + +} + +#endif diff --git a/camodocal/camera_models/ScaramuzzaCamera.cc b/camodocal/camera_models/ScaramuzzaCamera.cc new file mode 100644 index 0000000..260a940 --- /dev/null +++ b/camodocal/camera_models/ScaramuzzaCamera.cc @@ -0,0 +1,833 @@ +#include "../camera_models/ScaramuzzaCamera.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +//#include +//#include + +#include "../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::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); + + 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; + } + + Eigen::JacobiSVD svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV); + Eigen::VectorXd x = svd.solve(B); + + return x; +} + +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); +} + + + +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 (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"]); + + cv::FileNode n = fs["poly_parameters"]; + for(int i=0; i < SCARAMUZZA_POLY_SIZE; i++) + m_poly[i] = static_cast(n[std::string("p") + std::to_string(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") + std::to_string(i)]); + + 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"]); + + 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") + std::to_string(i) << m_poly[i]; + fs << "}"; + + fs << "inv_poly_parameters"; + fs << "{"; + for(int i=0; i < SCARAMUZZA_INV_POLY_SIZE; i++) + fs << std::string("p") + std::to_string(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; +} + +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") + std::to_string(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") + std::to_string(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(const OCAMCamera::Parameters& params) + : mParameters(params) +{ + m_inv_scale = 1.0 / (params.C() - params.D() * params.E()); +} + +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(); +} + +int +OCAMCamera::imageHeight(void) const +{ + return mParameters.imageHeight(); +} + +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"); + + // 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< Eigen::Matrix3d > RList; + std::vector< Eigen::Vector3d > 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; + } + + 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); + } + } + } + + // 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(v,u) = p(0); + mapY.at(v,u) = p(1); + } + } + + cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false); +} +#endif + +cv::Mat +OCAMCamera::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; + + K_rect << fx, 0, cx < 0 ? imageSize.width / 2 : cx, + 0, fy, cy < 0 ? imageSize.height / 2 : cy, + 0, 0, 1; + + if (fx < 0 || fy < 0) + { + throw std::string(std::string(__FUNCTION__) + ": Focal length must be specified"); + } + + 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; + + 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); + } + } + + 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; +} + +int +OCAMCamera::parameterCount(void) const +{ + return SCARAMUZZA_CAMERA_NUM_PARAMS; +} + +const OCAMCamera::Parameters& +OCAMCamera::getParameters(void) const +{ + return mParameters; +} + +void +OCAMCamera::setParameters(const OCAMCamera::Parameters& parameters) +{ + mParameters = parameters; + + m_inv_scale = 1.0 / (parameters.C() - parameters.D() * parameters.E()); +} + +void +OCAMCamera::readParameters(const std::vector& parameterVec) +{ + if ((int)parameterVec.size() != parameterCount()) + { + return; + } + + 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); + + 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::writeParametersToYamlFile(const std::string& filename) const +{ + mParameters.writeToYamlFile(filename); +} + +std::string +OCAMCamera::parametersToString(void) const +{ + std::ostringstream oss; + oss << mParameters; + + return oss.str(); +} + +} diff --git a/camodocal/camera_models/ScaramuzzaCamera.h b/camodocal/camera_models/ScaramuzzaCamera.h new file mode 100644 index 0000000..2e38867 --- /dev/null +++ b/camodocal/camera_models/ScaramuzzaCamera.h @@ -0,0 +1,245 @@ +#ifndef SCARAMUZZACAMERA_H +#define SCARAMUZZACAMERA_H + +#include +#include + +#include "Camera.h" + +namespace camodocal +{ + +#define SCARAMUZZA_POLY_SIZE 5 +#define SCARAMUZZA_INV_POLY_SIZE 20 + +#define SCARAMUZZA_CAMERA_NUM_PARAMS (SCARAMUZZA_POLY_SIZE + SCARAMUZZA_INV_POLY_SIZE + 2 /*center*/ + 3 /*affine*/) + +/** + * Scaramuzza Camera (Omnidirectional) + * https://sites.google.com/site/scarabotix/ocamcalib-toolbox + */ + +class OCAMCamera: public Camera +{ +public: + class Parameters: public Camera::Parameters + { + public: + Parameters(); + + double& C(void) { return m_C; } + double& D(void) { return m_D; } + double& E(void) { return m_E; } + + double& center_x(void) { return m_center_x; } + double& center_y(void) { return m_center_y; } + + double& poly(int idx) { return m_poly[idx]; } + double& inv_poly(int idx) { return m_inv_poly[idx]; } + + double C(void) const { return m_C; } + double D(void) const { return m_D; } + double E(void) const { return m_E; } + + double center_x(void) const { return m_center_x; } + double center_y(void) const { return m_center_y; } + + double poly(int idx) const { return m_poly[idx]; } + double inv_poly(int idx) const { return m_inv_poly[idx]; } + + bool readFromYamlFile(const std::string& filename); + void writeToYamlFile(const std::string& filename) const; + + Parameters& operator=(const Parameters& other); + friend std::ostream& operator<< (std::ostream& out, const Parameters& params); + + private: + double m_poly[SCARAMUZZA_POLY_SIZE]; + double m_inv_poly[SCARAMUZZA_INV_POLY_SIZE]; + double m_C; + double m_D; + double m_E; + double m_center_x; + double m_center_y; + }; + + OCAMCamera(); + + /** + * \brief Constructor from the projection model parameters + */ + OCAMCamera(const Parameters& params); + + Camera::ModelType modelType(void) const; + const std::string& cameraName(void) const; + int imageWidth(void) const; + int imageHeight(void) const; + + void estimateIntrinsics(const cv::Size& boardSize, + const std::vector< std::vector >& objectPoints, + const std::vector< std::vector >& imagePoints); + + // Lift points from the image plane to the sphere + void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; + //%output P + + // Lift points from the image plane to the projective space + void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; + //%output P + + // Projects 3D points to the image plane (Pi function) + void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const; + //%output p + + // Projects 3D points to the image plane (Pi function) + // and calculates jacobian + //void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, + // Eigen::Matrix& J) const; + //%output p + //%output J + + void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const; + //%output p + + template + static void spaceToPlane(const T* const params, + const T* const q, const T* const t, + const Eigen::Matrix& P, + Eigen::Matrix& p); + template + static void spaceToSphere(const T* const params, + const T* const q, const T* const t, + const Eigen::Matrix& P, + Eigen::Matrix& P_s); + template + static void LiftToSphere(const T* const params, + const Eigen::Matrix& p, + Eigen::Matrix& P); + + template + static void SphereToPlane(const T* const params, const Eigen::Matrix& P, + Eigen::Matrix& p); + + + void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const; + cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, + float fx = -1.0f, float fy = -1.0f, + cv::Size imageSize = cv::Size(0, 0), + float cx = -1.0f, float cy = -1.0f, + cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const; + + int parameterCount(void) const; + + const Parameters& getParameters(void) const; + void setParameters(const Parameters& parameters); + + void readParameters(const std::vector& parameterVec); + void writeParameters(std::vector& parameterVec) const; + + void writeParametersToYamlFile(const std::string& filename) const; + + std::string parametersToString(void) const; + +private: + Parameters mParameters; + + double m_inv_scale; +}; + +typedef std::shared_ptr OCAMCameraPtr; +typedef std::shared_ptr OCAMCameraConstPtr; + +template +void +OCAMCamera::LiftToSphere(const T* const params, + const Eigen::Matrix& p, + Eigen::Matrix& P) +{ + T c = params[0]; + T d = params[1]; + T e = params[2]; + T cc[2] = { params[3], params[4] }; + T poly[SCARAMUZZA_POLY_SIZE]; + for (int i=0; i < SCARAMUZZA_POLY_SIZE; i++) + poly[i] = params[5+i]; + + // Relative to Center + T p_2d[2]; + p_2d[0] = T(p(0)); + p_2d[1] = T(p(1)); + + T xc[2] = { p_2d[0] - cc[0], p_2d[1] - cc[1]}; + + T inv_scale = T(1.0) / (c - d * e); + + // Affine Transformation + T xc_a[2]; + + xc_a[0] = inv_scale * (xc[0] - d * xc[1]); + xc_a[1] = inv_scale * (-e * xc[0] + c * xc[1]); + + T norm_sqr = xc_a[0] * xc_a[0] + xc_a[1] * xc_a[1]; + T phi = sqrt(norm_sqr); + T phi_i = T(1.0); + T z = T(0.0); + + for (int i = 0; i < SCARAMUZZA_POLY_SIZE; i++) + { + if (i!=1) { + z += phi_i * poly[i]; + } + phi_i *= phi; + } + + T p_3d[3]; + p_3d[0] = xc[0]; + p_3d[1] = xc[1]; + p_3d[2] = -z; + + T p_3d_norm_sqr = p_3d[0] * p_3d[0] + p_3d[1] * p_3d[1] + p_3d[2] * p_3d[2]; + T p_3d_norm = sqrt(p_3d_norm_sqr); + + P << p_3d[0] / p_3d_norm, p_3d[1] / p_3d_norm, p_3d[2] / p_3d_norm; +} + +template +void OCAMCamera::SphereToPlane(const T* const params, const Eigen::Matrix& P, + Eigen::Matrix& p) { + T P_c[3]; + { + P_c[0] = T(P(0)); + P_c[1] = T(P(1)); + P_c[2] = T(P(2)); + } + + T c = params[0]; + T d = params[1]; + T e = params[2]; + T xc[2] = {params[3], params[4]}; + + T inv_poly[SCARAMUZZA_INV_POLY_SIZE]; + for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++) + inv_poly[i] = params[5 + SCARAMUZZA_POLY_SIZE + i]; + + T norm_sqr = P_c[0] * P_c[0] + P_c[1] * P_c[1]; + T norm = T(0.0); + if (norm_sqr > T(0.0)) norm = sqrt(norm_sqr); + + T theta = atan2(-P_c[2], norm); + T rho = T(0.0); + T theta_i = T(1.0); + + for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++) { + rho += theta_i * inv_poly[i]; + theta_i *= theta; + } + + T invNorm = T(1.0) / norm; + T xn[2] = {P_c[0] * invNorm * rho, P_c[1] * invNorm * rho}; + + p(0) = xn[0] * c + xn[1] * d + xc[0]; + p(1) = xn[0] * e + xn[1] + xc[1]; +} +} + +#endif diff --git a/camodocal/gpl/gpl.cc b/camodocal/gpl/gpl.cc new file mode 100644 index 0000000..db135b0 --- /dev/null +++ b/camodocal/gpl/gpl.cc @@ -0,0 +1,807 @@ +#include "../gpl/gpl.h" + +#include +#ifdef _WIN32 +#include +#else +#include +#endif + + +// source: https://stackoverflow.com/questions/5167269/clock-gettime-alternative-in-mac-os-x +#ifdef __APPLE__ +#include +#define ORWL_NANO (+1.0E-9) +#define ORWL_GIGA UINT64_C(1000000000) + +static double orwl_timebase = 0.0; +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(&tb); + orwl_timebase = tb.numer; + orwl_timebase /= tb.denom; + orwl_timestart = mach_absolute_time(); + } + struct timespec t; + double diff = (mach_absolute_time() - orwl_timestart) * orwl_timebase; + t.tv_sec = diff * ORWL_NANO; + t.tv_nsec = diff - (t.tv_sec * ORWL_GIGA); + return t; +} +#endif // __APPLE__ + + +const double WGS84_A = 6378137.0; +const double WGS84_ECCSQ = 0.00669437999013; + +// Windows lacks fminf +#ifndef fminf +#define fminf(x, y) (((x) < (y)) ? (x) : (y)) +#endif + +namespace camodocal +{ + + 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)); + } + + double d2r(double deg) + { + return deg / 180.0 * M_PI; + } + + float d2r(float deg) + { + return deg / 180.0f * M_PI; + } + + double r2d(double rad) + { + return rad / M_PI * 180.0; + } + + float r2d(float rad) + { + return rad / M_PI * 180.0f; + } + + double sinc(double theta) + { + return sin(theta) / theta; + } + + 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; + } + } + } + + 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]; + + return true; + } + else if (name.compare("autumn") == 0) + { + float* color = colormapAutumn[idx]; + + r = color[0]; + g = color[1]; + b = color[2]; + + return true; + } + + 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 cells; + + 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 err = dx - dy; + + while (1) + { + cells.push_back(cv::Point2i(x0, y0)); + + if (x0 == x1 && y0 == y1) + { + break; + } + + int e2 = 2 * err; + if (e2 > -dy) + { + err -= dy; + x0 += sx; + } + if (e2 < dx) + { + err += dx; + y0 += sy; + } + } + + 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; + } + + 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; + } + + 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 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; + + 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; + } + + // 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 + } + + 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; + } +} diff --git a/camodocal/gpl/gpl.h b/camodocal/gpl/gpl.h new file mode 100644 index 0000000..6103ba7 --- /dev/null +++ b/camodocal/gpl/gpl.h @@ -0,0 +1,114 @@ +#ifndef GPL_H +#define GPL_H + +#include +#include +#include + +#ifndef M_PI +const double M_PI = 3.1415926535; +#endif +namespace camodocal +{ + +template +const T clamp(const T& v, const T& a, const T& b) +{ + return std::min(b, std::max(a, v)); +} + +double hypot3(double x, double y, double z); +float hypot3f(float x, float y, float z); + +template +const T normalizeTheta(const T& theta) +{ + T normTheta = theta; + + while (normTheta < - M_PI) + { + normTheta += 2.0 * M_PI; + } + while (normTheta > M_PI) + { + normTheta -= 2.0 * M_PI; + } + + return normTheta; +} + +double d2r(double deg); +float d2r(float deg); +double r2d(double rad); +float r2d(float rad); + +double sinc(double theta); + +template +const T square(const T& x) +{ + return x * x; +} + +template +const T cube(const T& x) +{ + return x * x * x; +} + +template +const T random(const T& a, const T& b) +{ + return static_cast(rand()) / RAND_MAX * (b - a) + a; +} + +template +const T randomNormal(const T& sigma) +{ + T x1, x2, w; + + do + { + x1 = 2.0 * random(0.0, 1.0) - 1.0; + x2 = 2.0 * random(0.0, 1.0) - 1.0; + w = x1 * x1 + x2 * x2; + } + while (w >= 1.0 || w == 0.0); + + w = sqrt((-2.0 * log(w)) / w); + + return x1 * w * sigma; +} + +unsigned long long timeInMicroseconds(void); + +double timeInSeconds(void); + +void colorDepthImage(cv::Mat& imgDepth, + cv::Mat& imgColoredDepth, + float minRange, float maxRange); + +bool colormap(const std::string& name, unsigned char idx, + float& r, float& g, float& b); + +std::vector bresLine(int x0, int y0, int x1, int y1); +std::vector bresCircle(int x0, int y0, int r); + +void fitCircle(const std::vector& points, + double& centerX, double& centerY, double& radius); + +std::vector intersectCircles(double x1, double y1, double r1, + double x2, double y2, double r2); + +void LLtoUTM(double latitude, double longitude, + double& utmNorthing, double& utmEasting, + std::string& utmZone); +void UTMtoLL(double utmNorthing, double utmEasting, + const std::string& utmZone, + double& latitude, double& longitude); + +long int timestampDiff(uint64_t t1, uint64_t t2); + +} + +#endif diff --git a/config/carla/cam0_pinhole.yaml b/config/carla/cam0_pinhole.yaml new file mode 100644 index 0000000..0044fb0 --- /dev/null +++ b/config/carla/cam0_pinhole.yaml @@ -0,0 +1,16 @@ +%YAML:1.0 +--- +model_type: PINHOLE +camera_name: camera +image_width: 1024 +image_height: 768 +distortion_parameters: + k1: 0 + k2: 0 + p1: 0 + p2: 0 +projection_parameters: + fx: 886.8100 + fy: 886.8100 + cx: 512 + cy: 384 diff --git a/config/carla/cam1_pinhole.yaml b/config/carla/cam1_pinhole.yaml new file mode 100644 index 0000000..0044fb0 --- /dev/null +++ b/config/carla/cam1_pinhole.yaml @@ -0,0 +1,16 @@ +%YAML:1.0 +--- +model_type: PINHOLE +camera_name: camera +image_width: 1024 +image_height: 768 +distortion_parameters: + k1: 0 + k2: 0 + p1: 0 + p2: 0 +projection_parameters: + fx: 886.8100 + fy: 886.8100 + cx: 512 + cy: 384 diff --git a/config/carla/tracker.yaml b/config/carla/tracker.yaml new file mode 100644 index 0000000..359385d --- /dev/null +++ b/config/carla/tracker.yaml @@ -0,0 +1,52 @@ +%YAML:1.0 +--- + +imu_topic: "/imu0" +image0_topic: "/cam0/image_raw" +image1_topic: "/cam1/image_raw" + +cam0_calib: "cam0_pinhole.yaml" +cam1_calib: "cam1_pinhole.yaml" + +IPM_WIDTH: 400 +IPM_HEIGHT: 1000 +IPM_RESO: 0.015 + +priori_alpha: 0.0 +priori_theta: 0.0 +priori_H: 1.84 + +body_T_cam0: !!opencv-matrix + rows: 4 + cols: 4 + dt: d + data: [ 1, 0, 0, 0, + 0, 0, 1, 0, + 0, -1, 0, 0, + 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000] + + +body_T_cam1: !!opencv-matrix + rows: 4 + cols: 4 + dt: d + data: [ 1, 0, 0, 0, + 0, 0, 1, 0, + 0, -1, 0, 0, + 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000] + + +enable_pitch_comp: 1 +pitch_comp_windowsize: 4.0 + +feature_mode: 1 # 0: min dist-based; 1: grid-based + +# min dist-based +min_dist_ground: 30 +max_cnt_ground: 40 + +# grid-based +grid_row: 8 +grid_col: 4 +grid_min_feature_num: 2 +grid_max_feature_num: 3 diff --git a/config/realworld/cam0_pinhole.yaml b/config/realworld/cam0_pinhole.yaml new file mode 100644 index 0000000..09980ca --- /dev/null +++ b/config/realworld/cam0_pinhole.yaml @@ -0,0 +1,18 @@ +%YAML:1.0 +--- +model_type: PINHOLE +camera_name: camera +image_width: 1024 +image_height: 768 +distortion_parameters: + k1: -0.13095809 + k2: 0.06640391 + p1: -0.00094794 + p2: 0.0003389 +projection_parameters: + fx: 890.21388839 + fy: 889.56330572 + cx: 512.88196119 + cy: 381.38486858 +# cam0_intrinsics=" 890.21388839 889.56330572 512.88196119 381.38486858" +# cam0_distortion_coeffs="-0.13095809 0.06640391 -0.00094794 0.0003389" \ No newline at end of file diff --git a/config/realworld/cam1_pinhole.yaml b/config/realworld/cam1_pinhole.yaml new file mode 100644 index 0000000..9f2f141 --- /dev/null +++ b/config/realworld/cam1_pinhole.yaml @@ -0,0 +1,19 @@ +%YAML:1.0 +--- +model_type: PINHOLE +camera_name: camera +image_width: 1024 +image_height: 768 +distortion_parameters: + k1: -0.13523822 + k2: 0.07446051 + p1: -0.00159362 + p2: 0.00197937 +projection_parameters: + fx: 889.59227008 + fy: 889.28465437 + cx: 519.11179132 + cy: 392.62683405 + +# cam1_distortion_coeffs="-0.13523822 0.07446051 -0.00159362 0.00197937" +# cam1_intrinsics=" 887.59227008 887.28465437 519.11179132 392.62683405" \ No newline at end of file diff --git a/config/realworld/tracker.yaml b/config/realworld/tracker.yaml new file mode 100644 index 0000000..45669d8 --- /dev/null +++ b/config/realworld/tracker.yaml @@ -0,0 +1,50 @@ +%YAML:1.0 +--- + +imu_topic: "/imu0" +image0_topic: "/cam0/image_raw" +image1_topic: "/cam1/image_raw" + +cam0_calib: "cam0_pinhole.yaml" +cam1_calib: "cam1_pinhole.yaml" + +IPM_WIDTH: 400 +IPM_HEIGHT: 1000 +IPM_RESO: 0.015 + +priori_alpha: -0.15 +priori_theta: -1.15 +priori_H: 1.7803 + +body_T_cam0: !!opencv-matrix + rows: 4 + cols: 4 + dt: d + data: [ 0.99988370,-0.00563944,-0.01418468,-0.15590000, + 0.01424932,0.01159187,0.99983149,0.63466000, + -0.00547407,-0.99991712,0.01167088,0.04605000, + 0.00000000,0.00000000,0.00000000,1.00000000] + +body_T_cam1: !!opencv-matrix + rows: 4 + cols: 4 + dt: d + data: [ 0.99996024,-0.00201794,-0.00863170,0.14395000, + 0.00866529,0.01727883,0.99981269,0.63759000, + -0.00186842,-0.99984820,0.01729564,0.04528000, + 0.00000000,0.00000000,0.00000000,1.00000000] + +enable_pitch_comp: 1 +pitch_comp_windowsize: 4.0 + +feature_mode: 1 # 0: min dist-based; 1: grid-based + +# min dist-based +min_dist_ground: 30 # 20 for highway +max_cnt_ground: 40 + +# grid-based +grid_row: 8 +grid_col: 4 +grid_min_feature_num: 2 +grid_max_feature_num: 3 diff --git a/config/tracker.rviz b/config/tracker.rviz new file mode 100644 index 0000000..bc2564c --- /dev/null +++ b/config/tracker.rviz @@ -0,0 +1,207 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Grid1 + - /Odometry1 + - /Odometry1/Shape1 + Splitter Ratio: 0.5 + Tree Height: 360 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Name: Time + SyncMode: 0 + SyncSource: "" +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 20 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 100 + Reference Frame: + Value: true + - Angle Tolerance: 0.10000000149011612 + Class: rviz/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Keep: 100 + Name: Odometry + Position Tolerance: 0.10000000149011612 + Queue Size: 10 + Shape: + Alpha: 1 + Axes Length: 10 + Axes Radius: 2 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Axes + Topic: /pose_gt + Unreliable: false + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /ground_tracker_node/img_track + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /ground_tracker_node/ipm_track + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /ground_tracker_node/ipm_track + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + - Class: rviz/Image + Enabled: false + Image Topic: /ground_tracker_node/img_track + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 3382.865478515625 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: -41.67063903808594 + Y: 53.891353607177734 + Z: -7.665790557861328 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.9497965574264526 + Target Frame: + Yaw: 2.3853983879089355 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1495 + Hide Left Dock: false + Hide Right Dock: false + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000001b70000053dfc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000001f1000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000546000000160000001600fffffffb0000000a0049006d0061006700650000000562000000160000001600fffffffb0000000a0049006d0061006700650100000232000003460000001600ffffff000000010000033d0000053dfc0200000006fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b00000147000000a000fffffffb0000000a0049006d0061006700650100000188000003f00000001600fffffffb0000000a0049006d0061006700650100000224000001a60000000000000000fb0000000a0049006d00610067006501000003d0000001a80000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009f40000003efc0100000002fb0000000800540069006d00650100000000000009f40000030700fffffffb0000000800540069006d00650100000000000004500000000000000000000004f40000053d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2548 + X: -32 + Y: -32 diff --git a/example/dataset_utils.hpp b/example/dataset_utils.hpp new file mode 100644 index 0000000..8648d5b --- /dev/null +++ b/example/dataset_utils.hpp @@ -0,0 +1,63 @@ +#ifndef DATASET_UTILS_HPP +#define DATASET_UTILS_HPP + +#include +#include +#include +#include +#include + +using namespace std; + +map load_camstamp(const string & filename) +{ + map camstamp; + string line; + stringstream ss; + ifstream ifs(filename); + + double sow; string imagename; + while (ifs.good()) + { + getline(ifs, line); + if (line.size() < 1) continue; + if (line[0] == '#') continue; + for (int i = 0; i < line.size(); i++) + if (line[i] == ',') line[i] = ' '; + ss.clear(); ss.str(""); + ss << line; + ss >> sow >> imagename; + if(sow>1e9) sow/=1e9; + camstamp[sow] = imagename; + } + + return camstamp; +} + +map load_trajectory(const string & filename) +{ + map traj; + string line; + stringstream ss; + ifstream ifs(filename); + + double tt, x, y, z, qw, qx, qy, qz; + while (ifs.good()) + { + getline(ifs, line); + if (line.size() < 1) continue; + if (line[0] == '#') continue; + ss.clear(); ss.str(""); + ss << line; + ss >> tt >> x >> y >> z >> qw >> qx >> qy >> qz; + if(tt>1e9) tt/=1e9; + + Eigen::Matrix4d pose = Eigen::Matrix4d::Identity(); + pose.block<3,3>(0,0) = Eigen::Quaterniond(qw,qx,qy,qz).toRotationMatrix(); + pose.block<3,1>(0,3) = Eigen::Vector3d(x,y,z); + traj[tt] = pose; + } + return traj; +} + +#endif \ No newline at end of file diff --git a/example/track_dataset.cpp b/example/track_dataset.cpp new file mode 100644 index 0000000..0ff9ada --- /dev/null +++ b/example/track_dataset.cpp @@ -0,0 +1,85 @@ +#include "ground_tracker.h" +#include "dataset_utils.hpp" +#include "simple_compensation.hpp" + +#ifdef DEBUG_PITCH_COMPENSATION + std::ofstream ofs_debug_pitch("pitch.log"); +#endif + +int main(int argc,char *argv[]) +{ + if(argc<5 || argc>5) + { + cerr << endl + << "Usage: track_dataset path_yaml path_dataset path_stamp path_odom" + << endl; + return 1; + } + string path_yaml(argv[1]); + string path_dataset(argv[2]); + string path_stamp(argv[3]); + string path_odom(argv[4]); + + + gv::CameraConfig config(path_yaml); + gv::GroundTracker traker(config); + + cv::FileStorage fsSettings(path_yaml, cv::FileStorage::READ); + bool enable_pitch_comp = (int)fsSettings["enable_pitch_comp"]; + double pitch_comp_windowsize = fsSettings["pitch_comp_windowsize"]; + fsSettings.release(); + + std::map all_imu_pitch; + + auto traj = load_trajectory(path_odom); + auto stamp = load_camstamp(path_stamp); + + printf("odometry: %20.4f~%20.4f\n",traj.begin()->first,traj.rbegin()->first); + printf("camstamp: %20.4f~%20.4f\n",stamp.begin()->first,stamp.rbegin()->first); + + Eigen::Matrix4d last_pose = Eigen::Matrix4d::Identity(); + + for (auto stamp_iter = stamp.begin(); stamp_iter != stamp.end(); stamp_iter++) + { + double tt = stamp_iter->first; + auto pose_iter = traj.lower_bound(tt - 1e-3); + if (fabs(pose_iter->first - tt) > 1e-2) continue; + + // Using relative pose prediction to assist feature tracking + // (using GT poses in this example, but could be obtained via + // real-time estimation of VIO/VINS) + Eigen::Matrix4d Twik_1 = last_pose; + Eigen::Matrix4d Twik = pose_iter->second; + Eigen::Matrix4d Tckck_1 = (Twik * config.Tic).inverse() * (Twik_1 * config.Tic); + + // Here we use pre-calibrated camera-ground geometry. While + // this could be online estimated and continuously refined + // in a VIO estimator. + gv::CameraGroundGeometry cg = config.cg; + + // Calculate temporary camera-ground geometry considering + // high-frequency pitch compenstation. + if(enable_pitch_comp) + { + all_imu_pitch[tt] = gv::m2att(Twik.block<3,3>(0,0)).x() * 180 / M_PI; + double comp = simple_pitch_compenstation(tt, all_imu_pitch, pitch_comp_windowsize); + cg.update(cg.getAlpha(), cg.getTheta()+comp); + +#ifdef DEBUG_PITCH_COMPENSATION + ofs_debug_pitch<second); + cv::cvtColor(img, img, cv::COLOR_BGR2GRAY); + traker.trackImage(tt, img, 10.0, cg, Tckck_1,true); + // traker.trackImagePerspective(tt, img, 10.0, cg, Tckck_1,true); + + last_pose = pose_iter->second; + + } + return 0; +} + diff --git a/include/ground_tracker.h b/include/ground_tracker.h new file mode 100644 index 0000000..e6a3dcb --- /dev/null +++ b/include/ground_tracker.h @@ -0,0 +1,114 @@ +#ifndef GROUND_TRACKER_H +#define GROUND_TRACKER_H + +#include +#include +#include +#include +#include "gv_utils.h" +#include "ipm_processer.h" +#include + +namespace gv +{ + + using namespace std; + using namespace Eigen; + + enum GroundFeatureMode + { + MINDIST = 0, // 1) Min dist-based feature extraction, referring to VINS-Mono. + GRID = 1 // 2) Grid-based feature extraction, inspried by https://github.com/KumarRobotics/msckf_vio. + }; + + /** + * @brief Ground tracker for ground feature extraction and tracking. + * + * The ground tracker could work in either IPM or perspective mode. + * In both modes, the camera-ground geometry is used for feature prediction. + * + */ + class GroundTracker + { + public: + GroundTracker(CameraConfig conf); + ~GroundTracker(){}; + + /** + * Main function for ground feature tracking via IPM. + * + * @param _cur_time Current timestamp. + * @param _img Input image. + * @param threshold Outlier threshold to check optical flow with prediction. + * @param cg Current camera-ground geometry. + * @param Tckck_1 Relative pose of the camera. + * @param show_track Enable cv::imshow(). + * @return Features (normalized coords in the perspective image) in the current frame. + * + */ + map>>> trackImage(double _cur_time, + const cv::Mat &_img, const double &threshold, const CameraGroundGeometry &cg, + const Eigen::Matrix4d &Tckck_1, const bool &show_track = true); + + /** + * Main function for ground feature tracking on the perspective image (without IPM). + * + * @param _cur_time Current timestamp. + * @param _img Input image. + * @param threshold Outlier threshold to check optical flow with prediction. + * @param cg Current camera-ground geometry. + * @param Tckck_1 Relative pose of the camera. + * @param show_track Enable cv::imshow(). + * @return Features (normalized coords in the perspective image) in the current frame. + * + */ + map>>> trackImagePerspective(double _cur_time, + const cv::Mat &_img, const double &threshold, const CameraGroundGeometry &cg, + const Eigen::Matrix4d &Tckck_1, const bool &show_track = true); + + bool inBorder(const cv::Point2f &pt); + void reduceVector(vector &v, vector status); + void reduceVector(vector &v, vector status); + double distance(cv::Point2f &pt1, cv::Point2f &pt2); + + int row, col; + cv::Mat mask; + cv::Mat cur_img_semantic; + cv::Mat prev_img, cur_img, cur_img_show; + cv::Mat prev_ipm, cur_ipm, cur_ipm_show; + vector n_pts; + vector predict_pts; + vector prev_pts, cur_pts; + vector ids; + vector track_cnt; + double prev_time; + int n_id = 1000000000; // to identify the ground features + + CameraConfig config; + shared_ptr ipmproc; + cv::Mat ground_mask; + + CameraGroundGeometry prev_cg; + CameraGroundGeometry cur_cg; + + GroundFeatureMode feature_mode = GroundFeatureMode::MINDIST; + + // Min dist-based feature extraction. + int MAX_CNT = 30; + int MIN_DIST = 20; + + // Grid-based feature extraction. + cv::Ptr detector_ptr; + int GRID_ROW = 8; + int GRID_COL = 4; + int GRID_MIN_FEATURE_NUM = 2; + int GRID_MAX_FEATURE_NUM = 3; + int GRID_HEIGHT; + int GRID_WIDTH; + map>> feature2grid( + vector pts, vector ids); + + }; +} + +#endif \ No newline at end of file diff --git a/include/gv_utils.h b/include/gv_utils.h new file mode 100644 index 0000000..3813ce9 --- /dev/null +++ b/include/gv_utils.h @@ -0,0 +1,103 @@ +#ifndef GV_UTILS_H +#define GV_UTILS_H + +#include +#include +#include +#include +#include + +namespace gv +{ + /** + * Rotation matrix to euler angles. Assuming RFU (right-forward-up) and PRY (pitch-roll-yaw) conventions. + * + * @param att Euler angles. + * @return Rotation matrix. + */ + Eigen::Matrix3d a2mat(const Eigen::Vector3d &att); + + /** + * Euler angles to rotation matrix. Assuming RFU (right-forward-up) and PRY (pitch-roll-yaw) conventions. + * + * @param m Rotation matrix. + * @return Euler angles. + */ + Eigen::Vector3d m2att(const Eigen::Matrix3d &m); + + /** + * @brief Camera-ground geometry which parameterizes the ground plane in the camera frame. + * + * Note that there are different representations of the camera-ground geometry. + * 1) A height with a two-step rotation (alpha and theta) to make the camera frame parallel with the ground. + * 2) A height with a rotation matrix (R^c_cg, where cg is the virtual camera frame). + * 3) A height with a normal vector of the ground plane. + * + */ + class CameraGroundGeometry + { + public: + CameraGroundGeometry(){}; + CameraGroundGeometry(const double &a, const double &t, const double &h); + CameraGroundGeometry(const Eigen::Vector3d &n, const double &h); + CameraGroundGeometry(const Eigen::Matrix3d &R_c_cg, const double &h); + + double getAlpha() const; + double getTheta() const; + double getH() const; + Eigen::Matrix3d getR() const; // R^{camera}_{virtual camera} + Eigen::Vector3d getN() const; // n^{camera}_{ground norm} + + void update(const double &a, const double &t, const double &h = -1); + void update(const Eigen::Vector3d &n, const double &h = -1); + void update(const Eigen::Matrix3d &R_c_cg, const double &h = -1); + bool operator==(const CameraGroundGeometry c1) const; + + private: + double _alpha; + double _theta; + double _h; + + Eigen::Matrix3d _R_c_cg; // R^{camera}_{virtual camera} + + Eigen::Vector3d _n; // n^{camera}_{ground norm} + }; + + + /** + * @brief Configuration struct which contains camera parameters and IPM settings. + * + * The camera models from camodocal are used. + * + */ + struct CameraConfig + { + public: + CameraConfig(){}; + CameraConfig(const std::string &config_file); + + public: + // IPM settings. + int IPM_HEIGHT; + int IPM_WIDTH; + float IPM_RESO; + int RAW_RESIZE; + + // Extrinsic parameters. + Eigen::Matrix3d Ric; + Eigen::Vector3d tic; + Eigen::Matrix4d Tic; + + camodocal::CameraPtr camera; // Camera model. + CameraGroundGeometry cg; // Prior camera-ground geometry. + + int feature_mode; // 0: min dist-based; 1: grid-based + int min_dist_ground; + int max_cnt_ground; + int grid_row; + int grid_col; + int grid_min_feature_num; + int grid_max_feature_num; + }; +} +#endif \ No newline at end of file diff --git a/include/ipm_processer.h b/include/ipm_processer.h new file mode 100644 index 0000000..d85f12f --- /dev/null +++ b/include/ipm_processer.h @@ -0,0 +1,93 @@ +#ifndef IPM_PROCESSER_H +#define IPM_PROCESSER_H + +#include "gv_utils.h" +#include +#include + +namespace gv +{ + enum IPMType + { + NORMAL = 0 + }; + + class IPMProcesser + { + public: + IPMProcesser(CameraConfig conf, IPMType ipm_type); + ~IPMProcesser(); + + /** + * Update the camera-ground geometric parameters and generate the IPM mapping matrix. + * + * @param cg Camera-ground geometric parameters. + * @return 0 if no exception. + */ + int updateCameraGroundGeometry(CameraGroundGeometry cg); + + /** + * Generate the IPM image. + * + * @param mm Input perspective image. + * @param need_undistort If the input image needs undistortion or has been undistorted beforehand. + * @param mm_undist Output the undistorted perspective image. + * @return IPM image. + */ + cv::Mat genIPM(cv::Mat mm, bool need_undistort = true, cv::Mat mm_undist = cv::Mat()) const; + + /** + * Auxiliary function for image undistortion, since the IPMprecessor handles all camera parameters. + * + * @param mm Input perspective image. + * @return Undistorted perspective image. + */ + cv::Mat genUndistortedImage(cv::Mat mm) const; + cv::Vec4d getUndistortedIntrinsics(); + + /** + * Point-to-point transformation. + * + * @param p 2-D point or 3-D point. + * @param cg_temp Optional camera-ground geometry for temporary use. + * @return Transformed point. + */ + cv::Point2f IPM2Perspective(cv::Point2f p, CameraGroundGeometry *cg_temp = nullptr) const; + cv::Point2f Perspective2IPM(cv::Point2f p, CameraGroundGeometry *cg_temp = nullptr) const; + Eigen::Vector3d Perspective2Metric(cv::Point2f p, CameraGroundGeometry *cg_temp = nullptr) const; + Eigen::Vector3d NormalizedPerspective2Metric(Eigen::Vector3d p, CameraGroundGeometry *cg_temp = nullptr) const; + Eigen::Vector3d IPM2Metric(cv::Point2f p, CameraGroundGeometry *cg_temp = nullptr) const; + cv::Point2f Metric2IPM(Eigen::Vector3d p, CameraGroundGeometry *cg_temp = nullptr) const; + + /** + * Generate a coarse mask for ground feature extraction, according to CameraConfig. + * + * @return 0-1 Mask; + */ + cv::Mat guessGroundROI(); + + private: + int updateIPMMap(); + + CameraConfig _config; + + IPMType _ipm_type; + double _fx, _fy, _cx, _cy, _h, _w; // for undistorted images + cv::Mat _undist_M0, _undist_M1; + + // for pointwise IPM mapping (only for debug use) + float *_IPM_mapx_array = nullptr; + float *_IPM_mapy_array = nullptr; + cv::Mat _IPM_mapx; + cv::Mat _IPM_mapy; + + // for perspective-matrix-based IPM mapping + cv::Mat _IPM_transform; + + // online camera-ground parameters, updated by updateCameraGroundGeometry() + Eigen::Matrix3d _Rc0c1; // R^{camera}_{virtual camera} + double _d; + }; + +} +#endif \ No newline at end of file diff --git a/include/simple_compensation.hpp b/include/simple_compensation.hpp new file mode 100644 index 0000000..ba9c683 --- /dev/null +++ b/include/simple_compensation.hpp @@ -0,0 +1,101 @@ +#pragma once + +#include +#include + +using namespace std; + +double simple_pitch_compenstation(double tt, map history_pitch, double duration = 2.0); +Eigen::VectorXd polyfit1d(Eigen::VectorXd x, Eigen::VectorXd y, int dim, Eigen::VectorXd weight = Eigen::VectorXd::Zero(0)); +double modelPolyfit1d(Eigen::VectorXd param, double x); + +/** + * Simple implementation of temporal pitch compensation of camera-ground geometry + * based on IMU attitudes. + * + * @param tt Current time. + * @param history_pitch History IMU pitch estimates (in deg). + * @param duration Time window considered for poly-fitting. + * @return Pitch compenstation (in deg). + */ + +double simple_pitch_compenstation(double tt, map history_pitch, double duration) +{ + vector pitch_window; + vector t_window; + + double t_start = (tt - duration); + for (auto iter = history_pitch.rbegin(); iter != history_pitch.rend(); iter++) + { + if (tt - iter->first < duration) + { + t_window.push_back(iter->first - t_start); + pitch_window.push_back(iter->second); + } + } + if (pitch_window.size() < 10) + return 0.0; + else + { + Eigen::VectorXd t_v = Eigen::Map(t_window.data(), t_window.size()); + Eigen::VectorXd pitch_v = Eigen::Map(pitch_window.data(), pitch_window.size()); + Eigen::VectorXd param = polyfit1d(t_v, pitch_v, 2); + double pitch_trend = modelPolyfit1d(param, tt - t_start); + return pitch_trend - pitch_window.front(); + } +} + +Eigen::VectorXd polyfit1d(Eigen::VectorXd x, Eigen::VectorXd y, int dim, Eigen::VectorXd weight) +{ + Eigen::VectorXd param = Eigen::VectorXd::Zero(dim + 1); + Eigen::MatrixXd H(x.rows(), dim + 1); + Eigen::VectorXd r(x.rows()); + Eigen::MatrixXd A(dim + 1, dim + 1); + Eigen::VectorXd b(dim + 1); + if (weight.rows() == 0) + { + weight = Eigen::VectorXd::Zero(x.rows()); + weight.setConstant(1.0); + } + for (int i_iter = 0; i_iter < 2; i_iter++) + { + H.setZero(); + r.setZero(); + for (int i = 0; i < x.size(); i++) + { + double y_est = 0.0; + for (int j = 0; j < dim + 1; j++) + { + H(i, j) = pow(x(i), j); + y_est += pow(x(i), j) * param(j); + } + r(i) = (y_est - y(i)) * weight(i); + H.row(i) *= weight(i); + + // Hacky way to counter outliers. + // TODO + if (i_iter > 0) + { + double w; + if (fabs(r(i)) < 0.05) + w = 1.0; + else + w = 1 / (fabs(r(i)) / 0.05); + r(i) *= w; + H.row(i) *= w; + } + } + A = H.transpose() * H; + b = H.transpose() * r; + param -= A.inverse() * b; + } + return param; +} + +double modelPolyfit1d(Eigen::VectorXd param, double x) +{ + double y = 0.0; + for (int j = 0; j < param.rows(); j++) + y += param(j) * pow(x, j); + return y; +} diff --git a/launch/track_carla_example.launch b/launch/track_carla_example.launch new file mode 100644 index 0000000..7c7f9d4 --- /dev/null +++ b/launch/track_carla_example.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + \ No newline at end of file diff --git a/launch/track_realworld_example.launch b/launch/track_realworld_example.launch new file mode 100644 index 0000000..ec590a1 --- /dev/null +++ b/launch/track_realworld_example.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + \ No newline at end of file diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..83ac502 --- /dev/null +++ b/package.xml @@ -0,0 +1,28 @@ + + + gv_tools + 0.0.1 + Ground-vision toolkit. + + Yuxuan Zhou + + GPLv3 + + catkin + roscpp + rospy + std_msgs + cv_bridge + image_transport + tf + sensor_msgs + roscpp + rospy + std_msgs + cv_bridge + image_transport + tf + sensor_msgs + + + \ No newline at end of file diff --git a/pics/1.gif b/pics/1.gif new file mode 100644 index 0000000..1302c01 Binary files /dev/null and b/pics/1.gif differ diff --git a/pics/2.gif b/pics/2.gif new file mode 100644 index 0000000..dbb2641 Binary files /dev/null and b/pics/2.gif differ diff --git a/pics/abstract.png b/pics/abstract.png new file mode 100644 index 0000000..5ac6831 Binary files /dev/null and b/pics/abstract.png differ diff --git a/pics/abstract.svg b/pics/abstract.svg new file mode 100644 index 0000000..2b72545 --- /dev/null +++ b/pics/abstract.svg @@ -0,0 +1,750 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + θ + h + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + α + + + + + + + + + + + + + + + diff --git a/pics/r_a.png b/pics/r_a.png new file mode 100644 index 0000000..bb47261 Binary files /dev/null and b/pics/r_a.png differ diff --git a/pics/r_b.png b/pics/r_b.png new file mode 100644 index 0000000..acd0679 Binary files /dev/null and b/pics/r_b.png differ diff --git a/pics/r_c.png b/pics/r_c.png new file mode 100644 index 0000000..d6a243e Binary files /dev/null and b/pics/r_c.png differ diff --git a/pics/r_d.png b/pics/r_d.png new file mode 100644 index 0000000..02db629 Binary files /dev/null and b/pics/r_d.png differ diff --git a/pics/r_e.png b/pics/r_e.png new file mode 100644 index 0000000..09a7ce5 Binary files /dev/null and b/pics/r_e.png differ diff --git a/pics/r_f.png b/pics/r_f.png new file mode 100644 index 0000000..e39e772 Binary files /dev/null and b/pics/r_f.png differ diff --git a/pics/s_a.png b/pics/s_a.png new file mode 100644 index 0000000..b40ca90 Binary files /dev/null and b/pics/s_a.png differ diff --git a/pics/s_b.png b/pics/s_b.png new file mode 100644 index 0000000..837f770 Binary files /dev/null and b/pics/s_b.png differ diff --git a/pics/whu.png b/pics/whu.png new file mode 100644 index 0000000..e216f84 Binary files /dev/null and b/pics/whu.png differ diff --git a/ros/ground_tracker_node.cpp b/ros/ground_tracker_node.cpp new file mode 100644 index 0000000..3428b3b --- /dev/null +++ b/ros/ground_tracker_node.cpp @@ -0,0 +1,135 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "ground_tracker.h" +#include "simple_compensation.hpp" + +using namespace std; + +queue> img_queue; +queue> pose_queue; + +ros::Subscriber sub_img; +ros::Subscriber sub_pose; +image_transport::Publisher pub_img_track; +image_transport::Publisher pub_ipm_track; +shared_ptr tracker; + +std::map all_imu_pitch; +bool enable_pitch_comp; +double pitch_comp_windowsize; +Eigen::Matrix4d last_pose; + +void img_callback(const sensor_msgs::ImageConstPtr &msg) +{ + cv::Mat mm = cv_bridge::toCvCopy(msg, "bgr8")->image; + img_queue.push(make_pair(msg->header.stamp.toSec(), mm)); + + // Note: For VIO implementation, process IMU data here. + // Feed the pose prediction into feature tracker. + + ros::Rate wait_for_pose(100); + while (pose_queue.empty() && ros::ok()) + { + ROS_WARN("Waiting for pose!!!"); + wait_for_pose.sleep(); + } + + // **Attention**: Assuming synced image and pose inputs here. + while (!pose_queue.empty() && !img_queue.empty()&& ros::ok()) + { + if (pose_queue.front().first <= img_queue.front().first - 1e-2) + pose_queue.pop(); + else if (pose_queue.front().first >= img_queue.front().first + 1e-2) + img_queue.pop(); + else + { + ROS_INFO("Get synced data!!! %.5lf %.5lf",pose_queue.front().first, img_queue.front().first); + + double tt = pose_queue.front().first; + cv::Mat img = img_queue.front().second; + Eigen::Matrix4d Twik = pose_queue.front().second; + Eigen::Matrix4d Twik_1 = last_pose; + Eigen::Matrix4d Tckck_1 = (Twik * tracker->config.Tic).inverse() * (Twik_1 * tracker->config.Tic); + + pose_queue.pop(); + img_queue.pop(); + + gv::CameraGroundGeometry cg = tracker->config.cg; + + // Calculate temporary camera-ground geometry considering + // high-frequency pitch compenstation + if (enable_pitch_comp) + { + all_imu_pitch[tt] = gv::m2att(Twik.block<3, 3>(0, 0)).x() * 180 / M_PI; + double comp = simple_pitch_compenstation(tt, all_imu_pitch, pitch_comp_windowsize); + cg.update(cg.getAlpha(), cg.getTheta() + comp); + } + // main feature tracking + cv::cvtColor(img, img, cv::COLOR_BGR2GRAY); + auto feature_frame = tracker->trackImage(tt, img, 10.0, cg, Tckck_1, false); + + // visualization + std_msgs::Header header; + header.stamp = ros::Time(tt); + header.frame_id = "cam0"; + sensor_msgs::ImagePtr img_show_msg = cv_bridge::CvImage(header, "bgr8", tracker->cur_img_show).toImageMsg(); + sensor_msgs::ImagePtr ipm_show_msg = cv_bridge::CvImage(header, "bgr8", tracker->cur_ipm_show).toImageMsg(); + pub_img_track.publish(img_show_msg); + pub_ipm_track.publish(ipm_show_msg); + + last_pose = Twik; + } + } +} + +void imu_pose_callback(const nav_msgs::Odometry &msg) +{ + Eigen::Matrix4d pose = Eigen::Matrix4d::Identity(); + pose.block<3, 3>(0, 0) = Eigen::Quaterniond( + msg.pose.pose.orientation.w, + msg.pose.pose.orientation.x, + msg.pose.pose.orientation.y, + msg.pose.pose.orientation.z) + .toRotationMatrix(); + pose.block<3, 1>(0, 3) = Eigen::Vector3d(msg.pose.pose.position.x, + msg.pose.pose.position.y, + msg.pose.pose.position.z); + pose_queue.push(make_pair(msg.header.stamp.toSec(), pose)); +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "ground_tracker_node"); + ros::NodeHandle nh("~"); + + string config_path; + nh.getParam("config_path", config_path); + ROS_WARN("config_path: %s\n",config_path.c_str()); + cv::FileStorage fsSettings(config_path, cv::FileStorage::READ); + enable_pitch_comp = (int)fsSettings["enable_pitch_comp"]; + pitch_comp_windowsize = fsSettings["pitch_comp_windowsize"]; + fsSettings.release(); + + gv::CameraConfig config(config_path); + tracker = make_shared(config); + + ros::AsyncSpinner spinner(0); + spinner.start(); + + sub_img = nh.subscribe("/cam0/image_raw", 1, img_callback); + sub_pose = nh.subscribe("/pose_gt", 1, imu_pose_callback); + image_transport::ImageTransport it(nh); + pub_img_track = it.advertise("img_track", 2); + pub_ipm_track = it.advertise("ipm_track", 2); + + ros::waitForShutdown(); +} + diff --git a/src/ground_tracker.cpp b/src/ground_tracker.cpp new file mode 100644 index 0000000..736b5e6 --- /dev/null +++ b/src/ground_tracker.cpp @@ -0,0 +1,550 @@ +#include "ground_tracker.h" + +#define KLT_PATCH_SIZE 21 + +namespace gv +{ + GroundTracker::GroundTracker(CameraConfig conf) : config(conf) + { + ipmproc = shared_ptr(new IPMProcesser(conf, IPMType::NORMAL)); + switch(conf.feature_mode) + { + case 0:feature_mode= GroundFeatureMode::MINDIST; break; + case 1:feature_mode= GroundFeatureMode::GRID; break; + default: feature_mode= GroundFeatureMode::MINDIST; break; + } + ground_mask = cv::Mat(); + MIN_DIST = conf.min_dist_ground; + MAX_CNT = conf.max_cnt_ground; + + cur_cg = conf.cg; + prev_cg = conf.cg; // prior camera-ground geometry + + if (feature_mode == GroundFeatureMode::GRID) + { + GRID_ROW = config.grid_row; + GRID_COL = config.grid_col; + GRID_MIN_FEATURE_NUM = config.grid_min_feature_num; + GRID_MAX_FEATURE_NUM = config.grid_max_feature_num; + GRID_HEIGHT = config.IPM_HEIGHT / GRID_ROW; + GRID_WIDTH = config.IPM_WIDTH / GRID_COL; + detector_ptr = cv::FastFeatureDetector::create(5.0); + + } + }; + + map>>> GroundTracker::trackImage(double _cur_time, + const cv::Mat &_img, const double &threshold, const CameraGroundGeometry &cg, + const Eigen::Matrix4d &Tckck_1, const bool &show_track) + { + cur_pts.clear(); + cur_cg = cg; + + cv::Mat img_undist = ipmproc->genUndistortedImage(_img); + cv::Vec4d intrinsics = ipmproc->getUndistortedIntrinsics(); + + cur_img = img_undist; + + cv::Ptr clahe = cv::createCLAHE(3.0, cv::Size(8, 8)); + clahe->apply(cur_img, cur_img); + + // Add new features only in the top half region for good tracking. + mask = cv::Mat(config.IPM_HEIGHT, config.IPM_WIDTH, CV_8UC1); + mask.setTo(0); + mask(cv::Rect(0, 0, config.IPM_WIDTH, config.IPM_HEIGHT / 2)).setTo(255); + + if (show_track) + cv::imshow("mask", mask); + + if (!(cur_cg == prev_cg)) + { + ipmproc->updateCameraGroundGeometry(cur_cg); + } + cur_ipm = ipmproc->genIPM(cur_img); + + if (!cur_img_semantic.empty()) + { + vector img_channels_temp; + cv::split(cur_img_semantic, img_channels_temp); + cur_img_semantic = img_channels_temp[2]; + cur_img_semantic.setTo(255, cur_img_semantic == 7); + cur_img_semantic.setTo(255, cur_img_semantic == 6); + cur_img_semantic.setTo(255, cur_img_semantic == 20); + cv::Mat cur_ipm_semantic = ipmproc->genIPM(cur_img_semantic); + mask.setTo(0, cur_ipm_semantic != 255); + } + + // For visualization + cv::cvtColor(cur_ipm, cur_ipm_show, cv::COLOR_GRAY2BGR); + cv::cvtColor(img_undist, cur_img_show, cv::COLOR_GRAY2BGR); + + vector pred_pts; + vector prev_ipm_pts, cur_ipm_pts; + vector status; + vector err; + + if (prev_pts.size() > 0) + { + Eigen::Matrix3d Rckck_1 = Tckck_1.block<3, 3>(0, 0); + Eigen::Vector3d tckck_1 = Tckck_1.block<3, 1>(0, 3); + + // Predict tracked points + for (int i = 0; i < prev_pts.size(); i++) + { + cv::Point2f p_ipm = ipmproc->Perspective2IPM(prev_pts[i], &prev_cg); + prev_ipm_pts.push_back(p_ipm); + Eigen::Vector3d pc0k_1f = ipmproc->Perspective2Metric(prev_pts[i], &prev_cg); + Eigen::Vector3d pc0kf = Rckck_1 * pc0k_1f + tckck_1; + cv::Point2f uvc1kf = ipmproc->Metric2IPM(pc0kf); + pred_pts.push_back(uvc1kf); + } + + cur_ipm_pts = pred_pts; + + // Optical flow tracking + + if (threshold > 10) + cv::calcOpticalFlowPyrLK(prev_ipm, cur_ipm, prev_ipm_pts, cur_ipm_pts, status, err, cv::Size(KLT_PATCH_SIZE, KLT_PATCH_SIZE), 2, + cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 30, 0.01), cv::OPTFLOW_USE_INITIAL_FLOW); + else + cv::calcOpticalFlowPyrLK(prev_ipm, cur_ipm, prev_ipm_pts, cur_ipm_pts, status, err, cv::Size(KLT_PATCH_SIZE, KLT_PATCH_SIZE), 1, + cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 30, 0.01), cv::OPTFLOW_USE_INITIAL_FLOW); + + if (true) // flow back + { + vector reverse_status; + vector reverse_pts = prev_ipm_pts; + cv::calcOpticalFlowPyrLK(cur_ipm, prev_ipm, cur_ipm_pts, reverse_pts, reverse_status, err, cv::Size(KLT_PATCH_SIZE, KLT_PATCH_SIZE), 1, + 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] && distance(prev_ipm_pts[i], reverse_pts[i]) <= 0.5 && distance(cur_ipm_pts[i], pred_pts[i]) <= threshold) + { + status[i] = 1; + } + else + status[i] = 0; + } + } + + for (int i = 0; i < cur_ipm_pts.size(); i++) + { + cur_pts.push_back(ipmproc->IPM2Perspective(cur_ipm_pts[i])); + } + + reduceVector(prev_pts, status); + reduceVector(cur_pts, status); + reduceVector(prev_ipm_pts, status); + reduceVector(cur_ipm_pts, status); + reduceVector(ids, status); + reduceVector(track_cnt, status); + + // Coarse speed estimation + if (prev_ipm_pts.size() > 0) + { + vector d_pt; + for (int i = 0; i < prev_ipm_pts.size(); i++) + d_pt.push_back(cv::norm(cur_ipm_pts[i] - prev_ipm_pts[i]) * config.IPM_RESO); + std::sort(d_pt.begin(), d_pt.end()); + std::cerr << "Coarse speed estimation: " << d_pt[d_pt.size() / 2] / (_cur_time - prev_time) << " (m/s)" << std::endl; + } + + for (int i = 0; i < pred_pts.size(); i++) + { + if (status[i] == 1) + cv::circle(cur_ipm_show, pred_pts[i], 5, cv::Scalar(0, 255, 0), 1); + else + cv::circle(cur_ipm_show, pred_pts[i], 5, cv::Scalar(255, 0, 0), 1); + } + + for (int i = 0; i < cur_pts.size(); i++) + { + cv::circle(cur_ipm_show, cur_ipm_pts[i], 0, cv::Scalar(0, 255, 0), 4); + cv::line(cur_ipm_show, prev_ipm_pts[i], cur_ipm_pts[i], cv::Scalar(0, 255, 0)); + if (feature_mode == GroundFeatureMode::MINDIST) + { + cv::circle(mask, cur_ipm_pts[i], MIN_DIST, 0, -1); + } + cv::circle(cur_img_show, cur_pts[i], 0, cv::Scalar(0, 255, 0), 4); + cv::line(cur_img_show, prev_pts[i], cur_pts[i], cv::Scalar(0, 255, 0), 1); + } + + if (cur_pts.size() >= 8) + { + printf("HM ransac begins\n"); + + vector mask_outlier; + cv::Mat Hmatrix = cv::findHomography(cur_ipm_pts, prev_ipm_pts, mask_outlier, cv::RANSAC, 5.0); + // cv::Mat Hmatrix = cv::findHomography(cur_ipm_pts, prev_ipm_pts, mask_outlier, cv::LMEDS); + int size_a = cur_pts.size(); + printf("HM ransac: %d -> %lu: %f\n", size_a, cur_pts.size(), 1.0 * cur_pts.size() / size_a); + + for (int jj = 0; jj < mask_outlier.size(); jj++) + { + if ((int)mask_outlier[jj] == 0) + cv::circle(cur_ipm_show, cur_ipm_pts[jj], 10, cv::Scalar(0, 0, 255), 1); + } + + reduceVector(prev_pts, mask_outlier); + reduceVector(cur_pts, mask_outlier); + reduceVector(prev_ipm_pts, mask_outlier); + reduceVector(cur_ipm_pts, mask_outlier); + reduceVector(ids, mask_outlier); + reduceVector(track_cnt, mask_outlier); + } + } + + vector n_pts_ipm; + // 1) Min dist-based feature extraction, referring to VINS-Mono. + if (feature_mode == GroundFeatureMode::MINDIST) + { + // Extract new points + int n_max_cnt = MAX_CNT - static_cast(cur_pts.size()); + if (n_max_cnt > 0) + { + if (mask.empty()) + cout << "mask is empty " << endl; + if (mask.type() != CV_8UC1) + cout << "mask type wrong " << endl; + cv::goodFeaturesToTrack(cur_ipm, n_pts_ipm, MAX_CNT - cur_pts.size(), 0.001, MIN_DIST, mask, 3, true); + } + } + // 2) Grid-based feature extraction, inspried by https://github.com/KumarRobotics/msckf_vio. + else if (feature_mode == GroundFeatureMode::GRID) + { + auto cur_grid_features = feature2grid(cur_ipm_pts, ids); + + for (const auto &feature : cur_ipm_pts) + { + const cv::Point2f &pt = feature; + const int y = static_cast(pt.y); + const int x = static_cast(pt.x); + if(x<0 || y<0 || x>=cur_ipm.cols || y>=cur_ipm.rows) continue; + + int up_lim = y - 2, bottom_lim = y + 3, left_lim = x - 2, right_lim = x + 3; + if (up_lim < 0) + up_lim = 0; + if (bottom_lim > cur_ipm.rows) + bottom_lim = cur_ipm.rows; + if (left_lim < 0) + left_lim = 0; + if (right_lim > cur_ipm.cols) + right_lim = cur_ipm.cols; + mask(cv::Range(up_lim, bottom_lim), cv::Range(left_lim, right_lim)) = 0; + } + + // Detect new features. + vector new_features(0); + detector_ptr->detect(cur_ipm, new_features, mask); + + // Group the features into grids + map> grid_new_features; + for (int code = 0; code < GRID_ROW * GRID_COL; ++code) + grid_new_features[code] = vector(0); + + for (int i = 0; i < new_features.size(); ++i) + { + const cv::Point2f &pt = new_features[i].pt; + int row = static_cast(pt.y / GRID_HEIGHT); + int col = static_cast(pt.x / GRID_WIDTH); + int code = row * GRID_COL + col; + grid_new_features[code].push_back(new_features[i]); + } + + // Sort the new features in each grid based on its response. + for (auto &key_item : grid_new_features) + { + auto & item = key_item.second; + if (item.size() > GRID_MAX_FEATURE_NUM) + { + std::sort(item.begin(), item.end(), + [](cv::KeyPoint pt1, cv::KeyPoint pt2){return pt1.response > pt2.response;}); + item.erase( + item.begin() + GRID_MAX_FEATURE_NUM, item.end()); + } + } + + int new_added_feature_num = 0; + // Collect new features within each grid with high response. + for (int code = 0; code < GRID_ROW * GRID_COL; ++code) + { + const auto &features_this_grid = cur_grid_features[code]; + const auto &new_features_this_grid = grid_new_features[code]; + + if (features_this_grid.size() >= GRID_MIN_FEATURE_NUM) + continue; + + int vacancy_num = GRID_MIN_FEATURE_NUM - + features_this_grid.size(); + for (int k = 0; + k < vacancy_num && k < new_features_this_grid.size(); ++k) + { + n_pts_ipm.push_back(new_features_this_grid[k].pt); + ++new_added_feature_num; + } + } + } + + for (auto &p_ipm : n_pts_ipm) + { + cv::Point2f p = ipmproc->IPM2Perspective(p_ipm); + n_pts.push_back(p); + cur_pts.push_back(p); + cur_ipm_pts.push_back(p_ipm); + ids.push_back(n_id++); + track_cnt.push_back(1); + + cv::circle(cur_ipm_show, p_ipm, 0, cv::Scalar(0, 0, 255), 4); + cv::circle(cur_img_show, p, 0, cv::Scalar(0, 0, 255), 4); + } + + if (show_track) + { + cv::imshow("cur_ipm_show", cur_ipm_show); + cv::imshow("cur_img_show", cur_img_show); + cv::waitKey(1); + } + // cv::imwrite("/home/zhouyuxuan/data/cur_ipm_show_"+to_string((long long)(_cur_time*1e9))+".png",cur_ipm_show); + // cv::imwrite("/home/zhouyuxuan/data/cur_img_show_"+to_string((long long)(_cur_time*1e9))+".png",cur_img_show); + + + map>>> featureFrame; + for (size_t i = 0; i < ids.size(); i++) + { + int feature_id = ids[i]; + cv::Point2f uvc0 = cur_pts[i]; + double x, y, z; + x = (uvc0.x - intrinsics(2)) / intrinsics(0); + y = (uvc0.y - intrinsics(3)) / intrinsics(1); + z = 1; + int camera_id = 0; + + Eigen::Matrix xyz_uv_velocity; + xyz_uv_velocity << x, y, z, 0.0, 0.0, 0.0, 0.0; + featureFrame[feature_id].emplace_back(camera_id, xyz_uv_velocity); + } + + prev_img = cur_img; + prev_pts = cur_pts; + prev_ipm = cur_ipm; + prev_time = _cur_time; + + // save last camera-ground geometry for tracking + prev_cg = cur_cg; + + return featureFrame; + } + + map>>> GroundTracker::trackImagePerspective(double _cur_time, + const cv::Mat &_img, const double &threshold, const CameraGroundGeometry &cg, + const Eigen::Matrix4d &Tckck_1, const bool &show_track) + { + cur_pts.clear(); + cur_cg = cg; + + cv::Mat img_undist = ipmproc->genUndistortedImage(_img); + cv::Vec4d intrinsics = ipmproc->getUndistortedIntrinsics(); + + cur_img = img_undist; + + cv::Ptr clahe = cv::createCLAHE(3.0, cv::Size(8, 8)); + clahe->apply(cur_img, cur_img); + + if (!(cur_cg == prev_cg)) + { + ipmproc->updateCameraGroundGeometry(cur_cg); + } + + mask = ipmproc->guessGroundROI(); + + if (show_track) + cv::imshow("mask", mask); + + if (!cur_img_semantic.empty()) + { + vector img_channels_temp; + cv::split(cur_img_semantic, img_channels_temp); + cur_img_semantic = img_channels_temp[2]; + cur_img_semantic.setTo(255, cur_img_semantic == 7); + cur_img_semantic.setTo(255, cur_img_semantic == 6); + cur_img_semantic.setTo(255, cur_img_semantic == 20); + mask.setTo(0, cur_img_semantic != 255); + } + + // For visualization + cv::Mat cur_img_show; + cv::cvtColor(img_undist, cur_img_show, cv::COLOR_GRAY2BGR); + + vector pred_pts; + vector status; + vector err; + + if (prev_pts.size() > 0) + { + Eigen::Matrix3d Rckck_1 = Tckck_1.block<3, 3>(0, 0); + Eigen::Vector3d tckck_1 = Tckck_1.block<3, 1>(0, 3); + + // Predict tracked points + for (int i = 0; i < prev_pts.size(); i++) + { + Eigen::Vector3d pc0k_1f = ipmproc->Perspective2Metric(prev_pts[i], &prev_cg); + Eigen::Vector3d pc0kf = Rckck_1 * pc0k_1f + tckck_1; + cv::Point2f uvc1kf = ipmproc->IPM2Perspective(ipmproc->Metric2IPM(pc0kf)); + pred_pts.push_back(uvc1kf); + } + cur_pts = pred_pts; + // Optical flow tracking + + if (threshold > 10) + cv::calcOpticalFlowPyrLK(prev_img, cur_img, prev_pts, cur_pts, status, err, cv::Size(KLT_PATCH_SIZE, KLT_PATCH_SIZE), 2, + cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 30, 0.01), cv::OPTFLOW_USE_INITIAL_FLOW); + else + cv::calcOpticalFlowPyrLK(prev_img, cur_img, prev_pts, cur_pts, status, err, cv::Size(KLT_PATCH_SIZE, KLT_PATCH_SIZE), 1, + cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 30, 0.01), cv::OPTFLOW_USE_INITIAL_FLOW); + + if (true) // flow back + { + vector reverse_status; + vector reverse_pts = prev_pts; + cv::calcOpticalFlowPyrLK(cur_img, prev_img, cur_pts, reverse_pts, reverse_status, err, cv::Size(KLT_PATCH_SIZE, KLT_PATCH_SIZE), 1, + 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] && distance(prev_pts[i], reverse_pts[i]) <= 0.5 && distance(cur_pts[i], pred_pts[i]) <= threshold) + { + status[i] = 1; + } + else + status[i] = 0; + } + } + + reduceVector(prev_pts, status); + reduceVector(cur_pts, status); + reduceVector(ids, status); + reduceVector(track_cnt, status); + + for (int i = 0; i < cur_pts.size(); i++) + { + cv::circle(cur_img_show, cur_pts[i], 0, cv::Scalar(0, 255, 0), 4); + cv::line(cur_img_show, prev_pts[i], cur_pts[i], cv::Scalar(0, 255, 0), 1); + cv::circle(mask, cur_pts[i], MIN_DIST, 0, -1); + } + + if (cur_pts.size() >= 8) + { + printf("HM ransac begins\n"); + + vector mask_outlier; + cv::Mat Hmatrix = cv::findHomography(cur_pts, prev_pts, mask_outlier, cv::RANSAC, 5.0); + int size_a = cur_pts.size(); + printf("HM ransac: %d -> %lu: %f\n", size_a, cur_pts.size(), 1.0 * cur_pts.size() / size_a); + + reduceVector(prev_pts, mask_outlier); + reduceVector(cur_pts, mask_outlier); + reduceVector(ids, mask_outlier); + reduceVector(track_cnt, mask_outlier); + } + } + + // Extract new points + int n_max_cnt = MAX_CNT - static_cast(cur_pts.size()); + vector n_pts; + if (n_max_cnt > 0) + { + if (mask.empty()) + cout << "mask is empty " << endl; + if (mask.type() != CV_8UC1) + cout << "mask type wrong " << endl; + cv::goodFeaturesToTrack(cur_img, n_pts, MAX_CNT - cur_pts.size(), 0.001, MIN_DIST, mask, 3, true); + } + + for (auto &p : n_pts) + { + n_pts.push_back(p); + cur_pts.push_back(p); + ids.push_back(n_id++); + track_cnt.push_back(1); + cv::circle(cur_img_show, p, 0, cv::Scalar(0, 0, 255), 4); + } + + if (show_track) + { + cv::imshow("cur_img_show", cur_img_show); + cv::waitKey(1); + } + + map>>> featureFrame; + for (size_t i = 0; i < ids.size(); i++) + { + int feature_id = ids[i]; + cv::Point2f uvc0 = cur_pts[i]; + double x, y, z; + x = (uvc0.x - intrinsics(2)) / intrinsics(0); + y = (uvc0.y - intrinsics(3)) / intrinsics(1); + z = 1; + int camera_id = 0; + + Eigen::Matrix xyz_uv_velocity; + xyz_uv_velocity << x, y, z, 0.0, 0.0, 0.0, 0.0; + featureFrame[feature_id].emplace_back(camera_id, xyz_uv_velocity); + } + + prev_img = cur_img; + prev_pts = cur_pts; + prev_time = _cur_time; + + // save last camera-ground geometry for tracking + prev_cg = cur_cg; + + return featureFrame; + } + + bool GroundTracker::inBorder(const cv::Point2f &pt) + { + const int BORDER_SIZE = 1; + int img_x = cvRound(pt.x); + int img_y = cvRound(pt.y); + return BORDER_SIZE <= img_x && img_x < col - BORDER_SIZE && BORDER_SIZE <= img_y && img_y < row - BORDER_SIZE; + } + + double GroundTracker::distance(cv::Point2f &pt1, cv::Point2f &pt2) + { + // printf("pt1: %f %f pt2: %f %f\n", pt1.x, pt1.y, pt2.x, pt2.y); + double dx = pt1.x - pt2.x; + double dy = pt1.y - pt2.y; + return sqrt(dx * dx + dy * dy); + } + + map>> GroundTracker::feature2grid(vector pts, vector ids) + { + map>> grid_features; + vector> new_feature_sieve(GRID_ROW * GRID_COL); + for (int i = 0; i < pts.size(); i++) + { + int row = static_cast(pts[i].y / GRID_HEIGHT); + int col = static_cast(pts[i].x / GRID_WIDTH); + int code = row * GRID_COL + col; + grid_features[code].push_back(make_tuple(ids[i], pts[i])); + } + return grid_features; + } + + void GroundTracker::reduceVector(vector &v, vector status) + { + int j = 0; + for (int i = 0; i < int(v.size()); i++) + if (i < status.size() && status[i]) + v[j++] = v[i]; + v.resize(j); + } + + void GroundTracker::reduceVector(vector &v, vector status) + { + int j = 0; + for (int i = 0; i < int(v.size()); i++) + if (status[i]) + v[j++] = v[i]; + v.resize(j); + } +} \ No newline at end of file diff --git a/src/gv_utils.cpp b/src/gv_utils.cpp new file mode 100644 index 0000000..18dae1e --- /dev/null +++ b/src/gv_utils.cpp @@ -0,0 +1,144 @@ +#include "gv_utils.h" + +namespace gv +{ + Eigen::Matrix3d a2mat(const Eigen::Vector3d &att) + { + double sp = sin(att(0)), cp = cos(att(0)); + double sr = sin(att(1)), cr = cos(att(1)); + double sy = sin(att(2)), cy = cos(att(2)); + + Eigen::Matrix3d m; + m << cy * cr - sy * sp * sr, -sy * cp, cy * sr + sy * sp * cr, + sy * cr + cy * sp * sr, cy * cp, sy * sr - cy * sp * cr, + -cp * sr, sp, cp * cr; + return m; + } + + Eigen::Vector3d m2att(const Eigen::Matrix3d &m) + { + Eigen::Vector3d att; + att(0) = asin(m(2, 1)); + att(1) = atan2(-m(2, 0), m(2, 2)); + att(2) = atan2(-m(0, 1), m(1, 1)); + return att; + } + + CameraGroundGeometry::CameraGroundGeometry(const double &a, const double &t, const double &h) + { + this->update(a, t, h); + } + + CameraGroundGeometry::CameraGroundGeometry(const Eigen::Vector3d &n, const double &h) + { + this->update(n, h); + } + + CameraGroundGeometry::CameraGroundGeometry(const Eigen::Matrix3d &R_c_cg, const double &h) + { + this->update(R_c_cg, h); + } + + double CameraGroundGeometry::getAlpha() const + { + return _alpha * 180 / M_PI; + } + + double CameraGroundGeometry::getTheta() const + { + return _theta * 180 / M_PI; + } + + double CameraGroundGeometry::getH() const + { + return _h; + } + + Eigen::Matrix3d CameraGroundGeometry::getR() const + { + return _R_c_cg; + } + + Eigen::Vector3d CameraGroundGeometry::getN() const + { + return _n; + } + + void CameraGroundGeometry::update(const double &a, const double &t, const double &h) + { + if (h > 0) + _h = h; + + _alpha = a / 180 * M_PI; + _theta = t / 180 * M_PI; + + Eigen::Matrix3d Ra; + Ra << cos(_alpha), -sin(_alpha), 0, + sin(_alpha), cos(_alpha), 0, + 0, 0, 1; + Eigen::Matrix3d Rt; + Rt << 1, 0, 0, + 0, cos(_theta), -sin(_theta), + 0, sin(_theta), cos(_theta); + _R_c_cg = Ra * Rt; + _n = _R_c_cg * Eigen::Vector3d(0, -1, 0); + } + + void CameraGroundGeometry::update(const Eigen::Vector3d &n, const double &h) + { + Eigen::Matrix3d R_c_cg = Eigen::Quaterniond::FromTwoVectors(n.normalized(), Eigen::Vector3d(0, -1, 0)).toRotationMatrix(); + this->update(R_c_cg, h); + } + + void CameraGroundGeometry::update(const Eigen::Matrix3d &R_c_cg, const double &h) + { + double a = acos(R_c_cg(0, 0)) * 180 / M_PI; + double b = acos(R_c_cg(2, 2)) * 180 / M_PI; + this->update(a, b, h); + } + + bool CameraGroundGeometry::operator==(const CameraGroundGeometry c1) const + { + if (this->getAlpha() == c1.getAlpha() && this->getTheta() == c1.getTheta() && this->getH() == c1.getH()) + return true; + else + return false; + } + + CameraConfig::CameraConfig(const std::string &config_file) + { + cv::FileStorage fsSettings(config_file, cv::FileStorage::READ); + IPM_HEIGHT = fsSettings["IPM_HEIGHT"]; + IPM_WIDTH = fsSettings["IPM_WIDTH"]; + IPM_RESO = fsSettings["IPM_RESO"]; + + cg = CameraGroundGeometry(fsSettings["priori_alpha"], fsSettings["priori_theta"], fsSettings["priori_H"]); + + + cv::Mat Tic_mat; + fsSettings["body_T_cam0"] >> Tic_mat; + cv::cv2eigen(Tic_mat, Tic); + Ric = Tic.block(0, 0, 3, 3); + tic = Tic.block(0, 3, 3, 1); + + int pn = config_file.find_last_of('/'); + std::string configPath = config_file.substr(0, pn); + + std::string cam0Calib; + fsSettings["cam0_calib"] >> cam0Calib; + std::string cam0Path = configPath + "/" + cam0Calib; + + camera = camodocal::CameraFactory::instance()->generateCameraFromYamlFile(cam0Path); + + feature_mode = fsSettings["feature_mode"]; + min_dist_ground = fsSettings["min_dist_ground"]; + max_cnt_ground = fsSettings["max_cnt_ground"]; + grid_row = fsSettings["grid_row"]; + grid_col = fsSettings["grid_col"]; + grid_min_feature_num = fsSettings["grid_min_feature_num"]; + grid_max_feature_num = fsSettings["grid_max_feature_num"]; + + fsSettings.release(); + + } +} diff --git a/src/ipm_processer.cpp b/src/ipm_processer.cpp new file mode 100644 index 0000000..41e25ed --- /dev/null +++ b/src/ipm_processer.cpp @@ -0,0 +1,311 @@ +#include "ipm_processer.h" +#include +#include +#include +#include +#include + +//#define POINTWISE_MAPPING +namespace gv +{ + IPMProcesser::IPMProcesser(CameraConfig conf, IPMType ipm_type) : _config(conf), _ipm_type(ipm_type) { +#ifdef POINTWISE_MAPPING + _IPM_mapx_array = new float[_config.IPM_HEIGHT*_config.IPM_WIDTH]; + _IPM_mapy_array = new float[_config.IPM_HEIGHT*_config.IPM_WIDTH]; + + memset(_IPM_mapx_array, 0.0, _config.IPM_HEIGHT*_config.IPM_WIDTH * sizeof(float)); + memset(_IPM_mapy_array, 0.0, _config.IPM_HEIGHT*_config.IPM_WIDTH * sizeof(float)); + + _IPM_mapx = cv::Mat(_config.IPM_HEIGHT, _config.IPM_WIDTH, CV_32FC1); + _IPM_mapy = cv::Mat(_config.IPM_HEIGHT, _config.IPM_WIDTH, CV_32FC1); +#endif + + // config.camera = camodocal::CameraFactory::instance()->generateCameraFromYamlFile("H:/2022_10_12/data_20221012133020/config/cam0_pinhole.yaml"); + if (_config.camera->modelType() == camodocal::Camera::PINHOLE) + { + _h = _config.camera->imageHeight(); + _w = _config.camera->imageWidth(); + _fx = std::dynamic_pointer_cast(_config.camera)->getParameters().fx(); + _fy = std::dynamic_pointer_cast(_config.camera)->getParameters().fy(); + _cx = std::dynamic_pointer_cast(_config.camera)->getParameters().cx(); + _cy = std::dynamic_pointer_cast(_config.camera)->getParameters().cy(); + } + else if (_config.camera->modelType() == camodocal::Camera::PINHOLE_FULL + || _config.camera->modelType() == camodocal::Camera::KANNALA_BRANDT + || _config.camera->modelType() == camodocal::Camera::SCARAMUZZA + || _config.camera->modelType() == camodocal::Camera::MEI) + { + _h = _config.camera->imageHeight(); + _w = _config.camera->imageWidth(); + Eigen::Vector2d p0(0, 0); + Eigen::Vector2d p1(_w - 1, _h - 1); + Eigen::Vector3d p0_, p1_; + _config.camera->liftProjective(p0, p0_); + _config.camera->liftProjective(p1, p1_); + p0_ /= p0_(2); + p1_ /= p1_(2); + _fx = (_w - 1) / (p1_(0) - p0_(0)); + _fy = (_h - 1) / (p1_(1) - p0_(1)); + _cx = -_fx * p0_(0); + _cy = -_fy * p0_(1); + } + else + { + throw std::exception(); + } + _config.camera->initUndistortRectifyMap(_undist_M0, _undist_M1, _fx, _fy, cv::Size(_w, _h), _cx, _cy); + this->updateCameraGroundGeometry(_config.cg); + } + IPMProcesser::~IPMProcesser() + { + if (_IPM_mapx_array) delete[] _IPM_mapx_array; + if (_IPM_mapy_array) delete[] _IPM_mapy_array; + } + + int IPMProcesser::updateCameraGroundGeometry(CameraGroundGeometry cg) + { + _Rc0c1 = cg.getR(); + _d = cg.getH(); + return updateIPMMap(); + } + + cv::Mat IPMProcesser::guessGroundROI() + { + cv::Point p0 = IPM2Perspective(cv::Point2f(0,0)); + cv::Point p1 = IPM2Perspective(cv::Point2f(_config.IPM_WIDTH-1,0)); + cv::Point p2 = IPM2Perspective(cv::Point2f(_config.IPM_WIDTH-1,_config.IPM_HEIGHT/2)); + cv::Point p3 = IPM2Perspective(cv::Point2f(0,_config.IPM_HEIGHT/2)); + std::vector pts({p0,p1,p2,p3}); + cv::Mat mask = cv::Mat(_config.camera->imageHeight(), _config.camera->imageWidth(), CV_8UC1); + mask.setTo(0); + cv::fillConvexPoly(mask,pts,255); + return mask; + } + + int IPMProcesser::updateIPMMap() + { +#ifdef POINTWISE_MAPPING + double c1_p_c1f_array[3]; + double R_c0c1_array[3][3] = { {_Rc0c1(0, 0),_Rc0c1(0, 1),_Rc0c1(0, 2)}, + {_Rc0c1(1, 0),_Rc0c1(1, 1),_Rc0c1(1, 2)}, + {_Rc0c1(2, 0),_Rc0c1(2, 1),_Rc0c1(2, 2)} }; + double xy1_y_array[3]; + if (_ipm_type == IPMType::NORMAL) + { + for (int i = 0; i < _config.IPM_HEIGHT; i++) + { + for (int j = 0; j < _config.IPM_WIDTH; j++) + { + c1_p_c1f_array[0] = (j - _config.IPM_WIDTH / 2) * _config.IPM_RESO; + c1_p_c1f_array[1] = _d; + c1_p_c1f_array[2] =(_config.IPM_HEIGHT - i - 1) * _config.IPM_RESO; + xy1_y_array[0] = (R_c0c1_array[0][0] * c1_p_c1f_array[0] + R_c0c1_array[0][1] * c1_p_c1f_array[1] + R_c0c1_array[0][2] * c1_p_c1f_array[2]); + xy1_y_array[1] = (R_c0c1_array[1][0] * c1_p_c1f_array[0] + R_c0c1_array[1][1] * c1_p_c1f_array[1] + R_c0c1_array[1][2] * c1_p_c1f_array[2]); + xy1_y_array[2] = (R_c0c1_array[2][0] * c1_p_c1f_array[0] + R_c0c1_array[2][1] * c1_p_c1f_array[1] + R_c0c1_array[2][2] * c1_p_c1f_array[2]); + _IPM_mapx_array[i*_config.IPM_WIDTH + j] = _fx * xy1_y_array[0] / xy1_y_array[2] + _cx; + _IPM_mapy_array[i*_config.IPM_WIDTH + j] = _fy * xy1_y_array[1] / xy1_y_array[2] + _cy; + } + } + _IPM_mapx = cv::Mat(_config.IPM_HEIGHT, _config.IPM_WIDTH, CV_32FC1, _IPM_mapx_array); + _IPM_mapy = cv::Mat(_config.IPM_HEIGHT, _config.IPM_WIDTH, CV_32FC1, _IPM_mapy_array); + return 0; + } + else + { + return -1; + } + +#else + Eigen::Matrix3d K, K_IPM_inv; + K << _fx, 0, _cx, + 0, _fy, _cy, + 0, 0, 1; + if (_ipm_type == IPMType::NORMAL) + { + K_IPM_inv << _config.IPM_RESO / _d, 0, (-_config.IPM_WIDTH / 2)*_config.IPM_RESO / _d, + 0, 0, 1, + 0, -_config.IPM_RESO / _d, (_config.IPM_HEIGHT - 1)*_config.IPM_RESO / _d; + Eigen::Matrix3d T_IPM = (K * _Rc0c1 * _d * K_IPM_inv).inverse(); + cv::eigen2cv(T_IPM, _IPM_transform); + _IPM_transform.convertTo(_IPM_transform, CV_32F); + return 0; + } + else + { + return -1; + } +#endif + } + + cv::Mat IPMProcesser::genUndistortedImage(cv::Mat mm) const + { + cv::Mat mm_undist; + cv::remap(mm, mm_undist, _undist_M0, _undist_M1, cv::INTER_LINEAR); + return mm_undist; + } + + cv::Mat IPMProcesser::genIPM(cv::Mat mm, bool need_undistort, cv::Mat mm_undist) const + { + if (need_undistort) + { + cv::remap(mm, mm_undist, _undist_M0, _undist_M1, cv::INTER_LINEAR); + } + else + { + mm_undist = mm; + } + cv::Mat m_ipm(_config.IPM_HEIGHT, _config.IPM_WIDTH, CV_8U); +#ifdef POINTWISE_MAPPING + cv::remap(mm_undist, m_ipm, _IPM_mapx, _IPM_mapy, cv::INTER_NEAREST); + cv::remap(mm_undist, m_ipm, _IPM_mapx, _IPM_mapy, cv::INTER_LINEAR); +#else + cv::warpPerspective(mm_undist, m_ipm, _IPM_transform, m_ipm.size(), cv::INTER_LINEAR); +#endif + return m_ipm; + } + + cv::Vec4d IPMProcesser::getUndistortedIntrinsics() + { + return cv::Vec4d(_fx, _fy, _cx, _cy); + } + + cv::Point2f IPMProcesser::IPM2Perspective(cv::Point2f p, CameraGroundGeometry* cg_temp) const + { + Eigen::Matrix3d Rc0c1 = _Rc0c1; + double d = _d; + if(cg_temp!=nullptr) + { + Rc0c1 = cg_temp->getR(); + d = cg_temp->getH(); + } + + if (_ipm_type == IPMType::NORMAL) + { + Eigen::Vector3d pc1f = Eigen::Vector3d((p.x - _config.IPM_WIDTH / 2) * _config.IPM_RESO, + d, + (_config.IPM_HEIGHT - p.y - 1) * _config.IPM_RESO); + Eigen::Vector3d pc0f = Rc0c1 * pc1f; + Eigen::Vector3d uv_c0 = Eigen::Vector3d(pc0f(0) / pc0f(2) *_fx + _cx, pc0f(1) / pc0f(2)*_fy + _cy, 1); + return cv::Point2f(uv_c0(0), uv_c0(1)); + } + else + { + throw std::exception(); + } + } + + cv::Point2f IPMProcesser::Perspective2IPM(cv::Point2f p, CameraGroundGeometry* cg_temp) const + { + Eigen::Matrix3d Rc0c1 = _Rc0c1; + double d = _d; + if(cg_temp!=nullptr) + { + Rc0c1 = cg_temp->getR(); + d = cg_temp->getH(); + } + + double x = (p.x - _cx) / _fx; + double y = (p.y - _cy) / _fy; + + cv::Point2f pipm; + if (_ipm_type == IPMType::NORMAL) + { + Eigen::Vector3d xyzc1f = (Rc0c1.transpose()*Eigen::Vector3d(x, y, 1)); + double depth = d / xyzc1f(1); + //std::cerr << depth << std::endl; + Eigen::Vector3d pc1f = xyzc1f * depth; + //std::cerr << pc1f.transpose() << std::endl; + pipm = cv::Point2f(pc1f.x() / _config.IPM_RESO + _config.IPM_WIDTH / 2, + -pc1f.z() / _config.IPM_RESO + _config.IPM_HEIGHT - 1); + return pipm; + } + else + { + throw std::exception(); + } + } + + Eigen::Vector3d IPMProcesser::Perspective2Metric(cv::Point2f p, CameraGroundGeometry* cg_temp) const + { + Eigen::Matrix3d Rc0c1 = _Rc0c1; + double d = _d; + if(cg_temp!=nullptr) + { + Rc0c1 = cg_temp->getR(); + d = cg_temp->getH(); + } + + double x = (p.x - _cx) / _fx; + double y = (p.y - _cy) / _fy; + + Eigen::Vector3d pc0f; + if (_ipm_type == IPMType::NORMAL) + { + Eigen::Vector3d xyzc1f = (Rc0c1.transpose()*Eigen::Vector3d(x, y, 1)); + double depth = d / xyzc1f(1); + //std::cerr << depth << std::endl; + Eigen::Vector3d pc1f = xyzc1f * depth; + pc0f = Rc0c1 * pc1f; + return pc0f; + } + else + { + throw std::exception(); + } + } + + Eigen::Vector3d IPMProcesser::NormalizedPerspective2Metric(Eigen::Vector3d p, CameraGroundGeometry* cg_temp) const + { + return Perspective2Metric(cv::Point2f(p.x()*_fx + _cx, p.y()*_fy + _cy), cg_temp); + } + + Eigen::Vector3d IPMProcesser::IPM2Metric(cv::Point2f p, CameraGroundGeometry* cg_temp) const + { + Eigen::Matrix3d Rc0c1 = _Rc0c1; + double d = _d; + if(cg_temp!=nullptr) + { + Rc0c1 = cg_temp->getR(); + d = cg_temp->getH(); + } + + if (_ipm_type == IPMType::NORMAL) + { + Eigen::Vector3d pc1f = Eigen::Vector3d((p.x - _config.IPM_WIDTH / 2) * _config.IPM_RESO, + d, + (_config.IPM_HEIGHT - p.y - 1) * _config.IPM_RESO); + Eigen::Vector3d pc0f = Rc0c1 * pc1f; + return pc0f; + } + else + { + throw std::exception(); + } + } + + cv::Point2f IPMProcesser::Metric2IPM(Eigen::Vector3d p, CameraGroundGeometry* cg_temp) const + { + Eigen::Matrix3d Rc0c1 = _Rc0c1; + double d = _d; + if(cg_temp!=nullptr) + { + Rc0c1 = cg_temp->getR(); + d = cg_temp->getH(); + } + + cv::Point2f uv; + if (_ipm_type == IPMType::NORMAL) + { + Eigen::Vector3d pc1f = Rc0c1.transpose() * p; + uv.x = pc1f(0) / _config.IPM_RESO + _config.IPM_WIDTH / 2; + uv.y = _config.IPM_HEIGHT - 1 - pc1f(2) / _config.IPM_RESO; + return uv; + } + else + { + throw std::exception(); + } + } +} + + \ No newline at end of file