diff --git a/CMakeLists.txt b/CMakeLists.txt index 1c3859c..181ea40 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -106,6 +106,7 @@ set_outdir( ) ## main + if(WITH_GLOG) add_executable(main src/main.cc) target_link_libraries(main glog::glog) @@ -115,6 +116,42 @@ if(WITH_GLOG) ) endif() +## camodocal + +if(WITH_CAM_MODELS) + set(EIGEN_INCLUDE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/eigen3) + + LIST(APPEND CMAKE_PREFIX_PATH ${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/ceres/share/Ceres) + find_package(Ceres REQUIRED) + + message(STATUS "CERES_INCLUDE_DIRS: ${CERES_INCLUDE_DIRS}") + message(STATUS "CERES_LIBRARIES: ${CERES_LIBRARIES}") + message(STATUS "EIGEN_INCLUDE_DIRS: ${EIGEN_INCLUDE_DIRS}") + + include_directories( + ${catkin_INCLUDE_DIRS} + ${EIGEN_INCLUDE_DIRS} + src/mynteye/api/camodocal/include + ) + + add_library(camodocal STATIC + src/mynteye/api/camodocal/src/chessboard/Chessboard.cc + src/mynteye/api/camodocal/src/calib/CameraCalibration.cc + src/mynteye/api/camodocal/src/calib/StereoCameraCalibration.cc + src/mynteye/api/camodocal/src/camera_models/Camera.cc + src/mynteye/api/camodocal/src/camera_models/CameraFactory.cc + src/mynteye/api/camodocal/src/camera_models/CostFunctionFactory.cc + src/mynteye/api/camodocal/src/camera_models/PinholeCamera.cc + src/mynteye/api/camodocal/src/camera_models/CataCamera.cc + src/mynteye/api/camodocal/src/camera_models/EquidistantCamera.cc + src/mynteye/api/camodocal/src/camera_models/ScaramuzzaCamera.cc + src/mynteye/api/camodocal/src/sparse_graph/Transform.cc + src/mynteye/api/camodocal/src/gpl/gpl.cc + src/mynteye/api/camodocal/src/gpl/EigenQuaternionParameterization.cc + ) + target_link_libraries(camodocal ${CERES_LIBRARIES}) +endif() + ## libmynteye if(NOT WITH_GLOG AND NOT OS_WIN) @@ -210,6 +247,7 @@ endif() if(WITH_GLOG) list(APPEND MYNTEYE_LINKLIBS glog::glog) endif() + #message(STATUS "MYNTEYE_LINKLIBS: ${MYNTEYE_LINKLIBS}") add_library(${MYNTEYE_NAME} SHARED ${MYNTEYE_SRCS}) diff --git a/cmake/Option.cmake b/cmake/Option.cmake index dba2465..cdbd897 100644 --- a/cmake/Option.cmake +++ b/cmake/Option.cmake @@ -23,7 +23,7 @@ include(${CMAKE_CURRENT_LIST_DIR}/Utils.cmake) option(WITH_API "Build with API layer, need OpenCV" ON) option(WITH_DEVICE_INFO_REQUIRED "Build with device info required" ON) -option(WITH_CAM_MODELS "Build with more camera models" OFF) +option(WITH_CAM_MODELS "Build with more camera models, WITH_API must be ON" OFF) # 3rdparty components @@ -37,6 +37,9 @@ option(WITH_GLOG "Include glog support" OFF) if(WITH_API) include(${CMAKE_CURRENT_LIST_DIR}/DetectOpenCV.cmake) +else() + # Disable WITH_CAM_MODELS if WITH_API is OFF + set(WITH_CAM_MODELS OFF) endif() if(WITH_DEVICE_INFO_REQUIRED) diff --git a/src/mynteye/api/camodocal/include/camodocal/EigenUtils.h b/src/mynteye/api/camodocal/include/camodocal/EigenUtils.h new file mode 100644 index 0000000..7e49915 --- /dev/null +++ b/src/mynteye/api/camodocal/include/camodocal/EigenUtils.h @@ -0,0 +1,399 @@ +#ifndef EIGENUTILS_H +#define EIGENUTILS_H + +#include "Eigen/Dense" +#include + +#include "ceres/rotation.h" + +namespace camodocal { + +template +T square(const T &m) { + return m * m; +} + +// Returns the 3D cross product skew symmetric matrix of a given 3D vector +template +Eigen::Matrix skew(const Eigen::Matrix &vec) { + return (Eigen::Matrix() << T(0), -vec(2), vec(1), vec(2), T(0), + -vec(0), -vec(1), vec(0), T(0)) + .finished(); +} + +template +typename Eigen::MatrixBase::PlainObject sqrtm( + const Eigen::MatrixBase &A) { + Eigen::SelfAdjointEigenSolver es(A); + + return es.operatorSqrt(); +} + +template +Eigen::Matrix AngleAxisToRotationMatrix( + const Eigen::Matrix &rvec) { + T angle = rvec.norm(); + if (angle == T(0)) { + return Eigen::Matrix::Identity(); + } + + Eigen::Matrix axis; + axis = rvec.normalized(); + + Eigen::Matrix rmat; + rmat = Eigen::AngleAxis(angle, axis); + + return rmat; +} + +template +Eigen::Quaternion AngleAxisToQuaternion(const Eigen::Matrix &rvec) { + Eigen::Matrix rmat = AngleAxisToRotationMatrix(rvec); + + return Eigen::Quaternion(rmat); +} + +template +void AngleAxisToQuaternion(const Eigen::Matrix &rvec, T *q) { + Eigen::Quaternion quat = AngleAxisToQuaternion(rvec); + + q[0] = quat.x(); + q[1] = quat.y(); + q[2] = quat.z(); + q[3] = quat.w(); +} + +template +Eigen::Matrix RotationToAngleAxis(const Eigen::Matrix &rmat) { + Eigen::AngleAxis angleaxis; + angleaxis.fromRotationMatrix(rmat); + return angleaxis.angle() * angleaxis.axis(); +} + +template +void QuaternionToAngleAxis(const T *const q, Eigen::Matrix &rvec) { + Eigen::Quaternion quat(q[3], q[0], q[1], q[2]); + + Eigen::Matrix rmat = quat.toRotationMatrix(); + + Eigen::AngleAxis angleaxis; + angleaxis.fromRotationMatrix(rmat); + + rvec = angleaxis.angle() * angleaxis.axis(); +} + +template +Eigen::Matrix QuaternionToRotation(const T *const q) { + T R[9]; + ceres::QuaternionToRotation(q, R); + + Eigen::Matrix rmat; + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + rmat(i, j) = R[i * 3 + j]; + } + } + + return rmat; +} + +template +void QuaternionToRotation(const T *const q, T *rot) { + ceres::QuaternionToRotation(q, rot); +} + +template +Eigen::Matrix QuaternionMultMatLeft(const Eigen::Quaternion &q) { + return (Eigen::Matrix() << q.w(), -q.z(), q.y(), q.x(), q.z(), q.w(), + -q.x(), q.y(), -q.y(), q.x(), q.w(), q.z(), -q.x(), -q.y(), -q.z(), + q.w()) + .finished(); +} + +template +Eigen::Matrix QuaternionMultMatRight(const Eigen::Quaternion &q) { + return (Eigen::Matrix() << q.w(), q.z(), -q.y(), q.x(), -q.z(), + q.w(), q.x(), q.y(), q.y(), -q.x(), q.w(), q.z(), -q.x(), -q.y(), + -q.z(), q.w()) + .finished(); +} + +/// @param theta - rotation about screw axis +/// @param d - projection of tvec on the rotation axis +/// @param l - screw axis direction +/// @param m - screw axis moment +template +void AngleAxisAndTranslationToScrew( + const Eigen::Matrix &rvec, const Eigen::Matrix &tvec, + T &theta, T &d, Eigen::Matrix &l, Eigen::Matrix &m) { + theta = rvec.norm(); + if (theta == 0) { + l.setZero(); + m.setZero(); + std::cout << "Warning: Undefined screw! Returned 0. " << std::endl; + } + + l = rvec.normalized(); + + Eigen::Matrix t = tvec; + + d = t.transpose() * l; + + // point on screw axis - projection of origin on screw axis + Eigen::Matrix c; + c = 0.5 * (t - d * l + (1.0 / tan(theta / 2.0) * l).cross(t)); + + // c and hence the screw axis is not defined if theta is either 0 or M_PI + m = c.cross(l); +} + +template +Eigen::Matrix RPY2mat(T roll, T pitch, T yaw) { + Eigen::Matrix m; + + T cr = cos(roll); + T sr = sin(roll); + T cp = cos(pitch); + T sp = sin(pitch); + T cy = cos(yaw); + T sy = sin(yaw); + + m(0, 0) = cy * cp; + m(0, 1) = cy * sp * sr - sy * cr; + m(0, 2) = cy * sp * cr + sy * sr; + m(1, 0) = sy * cp; + m(1, 1) = sy * sp * sr + cy * cr; + m(1, 2) = sy * sp * cr - cy * sr; + m(2, 0) = -sp; + m(2, 1) = cp * sr; + m(2, 2) = cp * cr; + return m; +} + +template +void mat2RPY(const Eigen::Matrix &m, T &roll, T &pitch, T &yaw) { + roll = atan2(m(2, 1), m(2, 2)); + pitch = atan2(-m(2, 0), sqrt(m(2, 1) * m(2, 1) + m(2, 2) * m(2, 2))); + yaw = atan2(m(1, 0), m(0, 0)); +} + +template +Eigen::Matrix homogeneousTransform( + const Eigen::Matrix &R, const Eigen::Matrix &t) { + Eigen::Matrix H; + H.setIdentity(); + + H.block(0, 0, 3, 3) = R; + H.block(0, 3, 3, 1) = t; + + return H; +} + +template +Eigen::Matrix poseWithCartesianTranslation( + const T *const q, const T *const p) { + Eigen::Matrix pose = Eigen::Matrix::Identity(); + + T rotation[9]; + ceres::QuaternionToRotation(q, rotation); + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + pose(i, j) = rotation[i * 3 + j]; + } + } + + pose(0, 3) = p[0]; + pose(1, 3) = p[1]; + pose(2, 3) = p[2]; + + return pose; +} + +template +Eigen::Matrix poseWithSphericalTranslation( + const T *const q, const T *const p, const T scale = T(1.0)) { + Eigen::Matrix pose = Eigen::Matrix::Identity(); + + T rotation[9]; + ceres::QuaternionToRotation(q, rotation); + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + pose(i, j) = rotation[i * 3 + j]; + } + } + + T theta = p[0]; + T phi = p[1]; + pose(0, 3) = sin(theta) * cos(phi) * scale; + pose(1, 3) = sin(theta) * sin(phi) * scale; + pose(2, 3) = cos(theta) * scale; + + return pose; +} + +// Returns the Sampson error of a given essential matrix and 2 image points +template +T sampsonError( + const Eigen::Matrix &E, const Eigen::Matrix &p1, + const Eigen::Matrix &p2) { + Eigen::Matrix Ex1 = E * p1; + Eigen::Matrix Etx2 = E.transpose() * p2; + + T x2tEx1 = p2.dot(Ex1); + + // compute Sampson error + T err = square(x2tEx1) / (square(Ex1(0, 0)) + square(Ex1(1, 0)) + + square(Etx2(0, 0)) + square(Etx2(1, 0))); + + return err; +} + +// Returns the Sampson error of a given rotation/translation and 2 image points +template +T sampsonError( + const Eigen::Matrix &R, const Eigen::Matrix &t, + const Eigen::Matrix &p1, const Eigen::Matrix &p2) { + // construct essential matrix + Eigen::Matrix E = skew(t) * R; + + Eigen::Matrix Ex1 = E * p1; + Eigen::Matrix Etx2 = E.transpose() * p2; + + T x2tEx1 = p2.dot(Ex1); + + // compute Sampson error + T err = square(x2tEx1) / (square(Ex1(0, 0)) + square(Ex1(1, 0)) + + square(Etx2(0, 0)) + square(Etx2(1, 0))); + + return err; +} + +// Returns the Sampson error of a given rotation/translation and 2 image points +template +T sampsonError( + const Eigen::Matrix &H, const Eigen::Matrix &p1, + const Eigen::Matrix &p2) { + Eigen::Matrix R = H.block(0, 0, 3, 3); + Eigen::Matrix t = H.block(0, 3, 3, 1); + + return sampsonError(R, t, p1, p2); +} + +template +Eigen::Matrix transformPoint( + const Eigen::Matrix &H, const Eigen::Matrix &P) { + Eigen::Matrix P_trans = + H.block(0, 0, 3, 3) * P + H.block(0, 3, 3, 1); + + return P_trans; +} + +template +Eigen::Matrix estimate3DRigidTransform( + const std::vector, + Eigen::aligned_allocator > > + &points1, + const std::vector, + Eigen::aligned_allocator > > + &points2) { + // compute centroids + Eigen::Matrix c1, c2; + c1.setZero(); + c2.setZero(); + + for (size_t i = 0; i < points1.size(); ++i) { + c1 += points1.at(i); + c2 += points2.at(i); + } + + c1 /= points1.size(); + c2 /= points1.size(); + + Eigen::Matrix X(3, points1.size()); + Eigen::Matrix Y(3, points1.size()); + for (size_t i = 0; i < points1.size(); ++i) { + X.col(i) = points1.at(i) - c1; + Y.col(i) = points2.at(i) - c2; + } + + Eigen::Matrix H = X * Y.transpose(); + + Eigen::JacobiSVD > svd( + H, Eigen::ComputeFullU | Eigen::ComputeFullV); + + Eigen::Matrix U = svd.matrixU(); + Eigen::Matrix V = svd.matrixV(); + if (U.determinant() * V.determinant() < 0.0) { + V.col(2) *= -1.0; + } + + Eigen::Matrix R = V * U.transpose(); + Eigen::Matrix t = c2 - R * c1; + + return homogeneousTransform(R, t); +} + +template +Eigen::Matrix estimate3DRigidSimilarityTransform( + const std::vector, + Eigen::aligned_allocator > > + &points1, + const std::vector, + Eigen::aligned_allocator > > + &points2) { + // compute centroids + Eigen::Matrix c1, c2; + c1.setZero(); + c2.setZero(); + + for (size_t i = 0; i < points1.size(); ++i) { + c1 += points1.at(i); + c2 += points2.at(i); + } + + c1 /= points1.size(); + c2 /= points1.size(); + + Eigen::Matrix X(3, points1.size()); + Eigen::Matrix Y(3, points1.size()); + for (size_t i = 0; i < points1.size(); ++i) { + X.col(i) = points1.at(i) - c1; + Y.col(i) = points2.at(i) - c2; + } + + Eigen::Matrix H = X * Y.transpose(); + + Eigen::JacobiSVD > svd( + H, Eigen::ComputeFullU | Eigen::ComputeFullV); + + Eigen::Matrix U = svd.matrixU(); + Eigen::Matrix V = svd.matrixV(); + if (U.determinant() * V.determinant() < 0.0) { + V.col(2) *= -1.0; + } + + Eigen::Matrix R = V * U.transpose(); + + std::vector, + Eigen::aligned_allocator > > + rotatedPoints1(points1.size()); + for (size_t i = 0; i < points1.size(); ++i) { + rotatedPoints1.at(i) = R * (points1.at(i) - c1); + } + + double sum_ss = 0.0, sum_tt = 0.0; + for (size_t i = 0; i < points1.size(); ++i) { + sum_ss += (points1.at(i) - c1).squaredNorm(); + sum_tt += (points2.at(i) - c2).dot(rotatedPoints1.at(i)); + } + + double scale = sum_tt / sum_ss; + + Eigen::Matrix sR = scale * R; + Eigen::Matrix t = c2 - sR * c1; + + return homogeneousTransform(sR, t); +} +} + +#endif diff --git a/src/mynteye/api/camodocal/include/camodocal/calib/CameraCalibration.h b/src/mynteye/api/camodocal/include/camodocal/calib/CameraCalibration.h new file mode 100644 index 0000000..e2033d3 --- /dev/null +++ b/src/mynteye/api/camodocal/include/camodocal/calib/CameraCalibration.h @@ -0,0 +1,78 @@ +#ifndef CAMERACALIBRATION_H +#define CAMERACALIBRATION_H + +#include + +#include "camodocal/camera_models/Camera.h" + +namespace camodocal { + +class CameraCalibration { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + CameraCalibration(); + + CameraCalibration( + Camera::ModelType modelType, const std::string &cameraName, + const cv::Size &imageSize, const cv::Size &boardSize, float squareSize); + + void clear(void); + + void addChessboardData(const std::vector &corners); + + bool calibrate(void); + + int sampleCount(void) const; + std::vector > &imagePoints(void); + const std::vector > &imagePoints(void) const; + std::vector > &scenePoints(void); + const std::vector > &scenePoints(void) const; + CameraPtr &camera(void); + const CameraConstPtr camera(void) const; + + Eigen::Matrix2d &measurementCovariance(void); + const Eigen::Matrix2d &measurementCovariance(void) const; + + cv::Mat &cameraPoses(void); + const cv::Mat &cameraPoses(void) const; + + void drawResults(std::vector &images) const; + + void writeParams(const std::string &filename) const; + + bool writeChessboardData(const std::string &filename) const; + bool readChessboardData(const std::string &filename); + + void setVerbose(bool verbose); + + private: + bool calibrateHelper( + CameraPtr &camera, std::vector &rvecs, + std::vector &tvecs) const; + + void optimize( + CameraPtr &camera, std::vector &rvecs, + std::vector &tvecs) const; + + template + void readData(std::ifstream &ifs, T &data) const; + + template + void writeData(std::ofstream &ofs, T data) const; + + cv::Size m_boardSize; + float m_squareSize; + + CameraPtr m_camera; + cv::Mat m_cameraPoses; + + std::vector > m_imagePoints; + std::vector > m_scenePoints; + + Eigen::Matrix2d m_measurementCovariance; + + bool m_verbose; +}; +} + +#endif diff --git a/src/mynteye/api/camodocal/include/camodocal/calib/StereoCameraCalibration.h b/src/mynteye/api/camodocal/include/camodocal/calib/StereoCameraCalibration.h new file mode 100644 index 0000000..d9e6b85 --- /dev/null +++ b/src/mynteye/api/camodocal/include/camodocal/calib/StereoCameraCalibration.h @@ -0,0 +1,53 @@ +#ifndef STEREOCAMERACALIBRATION_H +#define STEREOCAMERACALIBRATION_H + +#include "CameraCalibration.h" + +namespace camodocal { + +class StereoCameraCalibration { + public: + StereoCameraCalibration( + Camera::ModelType modelType, const std::string &cameraLeftName, + const std::string &cameraRightName, const cv::Size &imageSize, + const cv::Size &boardSize, float squareSize); + + void clear(void); + + void addChessboardData( + const std::vector &cornersLeft, + const std::vector &cornersRight); + + bool calibrate(void); + + int sampleCount(void) const; + const std::vector > &imagePointsLeft(void) const; + const std::vector > &imagePointsRight(void) const; + const std::vector > &scenePoints(void) const; + + CameraPtr &cameraLeft(void); + const CameraConstPtr cameraLeft(void) const; + + CameraPtr &cameraRight(void); + const CameraConstPtr cameraRight(void) const; + + void drawResults( + std::vector &imagesLeft, + std::vector &imagesRight) const; + + void writeParams(const std::string &directory) const; + void setVerbose(bool verbose); + + private: + CameraCalibration m_calibLeft; + CameraCalibration m_calibRight; + + Eigen::Quaterniond m_q; + Eigen::Vector3d m_t; + + bool m_verbose; + std::vector stereo_error; +}; +} + +#endif diff --git a/src/mynteye/api/camodocal/include/camodocal/camera_models/Camera.h b/src/mynteye/api/camodocal/include/camodocal/camera_models/Camera.h new file mode 100644 index 0000000..eb3970f --- /dev/null +++ b/src/mynteye/api/camodocal/include/camodocal/camera_models/Camera.h @@ -0,0 +1,138 @@ +#ifndef CAMERA_H +#define CAMERA_H + +#include +#include "eigen3/Eigen/Dense" +#include +#include + +namespace camodocal { + +class Camera { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + enum ModelType { KANNALA_BRANDT, MEI, PINHOLE, 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 > &objectPoints, + const std::vector > &imagePoints) = 0; + virtual void estimateExtrinsics( + const std::vector &objectPoints, + const std::vector &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 ¶meters) = 0; + virtual void writeParameters(std::vector ¶meters) 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 > &objectPoints, + const std::vector > &imagePoints, + const std::vector &rvecs, const std::vector &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 &objectPoints, const cv::Mat &rvec, + const cv::Mat &tvec, std::vector &imagePoints) const; + + protected: + cv::Mat m_mask; +}; + +typedef boost::shared_ptr CameraPtr; +typedef boost::shared_ptr CameraConstPtr; +} + +#endif diff --git a/src/mynteye/api/camodocal/include/camodocal/camera_models/CameraFactory.h b/src/mynteye/api/camodocal/include/camodocal/camera_models/CameraFactory.h new file mode 100644 index 0000000..c74a693 --- /dev/null +++ b/src/mynteye/api/camodocal/include/camodocal/camera_models/CameraFactory.h @@ -0,0 +1,29 @@ +#ifndef CAMERAFACTORY_H +#define CAMERAFACTORY_H + +#include +#include + +#include "camodocal/camera_models/Camera.h" + +namespace camodocal { + +class CameraFactory { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + CameraFactory(); + + static boost::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 boost::shared_ptr m_instance; +}; +} + +#endif diff --git a/src/mynteye/api/camodocal/include/camodocal/camera_models/CataCamera.h b/src/mynteye/api/camodocal/include/camodocal/camera_models/CataCamera.h new file mode 100644 index 0000000..b830305 --- /dev/null +++ b/src/mynteye/api/camodocal/include/camodocal/camera_models/CataCamera.h @@ -0,0 +1,205 @@ +#ifndef CATACAMERA_H +#define CATACAMERA_H + +#include +#include + +#include "Camera.h" +#include "ceres/rotation.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 ¶ms); + + 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 ¶ms); + + 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 > &objectPoints, + const 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 ¶meters); + + void readParameters(const std::vector ¶meterVec); + void writeParameters(std::vector ¶meterVec) 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 boost::shared_ptr CataCameraPtr; +typedef boost::shared_ptr CataCameraConstPtr; + +template +void CataCamera::spaceToPlane( + const T *const params, const T *const q, const T *const t, + const Eigen::Matrix &P, Eigen::Matrix &p) { + T P_w[3]; + P_w[0] = T(P(0)); + P_w[1] = T(P(1)); + P_w[2] = T(P(2)); + + // Convert quaternion from Eigen convention (x, y, z, w) + // to Ceres convention (w, x, y, z) + T q_ceres[4] = {q[3], q[0], q[1], q[2]}; + + T P_c[3]; + ceres::QuaternionRotatePoint(q_ceres, P_w, P_c); + + P_c[0] += t[0]; + P_c[1] += t[1]; + P_c[2] += t[2]; + + // project 3D object point to the image plane + T xi = params[0]; + T k1 = params[1]; + T k2 = params[2]; + T p1 = params[3]; + T p2 = params[4]; + T gamma1 = params[5]; + T gamma2 = params[6]; + T alpha = T(0); // cameraParams.alpha(); + T u0 = params[7]; + T v0 = params[8]; + + // Transform to model plane + T len = sqrt(P_c[0] * P_c[0] + P_c[1] * P_c[1] + P_c[2] * P_c[2]); + P_c[0] /= len; + P_c[1] /= len; + P_c[2] /= len; + + T u = P_c[0] / (P_c[2] + xi); + T v = P_c[1] / (P_c[2] + xi); + + T rho_sqr = u * u + v * v; + T L = T(1.0) + k1 * rho_sqr + k2 * rho_sqr * rho_sqr; + T du = T(2.0) * p1 * u * v + p2 * (rho_sqr + T(2.0) * u * u); + T dv = p1 * (rho_sqr + T(2.0) * v * v) + T(2.0) * p2 * u * v; + + u = L * u + du; + v = L * v + dv; + p(0) = gamma1 * (u + alpha * v) + u0; + p(1) = gamma2 * v + v0; +} +} + +#endif diff --git a/src/mynteye/api/camodocal/include/camodocal/camera_models/CostFunctionFactory.h b/src/mynteye/api/camodocal/include/camodocal/camera_models/CostFunctionFactory.h new file mode 100644 index 0000000..f5471f8 --- /dev/null +++ b/src/mynteye/api/camodocal/include/camodocal/camera_models/CostFunctionFactory.h @@ -0,0 +1,71 @@ +#ifndef COSTFUNCTIONFACTORY_H +#define COSTFUNCTIONFACTORY_H + +#include +#include + +#include "camodocal/camera_models/Camera.h" + +namespace ceres { +class CostFunction; +} + +namespace camodocal { + +enum { + CAMERA_INTRINSICS = 1 << 0, + CAMERA_POSE = 1 << 1, + POINT_3D = 1 << 2, + ODOMETRY_INTRINSICS = 1 << 3, + ODOMETRY_3D_POSE = 1 << 4, + ODOMETRY_6D_POSE = 1 << 5, + CAMERA_ODOMETRY_TRANSFORM = 1 << 6 +}; + +class CostFunctionFactory { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + CostFunctionFactory(); + + static boost::shared_ptr instance(void); + + ceres::CostFunction *generateCostFunction( + const CameraConstPtr &camera, const Eigen::Vector3d &observed_P, + const Eigen::Vector2d &observed_p, int flags) const; + + ceres::CostFunction *generateCostFunction( + const CameraConstPtr &camera, const Eigen::Vector3d &observed_P, + const Eigen::Vector2d &observed_p, + const Eigen::Matrix2d &sqrtPrecisionMat, int flags) const; + + ceres::CostFunction *generateCostFunction( + const CameraConstPtr &camera, const Eigen::Vector2d &observed_p, + int flags, bool optimize_cam_odo_z = true) const; + + ceres::CostFunction *generateCostFunction( + const CameraConstPtr &camera, const Eigen::Vector2d &observed_p, + const Eigen::Matrix2d &sqrtPrecisionMat, int flags, + bool optimize_cam_odo_z = true) const; + + ceres::CostFunction *generateCostFunction( + const CameraConstPtr &camera, const Eigen::Vector3d &odo_pos, + const Eigen::Vector3d &odo_att, const Eigen::Vector2d &observed_p, + int flags, bool optimize_cam_odo_z = true) const; + + ceres::CostFunction *generateCostFunction( + const CameraConstPtr &camera, const Eigen::Quaterniond &cam_odo_q, + const Eigen::Vector3d &cam_odo_t, const Eigen::Vector3d &odo_pos, + const Eigen::Vector3d &odo_att, const Eigen::Vector2d &observed_p, + int flags) const; + + ceres::CostFunction *generateCostFunction( + const CameraConstPtr &cameraLeft, const CameraConstPtr &cameraRight, + const Eigen::Vector3d &observed_P, const Eigen::Vector2d &observed_p_left, + const Eigen::Vector2d &observed_p_right) const; + + private: + static boost::shared_ptr m_instance; +}; +} + +#endif diff --git a/src/mynteye/api/camodocal/include/camodocal/camera_models/EquidistantCamera.h b/src/mynteye/api/camodocal/include/camodocal/camera_models/EquidistantCamera.h new file mode 100644 index 0000000..9dd6316 --- /dev/null +++ b/src/mynteye/api/camodocal/include/camodocal/camera_models/EquidistantCamera.h @@ -0,0 +1,212 @@ +#ifndef EQUIDISTANTCAMERA_H +#define EQUIDISTANTCAMERA_H + +#include +#include + +#include "Camera.h" +#include "ceres/rotation.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 ¶ms); + + 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 ¶ms); + + 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 > &objectPoints, + const 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 ¶meters); + + void readParameters(const std::vector ¶meterVec); + void writeParameters(std::vector ¶meterVec) 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 boost::shared_ptr EquidistantCameraPtr; +typedef boost::shared_ptr EquidistantCameraConstPtr; + +template +T EquidistantCamera::r(T k2, T k3, T k4, T k5, T theta) { + // k1 = 1 + T theta2 = theta*theta; + T theta3 = theta2*theta; + T theta5 = theta2 * theta3; + T theta7 = theta5 * theta2; + T theta9 = theta7 * theta2; + return theta + k2 * theta3 + k3 * theta5 + k4 * theta7 + k5 * theta9; +// 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; +} + +template +void EquidistantCamera::spaceToPlane( + const T *const params, const T *const q, const T *const t, + const Eigen::Matrix &P, Eigen::Matrix &p) { + T P_w[3]; + P_w[0] = T(P(0)); + P_w[1] = T(P(1)); + P_w[2] = T(P(2)); + + // Convert quaternion from Eigen convention (x, y, z, w) + // to Ceres convention (w, x, y, z) + T q_ceres[4] = {q[3], q[0], q[1], q[2]}; + + T P_c[3]; + ceres::QuaternionRotatePoint(q_ceres, P_w, P_c); + + P_c[0] += t[0]; + P_c[1] += t[1]; + P_c[2] += t[2]; + + // project 3D object point to the image plane; + T k2 = params[0]; + T k3 = params[1]; + T k4 = params[2]; + T k5 = params[3]; + T mu = params[4]; + T mv = params[5]; + T u0 = params[6]; + T v0 = params[7]; + + T len = sqrt(P_c[0] * P_c[0] + P_c[1] * P_c[1] + P_c[2] * P_c[2]); + T theta = acos(P_c[2] / len); + T phi = atan2(P_c[1], P_c[0]); + + Eigen::Matrix p_u = + r(k2, k3, k4, k5, theta) * Eigen::Matrix(cos(phi), sin(phi)); + + p(0) = mu * p_u(0) + u0; + p(1) = mv * p_u(1) + v0; +} +} + +#endif diff --git a/src/mynteye/api/camodocal/include/camodocal/camera_models/PinholeCamera.h b/src/mynteye/api/camodocal/include/camodocal/camera_models/PinholeCamera.h new file mode 100644 index 0000000..ce7e710 --- /dev/null +++ b/src/mynteye/api/camodocal/include/camodocal/camera_models/PinholeCamera.h @@ -0,0 +1,191 @@ +#ifndef PINHOLECAMERA_H +#define PINHOLECAMERA_H + +#include +#include + +#include "Camera.h" +#include "ceres/rotation.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 ¶ms); + + 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 ¶ms); + + 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 > &objectPoints, + const 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 ¶meters); + + void readParameters(const std::vector ¶meterVec); + void writeParameters(std::vector ¶meterVec) 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 boost::shared_ptr PinholeCameraPtr; +typedef boost::shared_ptr PinholeCameraConstPtr; + +template +void PinholeCamera::spaceToPlane( + const T *const params, const T *const q, const T *const t, + const Eigen::Matrix &P, Eigen::Matrix &p) { + T P_w[3]; + P_w[0] = T(P(0)); + P_w[1] = T(P(1)); + P_w[2] = T(P(2)); + + // Convert quaternion from Eigen convention (x, y, z, w) + // to Ceres convention (w, x, y, z) + T q_ceres[4] = {q[3], q[0], q[1], q[2]}; + + T P_c[3]; + ceres::QuaternionRotatePoint(q_ceres, P_w, P_c); + + P_c[0] += t[0]; + P_c[1] += t[1]; + P_c[2] += t[2]; + + // project 3D object point to the image plane + T k1 = params[0]; + T k2 = params[1]; + T p1 = params[2]; + T p2 = params[3]; + T fx = params[4]; + T fy = params[5]; + T alpha = T(0); // cameraParams.alpha(); + T cx = params[6]; + T cy = params[7]; + + // Transform to model plane + T u = P_c[0] / P_c[2]; + T v = P_c[1] / P_c[2]; + + T rho_sqr = u * u + v * v; + T L = T(1.0) + k1 * rho_sqr + k2 * rho_sqr * rho_sqr; + T du = T(2.0) * p1 * u * v + p2 * (rho_sqr + T(2.0) * u * u); + T dv = p1 * (rho_sqr + T(2.0) * v * v) + T(2.0) * p2 * u * v; + + u = L * u + du; + v = L * v + dv; + p(0) = fx * (u + alpha * v) + cx; + p(1) = fy * v + cy; +} +} + +#endif diff --git a/src/mynteye/api/camodocal/include/camodocal/camera_models/ScaramuzzaCamera.h b/src/mynteye/api/camodocal/include/camodocal/camera_models/ScaramuzzaCamera.h new file mode 100644 index 0000000..d666955 --- /dev/null +++ b/src/mynteye/api/camodocal/include/camodocal/camera_models/ScaramuzzaCamera.h @@ -0,0 +1,364 @@ +#ifndef SCARAMUZZACAMERA_H +#define SCARAMUZZACAMERA_H + +#include +#include + +#include "Camera.h" +#include "ceres/rotation.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 ¢er_x(void) { + return m_center_x; + } + double ¢er_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 ¶ms); + + 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 ¶ms); + + 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 > &objectPoints, + const 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 ¶meters); + + void readParameters(const std::vector ¶meterVec); + void writeParameters(std::vector ¶meterVec) const; + + void writeParametersToYamlFile(const std::string &filename) const; + + std::string parametersToString(void) const; + + private: + Parameters mParameters; + + double m_inv_scale; +}; + +typedef boost::shared_ptr OCAMCameraPtr; +typedef boost::shared_ptr OCAMCameraConstPtr; + +template +void OCAMCamera::spaceToPlane( + const T *const params, const T *const q, const T *const t, + const Eigen::Matrix &P, Eigen::Matrix &p) { + T P_c[3]; + { + T P_w[3]; + P_w[0] = T(P(0)); + P_w[1] = T(P(1)); + P_w[2] = T(P(2)); + + // Convert quaternion from Eigen convention (x, y, z, w) + // to Ceres convention (w, x, y, z) + T q_ceres[4] = {q[3], q[0], q[1], q[2]}; + + ceres::QuaternionRotatePoint(q_ceres, P_w, P_c); + + P_c[0] += t[0]; + P_c[1] += t[1]; + P_c[2] += t[2]; + } + + T c = params[0]; + T d = params[1]; + T e = params[2]; + T xc[2] = {params[3], params[4]}; + + // T poly[SCARAMUZZA_POLY_SIZE]; + // for (int i=0; i < SCARAMUZZA_POLY_SIZE; i++) + // poly[i] = params[5+i]; + + 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]; +} + +template +void OCAMCamera::spaceToSphere( + const T *const params, const T *const q, const T *const t, + const Eigen::Matrix &P, Eigen::Matrix &P_s) { + T P_c[3]; + { + T P_w[3]; + P_w[0] = T(P(0)); + P_w[1] = T(P(1)); + P_w[2] = T(P(2)); + + // Convert quaternion from Eigen convention (x, y, z, w) + // to Ceres convention (w, x, y, z) + T q_ceres[4] = {q[3], q[0], q[1], q[2]}; + + ceres::QuaternionRotatePoint(q_ceres, P_w, P_c); + + P_c[0] += t[0]; + P_c[1] += t[1]; + P_c[2] += t[2]; + } + + // T poly[SCARAMUZZA_POLY_SIZE]; + // for (int i=0; i < SCARAMUZZA_POLY_SIZE; i++) + // poly[i] = params[5+i]; + + T norm_sqr = P_c[0] * P_c[0] + P_c[1] * P_c[1] + P_c[2] * P_c[2]; + T norm = T(0.0); + if (norm_sqr > T(0.0)) + norm = sqrt(norm_sqr); + + P_s(0) = P_c[0] / norm; + P_s(1) = P_c[1] / norm; + P_s(2) = P_c[2] / norm; +} + +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/src/mynteye/api/camodocal/include/camodocal/chessboard/Chessboard.h b/src/mynteye/api/camodocal/include/camodocal/chessboard/Chessboard.h new file mode 100644 index 0000000..5528462 --- /dev/null +++ b/src/mynteye/api/camodocal/include/camodocal/chessboard/Chessboard.h @@ -0,0 +1,85 @@ +#ifndef CHESSBOARD_H +#define CHESSBOARD_H + +#include +#include + +namespace camodocal { + +// forward declarations +class ChessboardCorner; +typedef boost::shared_ptr ChessboardCornerPtr; +class ChessboardQuad; +typedef boost::shared_ptr ChessboardQuadPtr; + +class Chessboard { + public: + Chessboard(cv::Size boardSize, cv::Mat &image); + + void findCorners(bool useOpenCV = false); + const std::vector &getCorners(void) const; + bool cornersFound(void) const; + + const cv::Mat &getImage(void) const; + const cv::Mat &getSketch(void) const; + + private: + bool findChessboardCorners( + const cv::Mat &image, const cv::Size &patternSize, + std::vector &corners, int flags, bool useOpenCV); + + bool findChessboardCornersImproved( + const cv::Mat &image, const cv::Size &patternSize, + std::vector &corners, int flags); + + void cleanFoundConnectedQuads( + std::vector &quadGroup, cv::Size patternSize); + + void findConnectedQuads( + std::vector &quads, + std::vector &group, int group_idx, int dilation); + + // int checkQuadGroup(std::vector& quadGroup, + // std::vector& outCorners, + // cv::Size patternSize); + + void labelQuadGroup( + std::vector &quad_group, cv::Size patternSize, + bool firstRun); + + void findQuadNeighbors(std::vector &quads, int dilation); + + int augmentBestRun( + std::vector &candidateQuads, int candidateDilation, + std::vector &existingQuads, int existingDilation); + + void generateQuads( + std::vector &quads, cv::Mat &image, int flags, + int dilation, bool firstRun); + + bool checkQuadGroup( + std::vector &quads, + std::vector &corners, cv::Size patternSize); + + void getQuadrangleHypotheses( + const std::vector > &contours, + std::vector > &quads, int classId) const; + + bool checkChessboard(const cv::Mat &image, cv::Size patternSize) const; + + bool checkBoardMonotony( + std::vector &corners, cv::Size patternSize); + + bool matchCorners( + ChessboardQuadPtr &quad1, int corner1, ChessboardQuadPtr &quad2, + int corner2) const; + + cv::Mat mImage; + cv::Mat mSketch; + std::vector mCorners; + cv::Size mBoardSize; + bool mCornersFound; +}; +} + +#endif diff --git a/src/mynteye/api/camodocal/include/camodocal/chessboard/ChessboardCorner.h b/src/mynteye/api/camodocal/include/camodocal/chessboard/ChessboardCorner.h new file mode 100644 index 0000000..cdf81b5 --- /dev/null +++ b/src/mynteye/api/camodocal/include/camodocal/chessboard/ChessboardCorner.h @@ -0,0 +1,39 @@ +#ifndef CHESSBOARDCORNER_H +#define CHESSBOARDCORNER_H + +#include +#include + +namespace camodocal { + +class ChessboardCorner; +typedef boost::shared_ptr ChessboardCornerPtr; + +class ChessboardCorner { + public: + ChessboardCorner() : row(0), column(0), needsNeighbor(true), count(0) {} + + float meanDist(int &n) const { + float sum = 0; + n = 0; + for (int i = 0; i < 4; ++i) { + if (neighbors[i].get()) { + float dx = neighbors[i]->pt.x - pt.x; + float dy = neighbors[i]->pt.y - pt.y; + sum += sqrt(dx * dx + dy * dy); + n++; + } + } + return sum / std::max(n, 1); + } + + cv::Point2f pt; // X and y coordinates + int row; // Row and column of the corner + int column; // in the found pattern + bool needsNeighbor; // Does the corner require a neighbor? + int count; // number of corner neighbors + ChessboardCornerPtr neighbors[4]; // pointer to all corner neighbors +}; +} + +#endif diff --git a/src/mynteye/api/camodocal/include/camodocal/chessboard/ChessboardQuad.h b/src/mynteye/api/camodocal/include/camodocal/chessboard/ChessboardQuad.h new file mode 100644 index 0000000..7f6b85a --- /dev/null +++ b/src/mynteye/api/camodocal/include/camodocal/chessboard/ChessboardQuad.h @@ -0,0 +1,27 @@ +#ifndef CHESSBOARDQUAD_H +#define CHESSBOARDQUAD_H + +#include + +#include "camodocal/chessboard/ChessboardCorner.h" + +namespace camodocal { + +class ChessboardQuad; +typedef boost::shared_ptr ChessboardQuadPtr; + +class ChessboardQuad { + public: + ChessboardQuad() + : count(0), group_idx(-1), edge_len(FLT_MAX), labeled(false) {} + + int count; // Number of quad neighbors + int group_idx; // Quad group ID + float edge_len; // Smallest side length^2 + ChessboardCornerPtr corners[4]; // Coordinates of quad corners + ChessboardQuadPtr neighbors[4]; // Pointers of quad neighbors + bool labeled; // Has this corner been labeled? +}; +} + +#endif diff --git a/src/mynteye/api/camodocal/include/camodocal/chessboard/Spline.h b/src/mynteye/api/camodocal/include/camodocal/chessboard/Spline.h new file mode 100644 index 0000000..f8e7c5e --- /dev/null +++ b/src/mynteye/api/camodocal/include/camodocal/chessboard/Spline.h @@ -0,0 +1,336 @@ +/* dynamo:- Event driven molecular dynamics simulator + http://www.marcusbannerman.co.uk/dynamo + Copyright (C) 2011 Marcus N Campbell Bannerman + + This program is free software: you can redistribute it and/or + modify it under the terms of the GNU General Public License + version 3 as published by the Free Software Foundation. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace ublas = boost::numeric::ublas; + +class Spline : private std::vector > { + public: + // The boundary conditions available + enum BC_type { FIXED_1ST_DERIV_BC, FIXED_2ND_DERIV_BC, PARABOLIC_RUNOUT_BC }; + + enum Spline_type { LINEAR, CUBIC }; + + // Constructor takes the boundary conditions as arguments, this + // sets the first derivative (gradient) at the lower and upper + // end points + Spline() + : _valid(false), + _BCLow(FIXED_2ND_DERIV_BC), + _BCHigh(FIXED_2ND_DERIV_BC), + _BCLowVal(0), + _BCHighVal(0), + _type(CUBIC) {} + + typedef std::vector > base; + typedef base::const_iterator const_iterator; + + // Standard STL read-only container stuff + const_iterator begin() const { + return base::begin(); + } + const_iterator end() const { + return base::end(); + } + void clear() { + _valid = false; + base::clear(); + _data.clear(); + } + size_t size() const { + return base::size(); + } + size_t max_size() const { + return base::max_size(); + } + size_t capacity() const { + return base::capacity(); + } + bool empty() const { + return base::empty(); + } + + // Add a point to the spline, and invalidate it so its + // recalculated on the next access + inline void addPoint(double x, double y) { + _valid = false; + base::push_back(std::pair(x, y)); + } + + // Reset the boundary conditions + inline void setLowBC(BC_type BC, double val = 0) { + _BCLow = BC; + _BCLowVal = val; + _valid = false; + } + + inline void setHighBC(BC_type BC, double val = 0) { + _BCHigh = BC; + _BCHighVal = val; + _valid = false; + } + + void setType(Spline_type type) { + _type = type; + _valid = false; + } + + // Check if the spline has been calculated, then generate the + // spline interpolated value + double operator()(double xval) { + if (!_valid) + generate(); + + // Special cases when we're outside the range of the spline points + if (xval <= x(0)) + return lowCalc(xval); + if (xval >= x(size() - 1)) + return highCalc(xval); + + // Check all intervals except the last one + for (std::vector::const_iterator iPtr = _data.begin(); + iPtr != _data.end() - 1; ++iPtr) + if ((xval >= iPtr->x) && (xval <= (iPtr + 1)->x)) + return splineCalc(iPtr, xval); + + return splineCalc(_data.end() - 1, xval); + } + + private: + ///////PRIVATE DATA MEMBERS + struct SplineData { + double x, a, b, c, d; + }; + // vector of calculated spline data + std::vector _data; + // Second derivative at each point + ublas::vector _ddy; + // Tracks whether the spline parameters have been calculated for + // the current set of points + bool _valid; + // The boundary conditions + BC_type _BCLow, _BCHigh; + // The values of the boundary conditions + double _BCLowVal, _BCHighVal; + + Spline_type _type; + + ///////PRIVATE FUNCTIONS + // Function to calculate the value of a given spline at a point xval + inline double splineCalc( + std::vector::const_iterator i, double xval) { + const double lx = xval - i->x; + return ((i->a * lx + i->b) * lx + i->c) * lx + i->d; + } + + inline double lowCalc(double xval) { + const double lx = xval - x(0); + + if (_type == LINEAR) + return lx * _BCHighVal + y(0); + + const double firstDeriv = + (y(1) - y(0)) / h(0) - 2 * h(0) * (_data[0].b + 2 * _data[1].b) / 6; + + switch (_BCLow) { + case FIXED_1ST_DERIV_BC: + return lx * _BCLowVal + y(0); + case FIXED_2ND_DERIV_BC: + return lx * lx * _BCLowVal + firstDeriv * lx + y(0); + case PARABOLIC_RUNOUT_BC: + return lx * lx * _ddy[0] + lx * firstDeriv + y(0); + } + throw std::runtime_error("Unknown BC"); + } + + inline double highCalc(double xval) { + const double lx = xval - x(size() - 1); + + if (_type == LINEAR) + return lx * _BCHighVal + y(size() - 1); + + const double firstDeriv = + 2 * h(size() - 2) * (_ddy[size() - 2] + 2 * _ddy[size() - 1]) / 6 + + (y(size() - 1) - y(size() - 2)) / h(size() - 2); + + switch (_BCHigh) { + case FIXED_1ST_DERIV_BC: + return lx * _BCHighVal + y(size() - 1); + case FIXED_2ND_DERIV_BC: + return lx * lx * _BCHighVal + firstDeriv * lx + y(size() - 1); + case PARABOLIC_RUNOUT_BC: + return lx * lx * _ddy[size() - 1] + lx * firstDeriv + y(size() - 1); + } + throw std::runtime_error("Unknown BC"); + } + + // These just provide access to the point data in a clean way + inline double x(size_t i) const { + return operator[](i).first; + } + inline double y(size_t i) const { + return operator[](i).second; + } + inline double h(size_t i) const { + return x(i + 1) - x(i); + } + + // Invert a arbitrary matrix using the boost ublas library + template + bool InvertMatrix(ublas::matrix A, ublas::matrix &inverse) { + using namespace ublas; + + // create a permutation matrix for the LU-factorization + permutation_matrix pm(A.size1()); + + // perform LU-factorization + int res = lu_factorize(A, pm); + if (res != 0) + return false; + + // create identity matrix of "inverse" + inverse.assign(ublas::identity_matrix(A.size1())); + + // backsubstitute to get the inverse + lu_substitute(A, pm, inverse); + + return true; + } + + // This function will recalculate the spline parameters and store + // them in _data, ready for spline interpolation + void generate() { + if (size() < 2) + throw std::runtime_error("Spline requires at least 2 points"); + + // If any spline points are at the same x location, we have to + // just slightly seperate them + { + bool testPassed(false); + while (!testPassed) { + testPassed = true; + std::sort(base::begin(), base::end()); + + for (base::iterator iPtr = base::begin(); iPtr != base::end() - 1; + ++iPtr) + if (iPtr->first == (iPtr + 1)->first) { + if ((iPtr + 1)->first != 0) + (iPtr + 1)->first += (iPtr + 1)->first * + std::numeric_limits::epsilon() * 10; + else + (iPtr + 1)->first = std::numeric_limits::epsilon() * 10; + testPassed = false; + break; + } + } + } + + const size_t e = size() - 1; + + switch (_type) { + case LINEAR: { + _data.resize(e); + for (size_t i(0); i < e; ++i) { + _data[i].x = x(i); + _data[i].a = 0; + _data[i].b = 0; + _data[i].c = (y(i + 1) - y(i)) / (x(i + 1) - x(i)); + _data[i].d = y(i); + } + break; + } + case CUBIC: { + ublas::matrix A(size(), size()); + for (size_t yv(0); yv <= e; ++yv) + for (size_t xv(0); xv <= e; ++xv) + A(xv, yv) = 0; + + for (size_t i(1); i < e; ++i) { + A(i - 1, i) = h(i - 1); + A(i, i) = 2 * (h(i - 1) + h(i)); + A(i + 1, i) = h(i); + } + + ublas::vector C(size()); + for (size_t xv(0); xv <= e; ++xv) + C(xv) = 0; + + for (size_t i(1); i < e; ++i) + C(i) = 6 * ((y(i + 1) - y(i)) / h(i) - (y(i) - y(i - 1)) / h(i - 1)); + + // Boundary conditions + switch (_BCLow) { + case FIXED_1ST_DERIV_BC: + C(0) = 6 * ((y(1) - y(0)) / h(0) - _BCLowVal); + A(0, 0) = 2 * h(0); + A(1, 0) = h(0); + break; + case FIXED_2ND_DERIV_BC: + C(0) = _BCLowVal; + A(0, 0) = 1; + break; + case PARABOLIC_RUNOUT_BC: + C(0) = 0; + A(0, 0) = 1; + A(1, 0) = -1; + break; + } + + switch (_BCHigh) { + case FIXED_1ST_DERIV_BC: + C(e) = 6 * (_BCHighVal - (y(e) - y(e - 1)) / h(e - 1)); + A(e, e) = 2 * h(e - 1); + A(e - 1, e) = h(e - 1); + break; + case FIXED_2ND_DERIV_BC: + C(e) = _BCHighVal; + A(e, e) = 1; + break; + case PARABOLIC_RUNOUT_BC: + C(e) = 0; + A(e, e) = 1; + A(e - 1, e) = -1; + break; + } + + ublas::matrix AInv(size(), size()); + InvertMatrix(A, AInv); + + _ddy = ublas::prod(C, AInv); + + _data.resize(size() - 1); + for (size_t i(0); i < e; ++i) { + _data[i].x = x(i); + _data[i].a = (_ddy(i + 1) - _ddy(i)) / (6 * h(i)); + _data[i].b = _ddy(i) / 2; + _data[i].c = (y(i + 1) - y(i)) / h(i) - _ddy(i + 1) * h(i) / 6 - + _ddy(i) * h(i) / 3; + _data[i].d = y(i); + } + } + } + _valid = true; + } +}; diff --git a/src/mynteye/api/camodocal/include/camodocal/gpl/EigenQuaternionParameterization.h b/src/mynteye/api/camodocal/include/camodocal/gpl/EigenQuaternionParameterization.h new file mode 100644 index 0000000..843d027 --- /dev/null +++ b/src/mynteye/api/camodocal/include/camodocal/gpl/EigenQuaternionParameterization.h @@ -0,0 +1,36 @@ +#ifndef EIGENQUATERNIONPARAMETERIZATION_H +#define EIGENQUATERNIONPARAMETERIZATION_H + +#include "ceres/local_parameterization.h" + +namespace camodocal { + +class EigenQuaternionParameterization : public ceres::LocalParameterization { + public: + virtual ~EigenQuaternionParameterization() {} + virtual bool Plus( + const double *x, const double *delta, double *x_plus_delta) const; + virtual bool ComputeJacobian(const double *x, double *jacobian) const; + virtual int GlobalSize() const { + return 4; + } + virtual int LocalSize() const { + return 3; + } + + private: + template + void EigenQuaternionProduct(const T z[4], const T w[4], T zw[4]) const; +}; + +template +void EigenQuaternionParameterization::EigenQuaternionProduct( + const T z[4], const T w[4], T zw[4]) const { + zw[0] = z[3] * w[0] + z[0] * w[3] + z[1] * w[2] - z[2] * w[1]; + zw[1] = z[3] * w[1] - z[0] * w[2] + z[1] * w[3] + z[2] * w[0]; + zw[2] = z[3] * w[2] + z[0] * w[1] - z[1] * w[0] + z[2] * w[3]; + zw[3] = z[3] * w[3] - z[0] * w[0] - z[1] * w[1] - z[2] * w[2]; +} +} + +#endif diff --git a/src/mynteye/api/camodocal/include/camodocal/gpl/EigenUtils.h b/src/mynteye/api/camodocal/include/camodocal/gpl/EigenUtils.h new file mode 100644 index 0000000..4fee38f --- /dev/null +++ b/src/mynteye/api/camodocal/include/camodocal/gpl/EigenUtils.h @@ -0,0 +1,394 @@ +#ifndef EIGENUTILS_H +#define EIGENUTILS_H + +#include "eigen3/Eigen/Dense" + +#include "camodocal/gpl/gpl.h" +#include "ceres/rotation.h" + +namespace camodocal { + +// Returns the 3D cross product skew symmetric matrix of a given 3D vector +template +Eigen::Matrix skew(const Eigen::Matrix &vec) { + return (Eigen::Matrix() << T(0), -vec(2), vec(1), vec(2), T(0), + -vec(0), -vec(1), vec(0), T(0)) + .finished(); +} + +template +typename Eigen::MatrixBase::PlainObject sqrtm( + const Eigen::MatrixBase &A) { + Eigen::SelfAdjointEigenSolver es(A); + + return es.operatorSqrt(); +} + +template +Eigen::Matrix AngleAxisToRotationMatrix( + const Eigen::Matrix &rvec) { + T angle = rvec.norm(); + if (angle == T(0)) { + return Eigen::Matrix::Identity(); + } + + Eigen::Matrix axis; + axis = rvec.normalized(); + + Eigen::Matrix rmat; + rmat = Eigen::AngleAxis(angle, axis); + + return rmat; +} + +template +Eigen::Quaternion AngleAxisToQuaternion(const Eigen::Matrix &rvec) { + Eigen::Matrix rmat = AngleAxisToRotationMatrix(rvec); + + return Eigen::Quaternion(rmat); +} + +template +void AngleAxisToQuaternion(const Eigen::Matrix &rvec, T *q) { + Eigen::Quaternion quat = AngleAxisToQuaternion(rvec); + + q[0] = quat.x(); + q[1] = quat.y(); + q[2] = quat.z(); + q[3] = quat.w(); +} + +template +Eigen::Matrix RotationToAngleAxis(const Eigen::Matrix &rmat) { + Eigen::AngleAxis angleaxis; + angleaxis.fromRotationMatrix(rmat); + return angleaxis.angle() * angleaxis.axis(); +} + +template +void QuaternionToAngleAxis(const T *const q, Eigen::Matrix &rvec) { + Eigen::Quaternion quat(q[3], q[0], q[1], q[2]); + + Eigen::Matrix rmat = quat.toRotationMatrix(); + + Eigen::AngleAxis angleaxis; + angleaxis.fromRotationMatrix(rmat); + + rvec = angleaxis.angle() * angleaxis.axis(); +} + +template +Eigen::Matrix QuaternionToRotation(const T *const q) { + T R[9]; + ceres::QuaternionToRotation(q, R); + + Eigen::Matrix rmat; + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + rmat(i, j) = R[i * 3 + j]; + } + } + + return rmat; +} + +template +void QuaternionToRotation(const T *const q, T *rot) { + ceres::QuaternionToRotation(q, rot); +} + +template +Eigen::Matrix QuaternionMultMatLeft(const Eigen::Quaternion &q) { + return (Eigen::Matrix() << q.w(), -q.z(), q.y(), q.x(), q.z(), q.w(), + -q.x(), q.y(), -q.y(), q.x(), q.w(), q.z(), -q.x(), -q.y(), -q.z(), + q.w()) + .finished(); +} + +template +Eigen::Matrix QuaternionMultMatRight(const Eigen::Quaternion &q) { + return (Eigen::Matrix() << q.w(), q.z(), -q.y(), q.x(), -q.z(), + q.w(), q.x(), q.y(), q.y(), -q.x(), q.w(), q.z(), -q.x(), -q.y(), + -q.z(), q.w()) + .finished(); +} + +/// @param theta - rotation about screw axis +/// @param d - projection of tvec on the rotation axis +/// @param l - screw axis direction +/// @param m - screw axis moment +template +void AngleAxisAndTranslationToScrew( + const Eigen::Matrix &rvec, const Eigen::Matrix &tvec, + T &theta, T &d, Eigen::Matrix &l, Eigen::Matrix &m) { + theta = rvec.norm(); + if (theta == 0) { + l.setZero(); + m.setZero(); + std::cout << "Warning: Undefined screw! Returned 0. " << std::endl; + } + + l = rvec.normalized(); + + Eigen::Matrix t = tvec; + + d = t.transpose() * l; + + // point on screw axis - projection of origin on screw axis + Eigen::Matrix c; + c = 0.5 * (t - d * l + (1.0 / tan(theta / 2.0) * l).cross(t)); + + // c and hence the screw axis is not defined if theta is either 0 or M_PI + m = c.cross(l); +} + +template +Eigen::Matrix RPY2mat(T roll, T pitch, T yaw) { + Eigen::Matrix m; + + T cr = cos(roll); + T sr = sin(roll); + T cp = cos(pitch); + T sp = sin(pitch); + T cy = cos(yaw); + T sy = sin(yaw); + + m(0, 0) = cy * cp; + m(0, 1) = cy * sp * sr - sy * cr; + m(0, 2) = cy * sp * cr + sy * sr; + m(1, 0) = sy * cp; + m(1, 1) = sy * sp * sr + cy * cr; + m(1, 2) = sy * sp * cr - cy * sr; + m(2, 0) = -sp; + m(2, 1) = cp * sr; + m(2, 2) = cp * cr; + return m; +} + +template +void mat2RPY(const Eigen::Matrix &m, T &roll, T &pitch, T &yaw) { + roll = atan2(m(2, 1), m(2, 2)); + pitch = atan2(-m(2, 0), sqrt(m(2, 1) * m(2, 1) + m(2, 2) * m(2, 2))); + yaw = atan2(m(1, 0), m(0, 0)); +} + +template +Eigen::Matrix homogeneousTransform( + const Eigen::Matrix &R, const Eigen::Matrix &t) { + Eigen::Matrix H; + H.setIdentity(); + + H.block(0, 0, 3, 3) = R; + H.block(0, 3, 3, 1) = t; + + return H; +} + +template +Eigen::Matrix poseWithCartesianTranslation( + const T *const q, const T *const p) { + Eigen::Matrix pose = Eigen::Matrix::Identity(); + + T rotation[9]; + ceres::QuaternionToRotation(q, rotation); + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + pose(i, j) = rotation[i * 3 + j]; + } + } + + pose(0, 3) = p[0]; + pose(1, 3) = p[1]; + pose(2, 3) = p[2]; + + return pose; +} + +template +Eigen::Matrix poseWithSphericalTranslation( + const T *const q, const T *const p, const T scale = T(1.0)) { + Eigen::Matrix pose = Eigen::Matrix::Identity(); + + T rotation[9]; + ceres::QuaternionToRotation(q, rotation); + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + pose(i, j) = rotation[i * 3 + j]; + } + } + + T theta = p[0]; + T phi = p[1]; + pose(0, 3) = sin(theta) * cos(phi) * scale; + pose(1, 3) = sin(theta) * sin(phi) * scale; + pose(2, 3) = cos(theta) * scale; + + return pose; +} + +// Returns the Sampson error of a given essential matrix and 2 image points +template +T sampsonError( + const Eigen::Matrix &E, const Eigen::Matrix &p1, + const Eigen::Matrix &p2) { + Eigen::Matrix Ex1 = E * p1; + Eigen::Matrix Etx2 = E.transpose() * p2; + + T x2tEx1 = p2.dot(Ex1); + + // compute Sampson error + T err = square(x2tEx1) / (square(Ex1(0, 0)) + square(Ex1(1, 0)) + + square(Etx2(0, 0)) + square(Etx2(1, 0))); + + return err; +} + +// Returns the Sampson error of a given rotation/translation and 2 image points +template +T sampsonError( + const Eigen::Matrix &R, const Eigen::Matrix &t, + const Eigen::Matrix &p1, const Eigen::Matrix &p2) { + // construct essential matrix + Eigen::Matrix E = skew(t) * R; + + Eigen::Matrix Ex1 = E * p1; + Eigen::Matrix Etx2 = E.transpose() * p2; + + T x2tEx1 = p2.dot(Ex1); + + // compute Sampson error + T err = square(x2tEx1) / (square(Ex1(0, 0)) + square(Ex1(1, 0)) + + square(Etx2(0, 0)) + square(Etx2(1, 0))); + + return err; +} + +// Returns the Sampson error of a given rotation/translation and 2 image points +template +T sampsonError( + const Eigen::Matrix &H, const Eigen::Matrix &p1, + const Eigen::Matrix &p2) { + Eigen::Matrix R = H.block(0, 0, 3, 3); + Eigen::Matrix t = H.block(0, 3, 3, 1); + + return sampsonError(R, t, p1, p2); +} + +template +Eigen::Matrix transformPoint( + const Eigen::Matrix &H, const Eigen::Matrix &P) { + Eigen::Matrix P_trans = + H.block(0, 0, 3, 3) * P + H.block(0, 3, 3, 1); + + return P_trans; +} + +template +Eigen::Matrix estimate3DRigidTransform( + const std::vector, + Eigen::aligned_allocator > > + &points1, + const std::vector, + Eigen::aligned_allocator > > + &points2) { + // compute centroids + Eigen::Matrix c1, c2; + c1.setZero(); + c2.setZero(); + + for (size_t i = 0; i < points1.size(); ++i) { + c1 += points1.at(i); + c2 += points2.at(i); + } + + c1 /= points1.size(); + c2 /= points1.size(); + + Eigen::Matrix X(3, points1.size()); + Eigen::Matrix Y(3, points1.size()); + for (size_t i = 0; i < points1.size(); ++i) { + X.col(i) = points1.at(i) - c1; + Y.col(i) = points2.at(i) - c2; + } + + Eigen::Matrix H = X * Y.transpose(); + + Eigen::JacobiSVD > svd( + H, Eigen::ComputeFullU | Eigen::ComputeFullV); + + Eigen::Matrix U = svd.matrixU(); + Eigen::Matrix V = svd.matrixV(); + if (U.determinant() * V.determinant() < 0.0) { + V.col(2) *= -1.0; + } + + Eigen::Matrix R = V * U.transpose(); + Eigen::Matrix t = c2 - R * c1; + + return homogeneousTransform(R, t); +} + +template +Eigen::Matrix estimate3DRigidSimilarityTransform( + const std::vector, + Eigen::aligned_allocator > > + &points1, + const std::vector, + Eigen::aligned_allocator > > + &points2) { + // compute centroids + Eigen::Matrix c1, c2; + c1.setZero(); + c2.setZero(); + + for (size_t i = 0; i < points1.size(); ++i) { + c1 += points1.at(i); + c2 += points2.at(i); + } + + c1 /= points1.size(); + c2 /= points1.size(); + + Eigen::Matrix X(3, points1.size()); + Eigen::Matrix Y(3, points1.size()); + for (size_t i = 0; i < points1.size(); ++i) { + X.col(i) = points1.at(i) - c1; + Y.col(i) = points2.at(i) - c2; + } + + Eigen::Matrix H = X * Y.transpose(); + + Eigen::JacobiSVD > svd( + H, Eigen::ComputeFullU | Eigen::ComputeFullV); + + Eigen::Matrix U = svd.matrixU(); + Eigen::Matrix V = svd.matrixV(); + if (U.determinant() * V.determinant() < 0.0) { + V.col(2) *= -1.0; + } + + Eigen::Matrix R = V * U.transpose(); + + std::vector, + Eigen::aligned_allocator > > + rotatedPoints1(points1.size()); + for (size_t i = 0; i < points1.size(); ++i) { + rotatedPoints1.at(i) = R * (points1.at(i) - c1); + } + + double sum_ss = 0.0, sum_tt = 0.0; + for (size_t i = 0; i < points1.size(); ++i) { + sum_ss += (points1.at(i) - c1).squaredNorm(); + sum_tt += (points2.at(i) - c2).dot(rotatedPoints1.at(i)); + } + + double scale = sum_tt / sum_ss; + + Eigen::Matrix sR = scale * R; + Eigen::Matrix t = c2 - sR * c1; + + return homogeneousTransform(sR, t); +} +} + +#endif diff --git a/src/mynteye/api/camodocal/include/camodocal/gpl/gpl.h b/src/mynteye/api/camodocal/include/camodocal/gpl/gpl.h new file mode 100644 index 0000000..7ffa1f9 --- /dev/null +++ b/src/mynteye/api/camodocal/include/camodocal/gpl/gpl.h @@ -0,0 +1,100 @@ +#ifndef GPL_H +#define GPL_H + +#include +#include +#include + +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 ¢erX, double ¢erY, + 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/src/mynteye/api/camodocal/include/camodocal/sparse_graph/Transform.h b/src/mynteye/api/camodocal/include/camodocal/sparse_graph/Transform.h new file mode 100644 index 0000000..fae6e22 --- /dev/null +++ b/src/mynteye/api/camodocal/include/camodocal/sparse_graph/Transform.h @@ -0,0 +1,35 @@ +#ifndef TRANSFORM_H +#define TRANSFORM_H + +#include +#include "eigen3/Eigen/Dense" +#include + +namespace camodocal { + +class Transform { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + Transform(); + Transform(const Eigen::Matrix4d &H); + + Eigen::Quaterniond &rotation(void); + const Eigen::Quaterniond &rotation(void) const; + double *rotationData(void); + const double *const rotationData(void) const; + + Eigen::Vector3d &translation(void); + const Eigen::Vector3d &translation(void) const; + double *translationData(void); + const double *const translationData(void) const; + + Eigen::Matrix4d toMatrix(void) const; + + private: + Eigen::Quaterniond m_q; + Eigen::Vector3d m_t; +}; +} + +#endif diff --git a/src/mynteye/api/camodocal/src/calib/CameraCalibration.cc b/src/mynteye/api/camodocal/src/calib/CameraCalibration.cc new file mode 100644 index 0000000..c507f1e --- /dev/null +++ b/src/mynteye/api/camodocal/src/calib/CameraCalibration.cc @@ -0,0 +1,493 @@ +#include "camodocal/calib/CameraCalibration.h" + +#include +#include +#include "eigen3/Eigen/Dense" +#include +#include +#include +#include +#include +#include +#include + +#include "camodocal/camera_models/CameraFactory.h" +#include "camodocal/camera_models/CostFunctionFactory.h" +#include "camodocal/gpl/EigenQuaternionParameterization.h" +#include "camodocal/gpl/EigenUtils.h" +#include "camodocal/sparse_graph/Transform.h" + +#include "ceres/ceres.h" +namespace camodocal { + +CameraCalibration::CameraCalibration() + : m_boardSize(cv::Size(0, 0)), m_squareSize(0.0f), m_verbose(false) {} + +CameraCalibration::CameraCalibration( + const Camera::ModelType modelType, const std::string &cameraName, + const cv::Size &imageSize, const cv::Size &boardSize, float squareSize) + : m_boardSize(boardSize), m_squareSize(squareSize), m_verbose(false) { + m_camera = CameraFactory::instance()->generateCamera( + modelType, cameraName, imageSize); +} + +void CameraCalibration::clear(void) { + m_imagePoints.clear(); + m_scenePoints.clear(); +} + +void CameraCalibration::addChessboardData( + const std::vector &corners) { + m_imagePoints.push_back(corners); + + std::vector scenePointsInView; + for (int i = 0; i < m_boardSize.height; ++i) { + for (int j = 0; j < m_boardSize.width; ++j) { + scenePointsInView.push_back( + cv::Point3f(i * m_squareSize, j * m_squareSize, 0.0)); + } + } + m_scenePoints.push_back(scenePointsInView); +} + +bool CameraCalibration::calibrate(void) { + int imageCount = m_imagePoints.size(); + + // compute intrinsic camera parameters and extrinsic parameters for each of + // the views + std::vector rvecs; + std::vector tvecs; + bool ret = calibrateHelper(m_camera, rvecs, tvecs); + + m_cameraPoses = cv::Mat(imageCount, 6, CV_64F); + for (int i = 0; i < imageCount; ++i) { + m_cameraPoses.at(i, 0) = rvecs.at(i).at(0); + m_cameraPoses.at(i, 1) = rvecs.at(i).at(1); + m_cameraPoses.at(i, 2) = rvecs.at(i).at(2); + m_cameraPoses.at(i, 3) = tvecs.at(i).at(0); + m_cameraPoses.at(i, 4) = tvecs.at(i).at(1); + m_cameraPoses.at(i, 5) = tvecs.at(i).at(2); + } + + // Compute measurement covariance. + std::vector > errVec(m_imagePoints.size()); + Eigen::Vector2d errSum = Eigen::Vector2d::Zero(); + size_t errCount = 0; + for (size_t i = 0; i < m_imagePoints.size(); ++i) { + std::vector estImagePoints; + m_camera->projectPoints( + m_scenePoints.at(i), rvecs.at(i), tvecs.at(i), estImagePoints); + + for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j) { + cv::Point2f pObs = m_imagePoints.at(i).at(j); + cv::Point2f pEst = estImagePoints.at(j); + + cv::Point2f err = pObs - pEst; + + errVec.at(i).push_back(err); + + errSum += Eigen::Vector2d(err.x, err.y); + } + + errCount += m_imagePoints.at(i).size(); + } + + Eigen::Vector2d errMean = errSum / static_cast(errCount); + + Eigen::Matrix2d measurementCovariance = Eigen::Matrix2d::Zero(); + for (size_t i = 0; i < errVec.size(); ++i) { + for (size_t j = 0; j < errVec.at(i).size(); ++j) { + cv::Point2f err = errVec.at(i).at(j); + double d0 = err.x - errMean(0); + double d1 = err.y - errMean(1); + + measurementCovariance(0, 0) += d0 * d0; + measurementCovariance(0, 1) += d0 * d1; + measurementCovariance(1, 1) += d1 * d1; + } + } + measurementCovariance /= static_cast(errCount); + measurementCovariance(1, 0) = measurementCovariance(0, 1); + + m_measurementCovariance = measurementCovariance; + + return ret; +} + +int CameraCalibration::sampleCount(void) const { + return m_imagePoints.size(); +} + +std::vector > &CameraCalibration::imagePoints(void) { + return m_imagePoints; +} + +const std::vector > &CameraCalibration::imagePoints( + void) const { + return m_imagePoints; +} + +std::vector > &CameraCalibration::scenePoints(void) { + return m_scenePoints; +} + +const std::vector > &CameraCalibration::scenePoints( + void) const { + return m_scenePoints; +} + +CameraPtr &CameraCalibration::camera(void) { + return m_camera; +} + +const CameraConstPtr CameraCalibration::camera(void) const { + return m_camera; +} + +Eigen::Matrix2d &CameraCalibration::measurementCovariance(void) { + return m_measurementCovariance; +} + +const Eigen::Matrix2d &CameraCalibration::measurementCovariance(void) const { + return m_measurementCovariance; +} + +cv::Mat &CameraCalibration::cameraPoses(void) { + return m_cameraPoses; +} + +const cv::Mat &CameraCalibration::cameraPoses(void) const { + return m_cameraPoses; +} + +void CameraCalibration::drawResults(std::vector &images) const { + std::vector rvecs, tvecs; + + for (size_t i = 0; i < images.size(); ++i) { + cv::Mat rvec(3, 1, CV_64F); + rvec.at(0) = m_cameraPoses.at(i, 0); + rvec.at(1) = m_cameraPoses.at(i, 1); + rvec.at(2) = m_cameraPoses.at(i, 2); + + cv::Mat tvec(3, 1, CV_64F); + tvec.at(0) = m_cameraPoses.at(i, 3); + tvec.at(1) = m_cameraPoses.at(i, 4); + tvec.at(2) = m_cameraPoses.at(i, 5); + + rvecs.push_back(rvec); + tvecs.push_back(tvec); + } + + int drawShiftBits = 4; + int drawMultiplier = 1 << drawShiftBits; + + cv::Scalar green(0, 255, 0); + cv::Scalar red(0, 0, 255); + + for (size_t i = 0; i < images.size(); ++i) { + cv::Mat &image = images.at(i); + if (image.channels() == 1) { + cv::cvtColor(image, image, CV_GRAY2RGB); + } + + std::vector estImagePoints; + m_camera->projectPoints( + m_scenePoints.at(i), rvecs.at(i), tvecs.at(i), estImagePoints); + + float errorSum = 0.0f; + float errorMax = std::numeric_limits::min(); + + for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j) { + cv::Point2f pObs = m_imagePoints.at(i).at(j); + cv::Point2f pEst = estImagePoints.at(j); + + cv::circle( + image, cv::Point( + cvRound(pObs.x * drawMultiplier), + cvRound(pObs.y * drawMultiplier)), + 5, green, 2, CV_AA, drawShiftBits); + + cv::circle( + image, cv::Point( + cvRound(pEst.x * drawMultiplier), + cvRound(pEst.y * drawMultiplier)), + 5, red, 2, CV_AA, drawShiftBits); + + float error = cv::norm(pObs - pEst); + + errorSum += error; + if (error > errorMax) { + errorMax = error; + } + } + + std::ostringstream oss; + oss << "Reprojection error: avg = " << errorSum / m_imagePoints.at(i).size() + << " max = " << errorMax; + + cv::putText( + image, oss.str(), cv::Point(10, image.rows - 10), + cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(255, 255, 255), 1, CV_AA); + } +} + +void CameraCalibration::writeParams(const std::string &filename) const { + m_camera->writeParametersToYamlFile(filename); +} + +bool CameraCalibration::writeChessboardData(const std::string &filename) const { + std::ofstream ofs(filename.c_str(), std::ios::out | std::ios::binary); + if (!ofs.is_open()) { + return false; + } + + writeData(ofs, m_boardSize.width); + writeData(ofs, m_boardSize.height); + writeData(ofs, m_squareSize); + + writeData(ofs, m_measurementCovariance(0, 0)); + writeData(ofs, m_measurementCovariance(0, 1)); + writeData(ofs, m_measurementCovariance(1, 0)); + writeData(ofs, m_measurementCovariance(1, 1)); + + writeData(ofs, m_cameraPoses.rows); + writeData(ofs, m_cameraPoses.cols); + writeData(ofs, m_cameraPoses.type()); + for (int i = 0; i < m_cameraPoses.rows; ++i) { + for (int j = 0; j < m_cameraPoses.cols; ++j) { + writeData(ofs, m_cameraPoses.at(i, j)); + } + } + + writeData(ofs, m_imagePoints.size()); + for (size_t i = 0; i < m_imagePoints.size(); ++i) { + writeData(ofs, m_imagePoints.at(i).size()); + for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j) { + const cv::Point2f &ipt = m_imagePoints.at(i).at(j); + + writeData(ofs, ipt.x); + writeData(ofs, ipt.y); + } + } + + writeData(ofs, m_scenePoints.size()); + for (size_t i = 0; i < m_scenePoints.size(); ++i) { + writeData(ofs, m_scenePoints.at(i).size()); + for (size_t j = 0; j < m_scenePoints.at(i).size(); ++j) { + const cv::Point3f &spt = m_scenePoints.at(i).at(j); + + writeData(ofs, spt.x); + writeData(ofs, spt.y); + writeData(ofs, spt.z); + } + } + + return true; +} + +bool CameraCalibration::readChessboardData(const std::string &filename) { + std::ifstream ifs(filename.c_str(), std::ios::in | std::ios::binary); + if (!ifs.is_open()) { + return false; + } + + readData(ifs, m_boardSize.width); + readData(ifs, m_boardSize.height); + readData(ifs, m_squareSize); + + readData(ifs, m_measurementCovariance(0, 0)); + readData(ifs, m_measurementCovariance(0, 1)); + readData(ifs, m_measurementCovariance(1, 0)); + readData(ifs, m_measurementCovariance(1, 1)); + + int rows, cols, type; + readData(ifs, rows); + readData(ifs, cols); + readData(ifs, type); + m_cameraPoses = cv::Mat(rows, cols, type); + + for (int i = 0; i < m_cameraPoses.rows; ++i) { + for (int j = 0; j < m_cameraPoses.cols; ++j) { + readData(ifs, m_cameraPoses.at(i, j)); + } + } + + size_t nImagePointSets; + readData(ifs, nImagePointSets); + + m_imagePoints.clear(); + m_imagePoints.resize(nImagePointSets); + for (size_t i = 0; i < m_imagePoints.size(); ++i) { + size_t nImagePoints; + readData(ifs, nImagePoints); + m_imagePoints.at(i).resize(nImagePoints); + + for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j) { + cv::Point2f &ipt = m_imagePoints.at(i).at(j); + readData(ifs, ipt.x); + readData(ifs, ipt.y); + } + } + + size_t nScenePointSets; + readData(ifs, nScenePointSets); + + m_scenePoints.clear(); + m_scenePoints.resize(nScenePointSets); + for (size_t i = 0; i < m_scenePoints.size(); ++i) { + size_t nScenePoints; + readData(ifs, nScenePoints); + m_scenePoints.at(i).resize(nScenePoints); + + for (size_t j = 0; j < m_scenePoints.at(i).size(); ++j) { + cv::Point3f &spt = m_scenePoints.at(i).at(j); + readData(ifs, spt.x); + readData(ifs, spt.y); + readData(ifs, spt.z); + } + } + + return true; +} + +void CameraCalibration::setVerbose(bool verbose) { + m_verbose = verbose; +} + +bool CameraCalibration::calibrateHelper( + CameraPtr &camera, std::vector &rvecs, + std::vector &tvecs) const { + rvecs.assign(m_scenePoints.size(), cv::Mat()); + tvecs.assign(m_scenePoints.size(), cv::Mat()); + + // STEP 1: Estimate intrinsics + camera->estimateIntrinsics(m_boardSize, m_scenePoints, m_imagePoints); + + // STEP 2: Estimate extrinsics + for (size_t i = 0; i < m_scenePoints.size(); ++i) { + camera->estimateExtrinsics( + m_scenePoints.at(i), m_imagePoints.at(i), rvecs.at(i), tvecs.at(i)); + } + + if (m_verbose) { + std::cout << "[" << camera->cameraName() << "] " + << "# INFO: " + << "Initial reprojection error: " << std::fixed + << std::setprecision(3) + << camera->reprojectionError( + m_scenePoints, m_imagePoints, rvecs, tvecs) + << " pixels" << std::endl; + } + + // STEP 3: optimization using ceres + optimize(camera, rvecs, tvecs); + + if (m_verbose) { + double err = + camera->reprojectionError(m_scenePoints, m_imagePoints, rvecs, tvecs); + std::cout << "[" << camera->cameraName() << "] " + << "# INFO: Final reprojection error: " << err << " pixels" + << std::endl; + std::cout << "[" << camera->cameraName() << "] " + << "# INFO: " << camera->parametersToString() << std::endl; + } + + return true; +} + +void CameraCalibration::optimize( + CameraPtr &camera, std::vector &rvecs, + std::vector &tvecs) const { + // Use ceres to do optimization + ceres::Problem problem; + + std::vector > transformVec( + rvecs.size()); + for (size_t i = 0; i < rvecs.size(); ++i) { + Eigen::Vector3d rvec; + cv::cv2eigen(rvecs.at(i), rvec); + + transformVec.at(i).rotation() = + Eigen::AngleAxisd(rvec.norm(), rvec.normalized()); + transformVec.at(i).translation() << tvecs[i].at(0), + tvecs[i].at(1), tvecs[i].at(2); + } + + std::vector intrinsicCameraParams; + m_camera->writeParameters(intrinsicCameraParams); + + // create residuals for each observation + for (size_t i = 0; i < m_imagePoints.size(); ++i) { + for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j) { + const cv::Point3f &spt = m_scenePoints.at(i).at(j); + const cv::Point2f &ipt = m_imagePoints.at(i).at(j); + + ceres::CostFunction *costFunction = + CostFunctionFactory::instance()->generateCostFunction( + camera, Eigen::Vector3d(spt.x, spt.y, spt.z), + Eigen::Vector2d(ipt.x, ipt.y), CAMERA_INTRINSICS | CAMERA_POSE); + + ceres::LossFunction *lossFunction = new ceres::CauchyLoss(1.0); + problem.AddResidualBlock( + costFunction, lossFunction, intrinsicCameraParams.data(), + transformVec.at(i).rotationData(), + transformVec.at(i).translationData()); + } + + ceres::LocalParameterization *quaternionParameterization = + new EigenQuaternionParameterization; + + problem.SetParameterization( + transformVec.at(i).rotationData(), quaternionParameterization); + } + + std::cout << "begin ceres" << std::endl; + ceres::Solver::Options options; + options.max_num_iterations = 1000; + options.num_threads = 1; + + if (m_verbose) { + options.minimizer_progress_to_stdout = true; + } + + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + std::cout << "end ceres" << std::endl; + + if (m_verbose) { + std::cout << summary.FullReport() << std::endl; + } + + camera->readParameters(intrinsicCameraParams); + + for (size_t i = 0; i < rvecs.size(); ++i) { + Eigen::AngleAxisd aa(transformVec.at(i).rotation()); + + Eigen::Vector3d rvec = aa.angle() * aa.axis(); + cv::eigen2cv(rvec, rvecs.at(i)); + + cv::Mat &tvec = tvecs.at(i); + tvec.at(0) = transformVec.at(i).translation()(0); + tvec.at(1) = transformVec.at(i).translation()(1); + tvec.at(2) = transformVec.at(i).translation()(2); + } +} + +template +void CameraCalibration::readData(std::ifstream &ifs, T &data) const { + char *buffer = new char[sizeof(T)]; + + ifs.read(buffer, sizeof(T)); + + data = *(reinterpret_cast(buffer)); + + delete[] buffer; +} + +template +void CameraCalibration::writeData(std::ofstream &ofs, T data) const { + char *pData = reinterpret_cast(&data); + + ofs.write(pData, sizeof(T)); +} +} diff --git a/src/mynteye/api/camodocal/src/calib/StereoCameraCalibration.cc b/src/mynteye/api/camodocal/src/calib/StereoCameraCalibration.cc new file mode 100644 index 0000000..1d8e925 --- /dev/null +++ b/src/mynteye/api/camodocal/src/calib/StereoCameraCalibration.cc @@ -0,0 +1,370 @@ +#include "camodocal/calib/StereoCameraCalibration.h" + +#include +#include + +#include "camodocal/EigenUtils.h" +#include "camodocal/camera_models/CameraFactory.h" +#include "camodocal/camera_models/CostFunctionFactory.h" +#include "camodocal/gpl/EigenQuaternionParameterization.h" +#include "ceres/ceres.h" + +namespace camodocal { + +StereoCameraCalibration::StereoCameraCalibration( + Camera::ModelType modelType, const std::string &cameraLeftName, + const std::string &cameraRightName, const cv::Size &imageSize, + const cv::Size &boardSize, float squareSize) + : m_calibLeft(modelType, cameraLeftName, imageSize, boardSize, squareSize), + m_calibRight( + modelType, cameraRightName, imageSize, boardSize, squareSize), + m_verbose(false) { + stereo_error.resize(2, 0.0); + } + +void StereoCameraCalibration::clear(void) { + m_calibLeft.clear(); + m_calibRight.clear(); +} + +void StereoCameraCalibration::addChessboardData( + const std::vector &cornersLeft, + const std::vector &cornersRight) { + m_calibLeft.addChessboardData(cornersLeft); + m_calibRight.addChessboardData(cornersRight); +} + +bool StereoCameraCalibration::calibrate(void) { + // calibrate cameras individually + if (!m_calibLeft.calibrate()) { + return false; + } + std::cout << "left_calibrate complete." << std::endl; + if (!m_calibRight.calibrate()) { + return false; + } + std::cout << "right_calibrate complete." << std::endl; + + // perform stereo calibration + int imageCount = imagePointsLeft().size(); + + // find best estimate for initial transform from left camera frame to right + // camera frame + double minReprojErr = std::numeric_limits::max(); + for (int i = 0; i < imageCount; ++i) { + Eigen::Vector3d rvec; + rvec << m_calibLeft.cameraPoses().at(i, 0), + m_calibLeft.cameraPoses().at(i, 1), + m_calibLeft.cameraPoses().at(i, 2); + + Eigen::Quaterniond q_l = AngleAxisToQuaternion(rvec); + + Eigen::Vector3d t_l; + t_l << m_calibLeft.cameraPoses().at(i, 3), + m_calibLeft.cameraPoses().at(i, 4), + m_calibLeft.cameraPoses().at(i, 5); + + rvec << m_calibRight.cameraPoses().at(i, 0), + m_calibRight.cameraPoses().at(i, 1), + m_calibRight.cameraPoses().at(i, 2); + + Eigen::Quaterniond q_r = AngleAxisToQuaternion(rvec); + + Eigen::Vector3d t_r; + t_r << m_calibRight.cameraPoses().at(i, 3), + m_calibRight.cameraPoses().at(i, 4), + m_calibRight.cameraPoses().at(i, 5); + + Eigen::Quaterniond q_l_r = q_r * q_l.conjugate(); + Eigen::Vector3d t_l_r = -q_l_r.toRotationMatrix() * t_l + t_r; + + std::vector rvecs(imageCount); + std::vector tvecs(imageCount); + + for (int j = 0; j < imageCount; ++j) { + rvec << m_calibLeft.cameraPoses().at(j, 0), + m_calibLeft.cameraPoses().at(j, 1), + m_calibLeft.cameraPoses().at(j, 2); + + q_l = AngleAxisToQuaternion(rvec); + + t_l << m_calibLeft.cameraPoses().at(j, 3), + m_calibLeft.cameraPoses().at(j, 4), + m_calibLeft.cameraPoses().at(j, 5); + + Eigen::Quaterniond q_r = q_l_r * q_l; + Eigen::Vector3d t_r = q_l_r.toRotationMatrix() * t_l + t_l_r; + + QuaternionToAngleAxis(q_r.coeffs().data(), rvec); + cv::eigen2cv(rvec, rvecs.at(j)); + + cv::eigen2cv(t_r, tvecs.at(j)); + } + + double reprojErr = cameraRight()->reprojectionError( + scenePoints(), imagePointsRight(), rvecs, tvecs); + + if (reprojErr < minReprojErr) { + minReprojErr = reprojErr; + m_q = q_l_r; + m_t = t_l_r; + } + } + + std::vector rvecsL(imageCount); + std::vector tvecsL(imageCount); + std::vector rvecsR(imageCount); + std::vector tvecsR(imageCount); + + double *extrinsicCameraLParams[scenePoints().size()]; + for (int i = 0; i < imageCount; ++i) { + extrinsicCameraLParams[i] = new double[7]; + + Eigen::Vector3d rvecL( + m_calibLeft.cameraPoses().at(i, 0), + m_calibLeft.cameraPoses().at(i, 1), + m_calibLeft.cameraPoses().at(i, 2)); + + AngleAxisToQuaternion(rvecL, extrinsicCameraLParams[i]); + + extrinsicCameraLParams[i][4] = m_calibLeft.cameraPoses().at(i, 3); + extrinsicCameraLParams[i][5] = m_calibLeft.cameraPoses().at(i, 4); + extrinsicCameraLParams[i][6] = m_calibLeft.cameraPoses().at(i, 5); + + cv::eigen2cv(rvecL, rvecsL.at(i)); + + Eigen::Vector3d tvecL; + tvecL << m_calibLeft.cameraPoses().at(i, 3), + m_calibLeft.cameraPoses().at(i, 4), + m_calibLeft.cameraPoses().at(i, 5); + + cv::eigen2cv(tvecL, tvecsL.at(i)); + + Eigen::Quaterniond q_r = m_q * AngleAxisToQuaternion(rvecL); + Eigen::Vector3d t_r = m_q.toRotationMatrix() * tvecL + m_t; + + Eigen::Vector3d rvecR; + QuaternionToAngleAxis(q_r.coeffs().data(), rvecR); + + cv::eigen2cv(rvecR, rvecsR.at(i)); + cv::eigen2cv(t_r, tvecsR.at(i)); + } + + if (m_verbose) { + double roll, pitch, yaw; + mat2RPY(m_q.toRotationMatrix(), roll, pitch, yaw); + + std::cout << "[stereo]" + << "# INFO: Initial extrinsics: " << std::endl + << "r: " << roll << " p: " << pitch << " yaw: " << yaw + << std::endl + << "x: " << m_t(0) << " y: " << m_t(1) << " z: " << m_t(2) + << std::endl; + + double error = cameraLeft()->reprojectionError( + scenePoints(), imagePointsLeft(), rvecsL, tvecsL); + std::cout << "[" << cameraLeft()->cameraName() << "] " + << "# INFO: Initial reprojection error: " << error << " pixels" + << std::endl; + + error = cameraRight()->reprojectionError( + scenePoints(), imagePointsRight(), rvecsR, tvecsR); + std::cout << "[" << cameraRight()->cameraName() << "] " + << "# INFO: Initial reprojection error: " << error << " pixels" + << std::endl; + } + + std::vector intrinsicCameraLParams; + cameraLeft()->writeParameters(intrinsicCameraLParams); + + std::vector intrinsicCameraRParams; + cameraRight()->writeParameters(intrinsicCameraRParams); + + ceres::Problem problem; + + for (int i = 0; i < imageCount; ++i) { + for (size_t j = 0; j < scenePoints().at(i).size(); ++j) { + const cv::Point3f &spt = scenePoints().at(i).at(j); + const cv::Point2f &iptL = imagePointsLeft().at(i).at(j); + const cv::Point2f &iptR = imagePointsRight().at(i).at(j); + + ceres::CostFunction *costFunction = + CostFunctionFactory::instance()->generateCostFunction( + cameraLeft(), cameraRight(), Eigen::Vector3d(spt.x, spt.y, spt.z), + Eigen::Vector2d(iptL.x, iptL.y), Eigen::Vector2d(iptR.x, iptR.y)); + + ceres::LossFunction *lossFunction = new ceres::CauchyLoss(1.0); + problem.AddResidualBlock( + costFunction, lossFunction, intrinsicCameraLParams.data(), + intrinsicCameraRParams.data(), extrinsicCameraLParams[i], + extrinsicCameraLParams[i] + 4, m_q.coeffs().data(), m_t.data()); + } + } + + for (int i = 0; i < imageCount; ++i) { + ceres::LocalParameterization *quaternionParameterization = + new EigenQuaternionParameterization; + + problem.SetParameterization( + extrinsicCameraLParams[i], quaternionParameterization); + } + + ceres::LocalParameterization *quaternionParameterization = + new EigenQuaternionParameterization; + + problem.SetParameterization(m_q.coeffs().data(), quaternionParameterization); + + ceres::Solver::Options options; + options.max_num_iterations = 1000; + options.num_threads = 8; + + if (m_verbose) { + options.minimizer_progress_to_stdout = true; + } + + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + + if (m_verbose) { + std::cout << summary.FullReport() << "\n"; + } + + cameraLeft()->readParameters(intrinsicCameraLParams); + cameraRight()->readParameters(intrinsicCameraRParams); + + for (int i = 0; i < imageCount; ++i) { + Eigen::Vector3d rvecL; + QuaternionToAngleAxis(extrinsicCameraLParams[i], rvecL); + + m_calibLeft.cameraPoses().at(i, 0) = rvecL(0); + m_calibLeft.cameraPoses().at(i, 1) = rvecL(1); + m_calibLeft.cameraPoses().at(i, 2) = rvecL(2); + m_calibLeft.cameraPoses().at(i, 3) = extrinsicCameraLParams[i][4]; + m_calibLeft.cameraPoses().at(i, 4) = extrinsicCameraLParams[i][5]; + m_calibLeft.cameraPoses().at(i, 5) = extrinsicCameraLParams[i][6]; + + cv::eigen2cv(rvecL, rvecsL.at(i)); + + Eigen::Vector3d tvecL; + tvecL << extrinsicCameraLParams[i][4], extrinsicCameraLParams[i][5], + extrinsicCameraLParams[i][6]; + + cv::eigen2cv(tvecL, tvecsL.at(i)); + + Eigen::Quaterniond q_r = m_q * AngleAxisToQuaternion(rvecL); + Eigen::Vector3d t_r = m_q.toRotationMatrix() * tvecL + m_t; + + Eigen::Vector3d rvecR; + QuaternionToAngleAxis(q_r.coeffs().data(), rvecR); + + m_calibRight.cameraPoses().at(i, 0) = rvecR(0); + m_calibRight.cameraPoses().at(i, 1) = rvecR(1); + m_calibRight.cameraPoses().at(i, 2) = rvecR(2); + m_calibRight.cameraPoses().at(i, 3) = t_r(0); + m_calibRight.cameraPoses().at(i, 4) = t_r(1); + m_calibRight.cameraPoses().at(i, 5) = t_r(2); + + cv::eigen2cv(rvecR, rvecsR.at(i)); + cv::eigen2cv(t_r, tvecsR.at(i)); + } + + if (m_verbose) { + double roll, pitch, yaw; + mat2RPY(m_q.toRotationMatrix(), roll, pitch, yaw); + + std::cout << "[stereo]" + << "# INFO: Final extrinsics: " << std::endl + << "r: " << roll << " p: " << pitch << " yaw: " << yaw + << std::endl + << "x: " << m_t(0) << " y: " << m_t(1) << " z: " << m_t(2) + << std::endl; + + stereo_error[0] = cameraLeft()->reprojectionError( + scenePoints(), imagePointsLeft(), rvecsL, tvecsL); + std::cout << "[" << cameraLeft()->cameraName() << "] " + << "# INFO: Final reprojection error: " << stereo_error[0] << " pixels" + << std::endl; + std::cout << "[" << cameraLeft()->cameraName() << "] " + << "# INFO: " << cameraLeft()->parametersToString() << std::endl; + + stereo_error[1] = cameraRight()->reprojectionError( + scenePoints(), imagePointsRight(), rvecsR, tvecsR); + std::cout << "[" << cameraRight()->cameraName() << "] " + << "# INFO: Final reprojection error: " << stereo_error[1] << " pixels" + << std::endl; + std::cout << "[" << cameraRight()->cameraName() << "] " + << "# INFO: " << cameraRight()->parametersToString() << std::endl; + } + + return true; +} + +int StereoCameraCalibration::sampleCount(void) const { + return m_calibLeft.sampleCount(); +} + +const std::vector > + &StereoCameraCalibration::imagePointsLeft(void) const { + return m_calibLeft.imagePoints(); +} + +const std::vector > + &StereoCameraCalibration::imagePointsRight(void) const { + return m_calibRight.imagePoints(); +} + +const std::vector > + &StereoCameraCalibration::scenePoints(void) const { + return m_calibLeft.scenePoints(); +} + +CameraPtr &StereoCameraCalibration::cameraLeft(void) { + return m_calibLeft.camera(); +} + +const CameraConstPtr StereoCameraCalibration::cameraLeft(void) const { + return m_calibLeft.camera(); +} + +CameraPtr &StereoCameraCalibration::cameraRight(void) { + return m_calibRight.camera(); +} + +const CameraConstPtr StereoCameraCalibration::cameraRight(void) const { + return m_calibRight.camera(); +} + +void StereoCameraCalibration::drawResults( + std::vector &imagesLeft, std::vector &imagesRight) const { + m_calibLeft.drawResults(imagesLeft); + m_calibRight.drawResults(imagesRight); +} + +void StereoCameraCalibration::writeParams(const std::string &directory) const { + if (!boost::filesystem::exists(directory)) { + boost::filesystem::create_directory(directory); + } + + cameraLeft()->writeParametersToYamlFile(directory + "/camera_left.yaml"); + cameraRight()->writeParametersToYamlFile(directory + "/camera_right.yaml"); + + cv::FileStorage fs(directory + "/extrinsics.yaml", cv::FileStorage::WRITE); + + fs << "transform"; + fs << "{" + << "q_x" << m_q.x() << "q_y" << m_q.y() << "q_z" << m_q.z() << "q_w" + << m_q.w() << "t_x" << m_t(0) << "t_y" << m_t(1) << "t_z" << m_t(2) << "}"; + + fs.release(); + cv::FileStorage error_file(directory + "/stereo_reprojection_error.yaml", cv::FileStorage::WRITE); + error_file << "left_reprojection_error" << stereo_error[0]; + error_file << "right_reprojection_error" << stereo_error[1]; + error_file.release(); +} + +void StereoCameraCalibration::setVerbose(bool verbose) { + m_verbose = verbose; + m_calibLeft.setVerbose(verbose); + m_calibRight.setVerbose(verbose); +} +} diff --git a/src/mynteye/api/camodocal/src/camera_models/.PinholeCamera.cc.swm b/src/mynteye/api/camodocal/src/camera_models/.PinholeCamera.cc.swm new file mode 100644 index 0000000..cec593e Binary files /dev/null and b/src/mynteye/api/camodocal/src/camera_models/.PinholeCamera.cc.swm differ diff --git a/src/mynteye/api/camodocal/src/camera_models/Camera.cc b/src/mynteye/api/camodocal/src/camera_models/Camera.cc new file mode 100644 index 0000000..1a663c5 --- /dev/null +++ b/src/mynteye/api/camodocal/src/camera_models/Camera.cc @@ -0,0 +1,206 @@ +#include "camodocal/camera_models/Camera.h" +#include "camodocal/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 > &objectPoints, + const std::vector > &imagePoints, + const std::vector &rvecs, const std::vector &tvecs, + cv::OutputArray _perViewErrors) const { + int imageCount = objectPoints.size(); + size_t pointsSoFar = 0; + double totalErr = 0.0; + + 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/src/mynteye/api/camodocal/src/camera_models/CameraFactory.cc b/src/mynteye/api/camodocal/src/camera_models/CameraFactory.cc new file mode 100644 index 0000000..f76784d --- /dev/null +++ b/src/mynteye/api/camodocal/src/camera_models/CameraFactory.cc @@ -0,0 +1,140 @@ +#include "camodocal/camera_models/CameraFactory.h" + +#include + +#include "camodocal/camera_models/CataCamera.h" +#include "camodocal/camera_models/EquidistantCamera.h" +#include "camodocal/camera_models/PinholeCamera.h" +#include "camodocal/camera_models/ScaramuzzaCamera.h" + +#include "ceres/ceres.h" + +namespace camodocal { + +boost::shared_ptr CameraFactory::m_instance; + +CameraFactory::CameraFactory() {} + +boost::shared_ptr 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::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()) { + std::cout << "# ERROR: can not open " << filename << std::endl; + return CameraPtr(); + } + + Camera::ModelType modelType = Camera::MEI; + if (!fs["model_type"].isNone()) { + std::string sModelType; + fs["model_type"] >> sModelType; + + if (boost::iequals(sModelType, "kannala_brandt")) { + modelType = Camera::KANNALA_BRANDT; + } else if (boost::iequals(sModelType, "mei")) { + modelType = Camera::MEI; + } else if (boost::iequals(sModelType, "scaramuzza")) { + modelType = Camera::SCARAMUZZA; + } else if (boost::iequals(sModelType, "pinhole")) { + modelType = Camera::PINHOLE; + } else { + 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::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/src/mynteye/api/camodocal/src/camera_models/CataCamera.cc b/src/mynteye/api/camodocal/src/camera_models/CataCamera.cc new file mode 100644 index 0000000..bc81580 --- /dev/null +++ b/src/mynteye/api/camodocal/src/camera_models/CataCamera.cc @@ -0,0 +1,863 @@ +#include "camodocal/camera_models/CataCamera.h" + +#include +#include +#include "eigen3/Eigen/Dense" +#include +#include +#include +#include +#include + +#include "camodocal/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 ¶ms) { + 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 ¶ms) + : 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 > &objectPoints, + const 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 ¶meters) { + 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 ¶meterVec) { + 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 ¶meterVec) 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/src/mynteye/api/camodocal/src/camera_models/CostFunctionFactory.cc b/src/mynteye/api/camodocal/src/camera_models/CostFunctionFactory.cc new file mode 100644 index 0000000..0b49443 --- /dev/null +++ b/src/mynteye/api/camodocal/src/camera_models/CostFunctionFactory.cc @@ -0,0 +1,1203 @@ +#include "camodocal/camera_models/CostFunctionFactory.h" + +#include "camodocal/camera_models/CataCamera.h" +#include "camodocal/camera_models/EquidistantCamera.h" +#include "camodocal/camera_models/PinholeCamera.h" +#include "camodocal/camera_models/ScaramuzzaCamera.h" +#include "ceres/ceres.h" + +namespace camodocal { + +template +void worldToCameraTransform( + const T *const q_cam_odo, const T *const t_cam_odo, const T *const p_odo, + const T *const att_odo, T *q, T *t, bool optimize_cam_odo_z = true) { + Eigen::Quaternion q_z_inv( + cos(att_odo[0] / T(2)), T(0), T(0), -sin(att_odo[0] / T(2))); + Eigen::Quaternion q_y_inv( + cos(att_odo[1] / T(2)), T(0), -sin(att_odo[1] / T(2)), T(0)); + Eigen::Quaternion q_x_inv( + cos(att_odo[2] / T(2)), -sin(att_odo[2] / T(2)), T(0), T(0)); + + Eigen::Quaternion q_zyx_inv = q_x_inv * q_y_inv * q_z_inv; + + T q_odo[4] = {q_zyx_inv.w(), q_zyx_inv.x(), q_zyx_inv.y(), q_zyx_inv.z()}; + + T q_odo_cam[4] = {q_cam_odo[3], -q_cam_odo[0], -q_cam_odo[1], -q_cam_odo[2]}; + + T q0[4]; + ceres::QuaternionProduct(q_odo_cam, q_odo, q0); + + T t0[3]; + T t_odo[3] = {p_odo[0], p_odo[1], p_odo[2]}; + + ceres::QuaternionRotatePoint(q_odo, t_odo, t0); + + t0[0] += t_cam_odo[0]; + t0[1] += t_cam_odo[1]; + + if (optimize_cam_odo_z) { + t0[2] += t_cam_odo[2]; + } + + ceres::QuaternionRotatePoint(q_odo_cam, t0, t); + t[0] = -t[0]; + t[1] = -t[1]; + t[2] = -t[2]; + + // Convert quaternion from Ceres convention (w, x, y, z) + // to Eigen convention (x, y, z, w) + q[0] = q0[1]; + q[1] = q0[2]; + q[2] = q0[3]; + q[3] = q0[0]; +} + +template +class ReprojectionError1 { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + ReprojectionError1( + const Eigen::Vector3d &observed_P, const Eigen::Vector2d &observed_p) + : m_observed_P(observed_P), + m_observed_p(observed_p), + m_sqrtPrecisionMat(Eigen::Matrix2d::Identity()) {} + + ReprojectionError1( + const Eigen::Vector3d &observed_P, const Eigen::Vector2d &observed_p, + const Eigen::Matrix2d &sqrtPrecisionMat) + : m_observed_P(observed_P), + m_observed_p(observed_p), + m_sqrtPrecisionMat(sqrtPrecisionMat) {} + + ReprojectionError1( + const std::vector &intrinsic_params, + const Eigen::Vector3d &observed_P, const Eigen::Vector2d &observed_p) + : m_intrinsic_params(intrinsic_params), + m_observed_P(observed_P), + m_observed_p(observed_p) {} + + // variables: camera intrinsics and camera extrinsics + template + bool operator()( + const T *const intrinsic_params, const T *const q, const T *const t, + T *residuals) const { + Eigen::Matrix P = m_observed_P.cast(); + + Eigen::Matrix predicted_p; + CameraT::spaceToPlane(intrinsic_params, q, t, P, predicted_p); + + Eigen::Matrix e = predicted_p - m_observed_p.cast(); + + Eigen::Matrix e_weighted = m_sqrtPrecisionMat.cast() * e; + + residuals[0] = e_weighted(0); + residuals[1] = e_weighted(1); + + return true; + } + + // variables: camera-odometry transforms and odometry poses + template + bool operator()( + const T *const q_cam_odo, const T *const t_cam_odo, const T *const p_odo, + const T *const att_odo, T *residuals) const { + T q[4], t[3]; + worldToCameraTransform(q_cam_odo, t_cam_odo, p_odo, att_odo, q, t); + + Eigen::Matrix P = m_observed_P.cast(); + + std::vector intrinsic_params( + m_intrinsic_params.begin(), m_intrinsic_params.end()); + + // project 3D object point to the image plane + Eigen::Matrix predicted_p; + CameraT::spaceToPlane(intrinsic_params.data(), q, t, P, predicted_p); + + residuals[0] = predicted_p(0) - T(m_observed_p(0)); + residuals[1] = predicted_p(1) - T(m_observed_p(1)); + + return true; + } + + // private: + // camera intrinsics + std::vector m_intrinsic_params; + + // observed 3D point + Eigen::Vector3d m_observed_P; + + // observed 2D point + Eigen::Vector2d m_observed_p; + + // square root of precision matrix + Eigen::Matrix2d m_sqrtPrecisionMat; +}; + +// variables: camera extrinsics, 3D point +template +class ReprojectionError2 { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + ReprojectionError2( + const std::vector &intrinsic_params, + const Eigen::Vector2d &observed_p) + : m_intrinsic_params(intrinsic_params), m_observed_p(observed_p) {} + + template + bool operator()( + const T *const q, const T *const t, const T *const point, + T *residuals) const { + Eigen::Matrix P; + P(0) = T(point[0]); + P(1) = T(point[1]); + P(2) = T(point[2]); + + std::vector intrinsic_params( + m_intrinsic_params.begin(), m_intrinsic_params.end()); + + // project 3D object point to the image plane + Eigen::Matrix predicted_p; + CameraT::spaceToPlane(intrinsic_params.data(), q, t, P, predicted_p); + + residuals[0] = predicted_p(0) - T(m_observed_p(0)); + residuals[1] = predicted_p(1) - T(m_observed_p(1)); + + return true; + } + + private: + // camera intrinsics + std::vector m_intrinsic_params; + + // observed 2D point + Eigen::Vector2d m_observed_p; +}; + +template +class ReprojectionError3 { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + ReprojectionError3(const Eigen::Vector2d &observed_p) + : m_observed_p(observed_p), + m_sqrtPrecisionMat(Eigen::Matrix2d::Identity()), + m_optimize_cam_odo_z(true) {} + + ReprojectionError3( + const Eigen::Vector2d &observed_p, + const Eigen::Matrix2d &sqrtPrecisionMat) + : m_observed_p(observed_p), + m_sqrtPrecisionMat(sqrtPrecisionMat), + m_optimize_cam_odo_z(true) {} + + ReprojectionError3( + const std::vector &intrinsic_params, + const Eigen::Vector2d &observed_p) + : m_intrinsic_params(intrinsic_params), + m_observed_p(observed_p), + m_sqrtPrecisionMat(Eigen::Matrix2d::Identity()), + m_optimize_cam_odo_z(true) {} + + ReprojectionError3( + const std::vector &intrinsic_params, + const Eigen::Vector2d &observed_p, + const Eigen::Matrix2d &sqrtPrecisionMat) + : m_intrinsic_params(intrinsic_params), + m_observed_p(observed_p), + m_sqrtPrecisionMat(sqrtPrecisionMat), + m_optimize_cam_odo_z(true) {} + + ReprojectionError3( + const std::vector &intrinsic_params, + const Eigen::Vector3d &odo_pos, const Eigen::Vector3d &odo_att, + const Eigen::Vector2d &observed_p, bool optimize_cam_odo_z) + : m_intrinsic_params(intrinsic_params), + m_odo_pos(odo_pos), + m_odo_att(odo_att), + m_observed_p(observed_p), + m_optimize_cam_odo_z(optimize_cam_odo_z) {} + + ReprojectionError3( + const std::vector &intrinsic_params, + const Eigen::Quaterniond &cam_odo_q, const Eigen::Vector3d &cam_odo_t, + const Eigen::Vector3d &odo_pos, const Eigen::Vector3d &odo_att, + const Eigen::Vector2d &observed_p) + : m_intrinsic_params(intrinsic_params), + m_cam_odo_q(cam_odo_q), + m_cam_odo_t(cam_odo_t), + m_odo_pos(odo_pos), + m_odo_att(odo_att), + m_observed_p(observed_p), + m_optimize_cam_odo_z(true) {} + + // variables: camera intrinsics, camera-to-odometry transform, + // odometry extrinsics, 3D point + template + bool operator()( + const T *const intrinsic_params, const T *const q_cam_odo, + const T *const t_cam_odo, const T *const p_odo, const T *const att_odo, + const T *const point, T *residuals) const { + T q[4], t[3]; + worldToCameraTransform( + q_cam_odo, t_cam_odo, p_odo, att_odo, q, t, m_optimize_cam_odo_z); + + Eigen::Matrix P(point[0], point[1], point[2]); + + // project 3D object point to the image plane + Eigen::Matrix predicted_p; + CameraT::spaceToPlane(intrinsic_params, q, t, P, predicted_p); + + Eigen::Matrix err = predicted_p - m_observed_p.cast(); + Eigen::Matrix err_weighted = m_sqrtPrecisionMat.cast() * err; + + residuals[0] = err_weighted(0); + residuals[1] = err_weighted(1); + + return true; + } + + // variables: camera-to-odometry transform, 3D point + template + bool operator()( + const T *const q_cam_odo, const T *const t_cam_odo, const T *const point, + T *residuals) const { + T p_odo[3] = {T(m_odo_pos(0)), T(m_odo_pos(1)), T(m_odo_pos(2))}; + T att_odo[3] = {T(m_odo_att(0)), T(m_odo_att(1)), T(m_odo_att(2))}; + T q[4], t[3]; + + worldToCameraTransform( + q_cam_odo, t_cam_odo, p_odo, att_odo, q, t, m_optimize_cam_odo_z); + + std::vector intrinsic_params( + m_intrinsic_params.begin(), m_intrinsic_params.end()); + Eigen::Matrix P(point[0], point[1], point[2]); + + // project 3D object point to the image plane + Eigen::Matrix predicted_p; + CameraT::spaceToPlane(intrinsic_params.data(), q, t, P, predicted_p); + + residuals[0] = predicted_p(0) - T(m_observed_p(0)); + residuals[1] = predicted_p(1) - T(m_observed_p(1)); + + return true; + } + + // variables: camera-to-odometry transform, odometry extrinsics, 3D point + template + bool operator()( + const T *const q_cam_odo, const T *const t_cam_odo, const T *const p_odo, + const T *const att_odo, const T *const point, T *residuals) const { + T q[4], t[3]; + worldToCameraTransform( + q_cam_odo, t_cam_odo, p_odo, att_odo, q, t, m_optimize_cam_odo_z); + + std::vector intrinsic_params( + m_intrinsic_params.begin(), m_intrinsic_params.end()); + Eigen::Matrix P(point[0], point[1], point[2]); + + // project 3D object point to the image plane + Eigen::Matrix predicted_p; + CameraT::spaceToPlane(intrinsic_params.data(), q, t, P, predicted_p); + + Eigen::Matrix err = predicted_p - m_observed_p.cast(); + Eigen::Matrix err_weighted = m_sqrtPrecisionMat.cast() * err; + + residuals[0] = err_weighted(0); + residuals[1] = err_weighted(1); + + return true; + } + + // variables: 3D point + template + bool operator()(const T *const point, T *residuals) const { + T q_cam_odo[4] = {T(m_cam_odo_q.coeffs()(0)), T(m_cam_odo_q.coeffs()(1)), + T(m_cam_odo_q.coeffs()(2)), T(m_cam_odo_q.coeffs()(3))}; + T t_cam_odo[3] = {T(m_cam_odo_t(0)), T(m_cam_odo_t(1)), T(m_cam_odo_t(2))}; + T p_odo[3] = {T(m_odo_pos(0)), T(m_odo_pos(1)), T(m_odo_pos(2))}; + T att_odo[3] = {T(m_odo_att(0)), T(m_odo_att(1)), T(m_odo_att(2))}; + T q[4], t[3]; + + worldToCameraTransform( + q_cam_odo, t_cam_odo, p_odo, att_odo, q, t, m_optimize_cam_odo_z); + + std::vector intrinsic_params( + m_intrinsic_params.begin(), m_intrinsic_params.end()); + Eigen::Matrix P(point[0], point[1], point[2]); + + // project 3D object point to the image plane + Eigen::Matrix predicted_p; + CameraT::spaceToPlane(intrinsic_params.data(), q, t, P, predicted_p); + + residuals[0] = predicted_p(0) - T(m_observed_p(0)); + residuals[1] = predicted_p(1) - T(m_observed_p(1)); + + return true; + } + + private: + // camera intrinsics + std::vector m_intrinsic_params; + + // observed camera-odometry transform + Eigen::Quaterniond m_cam_odo_q; + Eigen::Vector3d m_cam_odo_t; + + // observed odometry + Eigen::Vector3d m_odo_pos; + Eigen::Vector3d m_odo_att; + + // observed 2D point + Eigen::Vector2d m_observed_p; + + Eigen::Matrix2d m_sqrtPrecisionMat; + + bool m_optimize_cam_odo_z; +}; + +// variables: camera intrinsics and camera extrinsics +template +class StereoReprojectionError { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + StereoReprojectionError( + const Eigen::Vector3d &observed_P, const Eigen::Vector2d &observed_p_l, + const Eigen::Vector2d &observed_p_r) + : m_observed_P(observed_P), + m_observed_p_l(observed_p_l), + m_observed_p_r(observed_p_r) {} + + template + bool operator()( + const T *const intrinsic_params_l, const T *const intrinsic_params_r, + const T *const q_l, const T *const t_l, const T *const q_l_r, + const T *const t_l_r, T *residuals) const { + Eigen::Matrix P; + P(0) = T(m_observed_P(0)); + P(1) = T(m_observed_P(1)); + P(2) = T(m_observed_P(2)); + + Eigen::Matrix predicted_p_l; + CameraT::spaceToPlane(intrinsic_params_l, q_l, t_l, P, predicted_p_l); + + Eigen::Quaternion q_r = + Eigen::Quaternion(q_l_r) * Eigen::Quaternion(q_l); + + Eigen::Matrix t_r; + t_r(0) = t_l[0]; + t_r(1) = t_l[1]; + t_r(2) = t_l[2]; + + t_r = Eigen::Quaternion(q_l_r) * t_r; + t_r(0) += t_l_r[0]; + t_r(1) += t_l_r[1]; + t_r(2) += t_l_r[2]; + + Eigen::Matrix predicted_p_r; + CameraT::spaceToPlane( + intrinsic_params_r, q_r.coeffs().data(), t_r.data(), P, predicted_p_r); + + residuals[0] = predicted_p_l(0) - T(m_observed_p_l(0)); + residuals[1] = predicted_p_l(1) - T(m_observed_p_l(1)); + residuals[2] = predicted_p_r(0) - T(m_observed_p_r(0)); + residuals[3] = predicted_p_r(1) - T(m_observed_p_r(1)); + + return true; + } + + private: + // observed 3D point + Eigen::Vector3d m_observed_P; + + // observed 2D point + Eigen::Vector2d m_observed_p_l; + Eigen::Vector2d m_observed_p_r; +}; + +template +class ComprehensionError { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + ComprehensionError( + const Eigen::Vector3d &observed_P, const Eigen::Vector2d &observed_p) + : m_observed_P(observed_P), + m_observed_p(observed_p), + m_sqrtPrecisionMat(Eigen::Matrix2d::Identity()) {} + + template + bool operator()( + const T *const intrinsic_params, const T *const q, const T *const t, + T *residuals) const { + { + Eigen::Matrix p = m_observed_p.cast(); + + Eigen::Matrix predicted_img_P; + CameraT::LiftToSphere(intrinsic_params, p, predicted_img_P); + + Eigen::Matrix predicted_p; + CameraT::SphereToPlane(intrinsic_params, predicted_img_P, predicted_p); + + Eigen::Matrix e = predicted_p - m_observed_p.cast(); + + Eigen::Matrix e_weighted = m_sqrtPrecisionMat.cast() * e; + + residuals[0] = e_weighted(0); + residuals[1] = e_weighted(1); + } + + { + Eigen::Matrix P = m_observed_P.cast(); + + Eigen::Matrix predicted_p; + CameraT::spaceToPlane(intrinsic_params, q, t, P, predicted_p); + + Eigen::Matrix e = predicted_p - m_observed_p.cast(); + + Eigen::Matrix e_weighted = m_sqrtPrecisionMat.cast() * e; + + residuals[2] = e_weighted(0); + residuals[3] = e_weighted(1); + } + + return true; + } + + // private: + // camera intrinsics + // std::vector m_intrinsic_params; + + // observed 3D point + Eigen::Vector3d m_observed_P; + + // observed 2D point + Eigen::Vector2d m_observed_p; + + // square root of precision matrix + Eigen::Matrix2d m_sqrtPrecisionMat; +}; + +boost::shared_ptr CostFunctionFactory::m_instance; + +CostFunctionFactory::CostFunctionFactory() {} + +boost::shared_ptr CostFunctionFactory::instance(void) { + if (m_instance.get() == 0) { + m_instance.reset(new CostFunctionFactory); + } + + return m_instance; +} + +ceres::CostFunction *CostFunctionFactory::generateCostFunction( + const CameraConstPtr &camera, const Eigen::Vector3d &observed_P, + const Eigen::Vector2d &observed_p, int flags) const { + ceres::CostFunction *costFunction = 0; + + std::vector intrinsic_params; + camera->writeParameters(intrinsic_params); + + switch (flags) { + case CAMERA_INTRINSICS | CAMERA_POSE: + switch (camera->modelType()) { + case Camera::KANNALA_BRANDT: + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError1, 2, 8, 4, 3>( + new ReprojectionError1( + observed_P, observed_p)); + break; + case Camera::PINHOLE: + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError1, 2, 8, 4, 3>( + new ReprojectionError1(observed_P, observed_p)); + break; + case Camera::MEI: + costFunction = + new ceres::AutoDiffCostFunction, 2, + 9, 4, 3>( + new ReprojectionError1(observed_P, observed_p)); + break; + case Camera::SCARAMUZZA: + costFunction = + new ceres::AutoDiffCostFunction, 4, + SCARAMUZZA_CAMERA_NUM_PARAMS, 4, + 3>( + new ComprehensionError(observed_P, observed_p)); + // new ceres::AutoDiffCostFunction, 2, + // SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3>( + // new ReprojectionError1(observed_P, observed_p)); + break; + } + break; + case CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE: + switch (camera->modelType()) { + case Camera::KANNALA_BRANDT: + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError1, 2, 4, 3, 3, 3>( + new ReprojectionError1( + intrinsic_params, observed_P, observed_p)); + break; + case Camera::PINHOLE: + costFunction = + new ceres::AutoDiffCostFunction, + 2, 4, 3, 3, 3>( + new ReprojectionError1( + intrinsic_params, observed_P, observed_p)); + break; + case Camera::MEI: + costFunction = + new ceres::AutoDiffCostFunction, 2, + 4, 3, 3, 3>( + new ReprojectionError1( + intrinsic_params, observed_P, observed_p)); + break; + case Camera::SCARAMUZZA: + costFunction = + new ceres::AutoDiffCostFunction, 2, + 4, 3, 3, 3>( + new ReprojectionError1( + intrinsic_params, observed_P, observed_p)); + break; + } + break; + } + + return costFunction; +} + +ceres::CostFunction *CostFunctionFactory::generateCostFunction( + const CameraConstPtr &camera, const Eigen::Vector3d &observed_P, + const Eigen::Vector2d &observed_p, const Eigen::Matrix2d &sqrtPrecisionMat, + int flags) const { + ceres::CostFunction *costFunction = 0; + + std::vector intrinsic_params; + camera->writeParameters(intrinsic_params); + + switch (flags) { + case CAMERA_INTRINSICS | CAMERA_POSE: + switch (camera->modelType()) { + case Camera::KANNALA_BRANDT: + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError1, 2, 8, 4, 3>( + new ReprojectionError1( + observed_P, observed_p, sqrtPrecisionMat)); + break; + case Camera::PINHOLE: + costFunction = + new ceres::AutoDiffCostFunction, + 2, 8, 4, 3>( + new ReprojectionError1( + observed_P, observed_p, sqrtPrecisionMat)); + break; + case Camera::MEI: + costFunction = + new ceres::AutoDiffCostFunction, 2, + 9, 4, 3>( + new ReprojectionError1( + observed_P, observed_p, sqrtPrecisionMat)); + break; + case Camera::SCARAMUZZA: + costFunction = + new ceres::AutoDiffCostFunction, 2, + SCARAMUZZA_CAMERA_NUM_PARAMS, 4, + 3>( + new ReprojectionError1( + observed_P, observed_p, sqrtPrecisionMat)); + break; + } + break; + } + + return costFunction; +} + +ceres::CostFunction *CostFunctionFactory::generateCostFunction( + const CameraConstPtr &camera, const Eigen::Vector2d &observed_p, int flags, + bool optimize_cam_odo_z) const { + ceres::CostFunction *costFunction = 0; + + std::vector intrinsic_params; + camera->writeParameters(intrinsic_params); + + switch (flags) { + case CAMERA_POSE | POINT_3D: + switch (camera->modelType()) { + case Camera::KANNALA_BRANDT: + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError2, 2, 4, 3, 3>( + new ReprojectionError2( + intrinsic_params, observed_p)); + break; + case Camera::PINHOLE: + costFunction = + new ceres::AutoDiffCostFunction, + 2, 4, 3, 3>( + new ReprojectionError2( + intrinsic_params, observed_p)); + break; + case Camera::MEI: + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError2, 2, 4, 3, 3>( + new ReprojectionError2(intrinsic_params, observed_p)); + break; + case Camera::SCARAMUZZA: + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError2, 2, 4, 3, 3>( + new ReprojectionError2(intrinsic_params, observed_p)); + break; + } + break; + case CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_3D_POSE | POINT_3D: + switch (camera->modelType()) { + case Camera::KANNALA_BRANDT: + if (optimize_cam_odo_z) { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 4, 3, 2, 1, 3>( + new ReprojectionError3( + intrinsic_params, observed_p)); + } else { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 4, 2, 2, 1, 3>( + new ReprojectionError3( + intrinsic_params, observed_p)); + } + break; + case Camera::PINHOLE: + if (optimize_cam_odo_z) { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 4, 3, 2, 1, 3>( + new ReprojectionError3( + intrinsic_params, observed_p)); + } else { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 4, 2, 2, 1, 3>( + new ReprojectionError3( + intrinsic_params, observed_p)); + } + break; + case Camera::MEI: + if (optimize_cam_odo_z) { + costFunction = + new ceres::AutoDiffCostFunction, + 2, 4, 3, 2, 1, 3>( + new ReprojectionError3( + intrinsic_params, observed_p)); + } else { + costFunction = + new ceres::AutoDiffCostFunction, + 2, 4, 2, 2, 1, 3>( + new ReprojectionError3( + intrinsic_params, observed_p)); + } + break; + case Camera::SCARAMUZZA: + if (optimize_cam_odo_z) { + costFunction = + new ceres::AutoDiffCostFunction, + 2, 4, 3, 2, 1, 3>( + new ReprojectionError3( + intrinsic_params, observed_p)); + } else { + costFunction = + new ceres::AutoDiffCostFunction, + 2, 4, 2, 2, 1, 3>( + new ReprojectionError3( + intrinsic_params, observed_p)); + } + break; + } + break; + case CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE | POINT_3D: + switch (camera->modelType()) { + case Camera::KANNALA_BRANDT: + if (optimize_cam_odo_z) { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 4, 3, 3, 3, 3>( + new ReprojectionError3( + intrinsic_params, observed_p)); + } else { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 4, 2, 3, 3, 3>( + new ReprojectionError3( + intrinsic_params, observed_p)); + } + break; + case Camera::PINHOLE: + if (optimize_cam_odo_z) { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 4, 3, 3, 3, 3>( + new ReprojectionError3( + intrinsic_params, observed_p)); + } else { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 4, 2, 3, 3, 3>( + new ReprojectionError3( + intrinsic_params, observed_p)); + } + break; + case Camera::MEI: + if (optimize_cam_odo_z) { + costFunction = + new ceres::AutoDiffCostFunction, + 2, 4, 3, 3, 3, 3>( + new ReprojectionError3( + intrinsic_params, observed_p)); + } else { + costFunction = + new ceres::AutoDiffCostFunction, + 2, 4, 2, 3, 3, 3>( + new ReprojectionError3( + intrinsic_params, observed_p)); + } + break; + case Camera::SCARAMUZZA: + if (optimize_cam_odo_z) { + costFunction = + new ceres::AutoDiffCostFunction, + 2, 4, 3, 3, 3, 3>( + new ReprojectionError3( + intrinsic_params, observed_p)); + } else { + costFunction = + new ceres::AutoDiffCostFunction, + 2, 4, 2, 3, 3, 3>( + new ReprojectionError3( + intrinsic_params, observed_p)); + } + break; + } + break; + case CAMERA_INTRINSICS | CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_3D_POSE | + POINT_3D: + switch (camera->modelType()) { + case Camera::KANNALA_BRANDT: + if (optimize_cam_odo_z) { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 8, 4, 3, 2, 1, 3>( + new ReprojectionError3(observed_p)); + } else { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 8, 4, 2, 2, 1, 3>( + new ReprojectionError3(observed_p)); + } + break; + case Camera::PINHOLE: + if (optimize_cam_odo_z) { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 8, 4, 3, 2, 1, 3>( + new ReprojectionError3(observed_p)); + } else { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 8, 4, 2, 2, 1, 3>( + new ReprojectionError3(observed_p)); + } + break; + case Camera::MEI: + if (optimize_cam_odo_z) { + costFunction = + new ceres::AutoDiffCostFunction, + 2, 9, 4, 3, 2, 1, 3>( + new ReprojectionError3(observed_p)); + } else { + costFunction = + new ceres::AutoDiffCostFunction, + 2, 9, 4, 2, 2, 1, 3>( + new ReprojectionError3(observed_p)); + } + break; + case Camera::SCARAMUZZA: + if (optimize_cam_odo_z) { + costFunction = + new ceres::AutoDiffCostFunction, + 2, SCARAMUZZA_CAMERA_NUM_PARAMS, + 4, 3, 2, 1, 3>( + new ReprojectionError3(observed_p)); + } else { + costFunction = + new ceres::AutoDiffCostFunction, + 2, SCARAMUZZA_CAMERA_NUM_PARAMS, + 4, 2, 2, 1, 3>( + new ReprojectionError3(observed_p)); + } + break; + } + break; + case CAMERA_INTRINSICS | CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE | + POINT_3D: + switch (camera->modelType()) { + case Camera::KANNALA_BRANDT: + if (optimize_cam_odo_z) { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 8, 4, 3, 3, 3, 3>( + new ReprojectionError3(observed_p)); + } else { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 8, 4, 2, 3, 3, 3>( + new ReprojectionError3(observed_p)); + } + break; + case Camera::PINHOLE: + if (optimize_cam_odo_z) { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 8, 4, 3, 3, 3, 3>( + new ReprojectionError3(observed_p)); + } else { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 8, 4, 2, 3, 3, 3>( + new ReprojectionError3(observed_p)); + } + break; + case Camera::MEI: + if (optimize_cam_odo_z) { + costFunction = + new ceres::AutoDiffCostFunction, + 2, 9, 4, 3, 3, 3, 3>( + new ReprojectionError3(observed_p)); + } else { + costFunction = + new ceres::AutoDiffCostFunction, + 2, 9, 4, 2, 3, 3, 3>( + new ReprojectionError3(observed_p)); + } + break; + case Camera::SCARAMUZZA: + if (optimize_cam_odo_z) { + costFunction = + new ceres::AutoDiffCostFunction, + 2, SCARAMUZZA_CAMERA_NUM_PARAMS, + 4, 3, 3, 3, 3>( + new ReprojectionError3(observed_p)); + } else { + costFunction = + new ceres::AutoDiffCostFunction, + 2, SCARAMUZZA_CAMERA_NUM_PARAMS, + 4, 2, 3, 3, 3>( + new ReprojectionError3(observed_p)); + } + break; + } + break; + } + + return costFunction; +} + +ceres::CostFunction *CostFunctionFactory::generateCostFunction( + const CameraConstPtr &camera, const Eigen::Vector2d &observed_p, + const Eigen::Matrix2d &sqrtPrecisionMat, int flags, + bool optimize_cam_odo_z) const { + ceres::CostFunction *costFunction = 0; + + std::vector intrinsic_params; + camera->writeParameters(intrinsic_params); + + switch (flags) { + case CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE | POINT_3D: + switch (camera->modelType()) { + case Camera::KANNALA_BRANDT: + if (optimize_cam_odo_z) { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 4, 3, 3, 3, 3>( + new ReprojectionError3( + intrinsic_params, observed_p, sqrtPrecisionMat)); + } else { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 4, 2, 3, 3, 3>( + new ReprojectionError3( + intrinsic_params, observed_p, sqrtPrecisionMat)); + } + break; + case Camera::PINHOLE: + if (optimize_cam_odo_z) { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 4, 3, 3, 3, 3>( + new ReprojectionError3( + intrinsic_params, observed_p, sqrtPrecisionMat)); + } else { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 4, 2, 3, 3, 3>( + new ReprojectionError3( + intrinsic_params, observed_p, sqrtPrecisionMat)); + } + break; + case Camera::MEI: + if (optimize_cam_odo_z) { + costFunction = + new ceres::AutoDiffCostFunction, + 2, 4, 3, 3, 3, 3>( + new ReprojectionError3( + intrinsic_params, observed_p, sqrtPrecisionMat)); + } else { + costFunction = + new ceres::AutoDiffCostFunction, + 2, 4, 2, 3, 3, 3>( + new ReprojectionError3( + intrinsic_params, observed_p, sqrtPrecisionMat)); + } + break; + case Camera::SCARAMUZZA: + if (optimize_cam_odo_z) { + costFunction = + new ceres::AutoDiffCostFunction, + 2, 4, 3, 3, 3, 3>( + new ReprojectionError3( + intrinsic_params, observed_p, sqrtPrecisionMat)); + } else { + costFunction = + new ceres::AutoDiffCostFunction, + 2, 4, 2, 3, 3, 3>( + new ReprojectionError3( + intrinsic_params, observed_p, sqrtPrecisionMat)); + } + break; + } + break; + case CAMERA_INTRINSICS | CAMERA_ODOMETRY_TRANSFORM | ODOMETRY_6D_POSE | + POINT_3D: + switch (camera->modelType()) { + case Camera::KANNALA_BRANDT: + if (optimize_cam_odo_z) { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 8, 4, 3, 3, 3, 3>( + new ReprojectionError3( + observed_p, sqrtPrecisionMat)); + } else { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 8, 4, 2, 3, 3, 3>( + new ReprojectionError3( + observed_p, sqrtPrecisionMat)); + } + break; + case Camera::PINHOLE: + if (optimize_cam_odo_z) { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 8, 4, 3, 3, 3, 3>( + new ReprojectionError3( + observed_p, sqrtPrecisionMat)); + } else { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 8, 4, 2, 3, 3, 3>( + new ReprojectionError3( + observed_p, sqrtPrecisionMat)); + } + break; + case Camera::MEI: + if (optimize_cam_odo_z) { + costFunction = + new ceres::AutoDiffCostFunction, + 2, 9, 4, 3, 3, 3, 3>( + new ReprojectionError3( + observed_p, sqrtPrecisionMat)); + } else { + costFunction = + new ceres::AutoDiffCostFunction, + 2, 9, 4, 2, 3, 3, 3>( + new ReprojectionError3( + observed_p, sqrtPrecisionMat)); + } + break; + case Camera::SCARAMUZZA: + if (optimize_cam_odo_z) { + costFunction = + new ceres::AutoDiffCostFunction, + 2, SCARAMUZZA_CAMERA_NUM_PARAMS, + 4, 3, 3, 3, 3>( + new ReprojectionError3( + observed_p, sqrtPrecisionMat)); + } else { + costFunction = + new ceres::AutoDiffCostFunction, + 2, SCARAMUZZA_CAMERA_NUM_PARAMS, + 4, 2, 3, 3, 3>( + new ReprojectionError3( + observed_p, sqrtPrecisionMat)); + } + break; + } + break; + } + + return costFunction; +} + +ceres::CostFunction *CostFunctionFactory::generateCostFunction( + const CameraConstPtr &camera, const Eigen::Vector3d &odo_pos, + const Eigen::Vector3d &odo_att, const Eigen::Vector2d &observed_p, + int flags, bool optimize_cam_odo_z) const { + ceres::CostFunction *costFunction = 0; + + std::vector intrinsic_params; + camera->writeParameters(intrinsic_params); + + switch (flags) { + case CAMERA_ODOMETRY_TRANSFORM | POINT_3D: + switch (camera->modelType()) { + case Camera::KANNALA_BRANDT: + if (optimize_cam_odo_z) { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 4, 3, 3>( + new ReprojectionError3( + intrinsic_params, odo_pos, odo_att, observed_p, + optimize_cam_odo_z)); + } else { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 4, 2, 3>( + new ReprojectionError3( + intrinsic_params, odo_pos, odo_att, observed_p, + optimize_cam_odo_z)); + } + break; + case Camera::PINHOLE: + if (optimize_cam_odo_z) { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 4, 3, 3>( + new ReprojectionError3( + intrinsic_params, odo_pos, odo_att, observed_p, + optimize_cam_odo_z)); + } else { + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 4, 2, 3>( + new ReprojectionError3( + intrinsic_params, odo_pos, odo_att, observed_p, + optimize_cam_odo_z)); + } + break; + case Camera::MEI: + if (optimize_cam_odo_z) { + costFunction = + new ceres::AutoDiffCostFunction, + 2, 4, 3, 3>( + new ReprojectionError3( + intrinsic_params, odo_pos, odo_att, observed_p, + optimize_cam_odo_z)); + } else { + costFunction = + new ceres::AutoDiffCostFunction, + 2, 4, 2, 3>( + new ReprojectionError3( + intrinsic_params, odo_pos, odo_att, observed_p, + optimize_cam_odo_z)); + } + break; + case Camera::SCARAMUZZA: + if (optimize_cam_odo_z) { + costFunction = + new ceres::AutoDiffCostFunction, + 2, 4, 3, 3>( + new ReprojectionError3( + intrinsic_params, odo_pos, odo_att, observed_p, + optimize_cam_odo_z)); + } else { + costFunction = + new ceres::AutoDiffCostFunction, + 2, 4, 2, 3>( + new ReprojectionError3( + intrinsic_params, odo_pos, odo_att, observed_p, + optimize_cam_odo_z)); + } + break; + } + break; + } + + return costFunction; +} + +ceres::CostFunction *CostFunctionFactory::generateCostFunction( + const CameraConstPtr &camera, const Eigen::Quaterniond &cam_odo_q, + const Eigen::Vector3d &cam_odo_t, const Eigen::Vector3d &odo_pos, + const Eigen::Vector3d &odo_att, const Eigen::Vector2d &observed_p, + int flags) const { + ceres::CostFunction *costFunction = 0; + + std::vector intrinsic_params; + camera->writeParameters(intrinsic_params); + + switch (flags) { + case POINT_3D: + switch (camera->modelType()) { + case Camera::KANNALA_BRANDT: + costFunction = new ceres::AutoDiffCostFunction< + ReprojectionError3, 2, 3>( + new ReprojectionError3( + intrinsic_params, cam_odo_q, cam_odo_t, odo_pos, odo_att, + observed_p)); + break; + case Camera::PINHOLE: + costFunction = + new ceres::AutoDiffCostFunction, + 2, 3>( + new ReprojectionError3( + intrinsic_params, cam_odo_q, cam_odo_t, odo_pos, odo_att, + observed_p)); + break; + case Camera::MEI: + costFunction = + new ceres::AutoDiffCostFunction, 2, + 3>( + new ReprojectionError3( + intrinsic_params, cam_odo_q, cam_odo_t, odo_pos, odo_att, + observed_p)); + break; + case Camera::SCARAMUZZA: + costFunction = + new ceres::AutoDiffCostFunction, 2, + 3>( + new ReprojectionError3( + intrinsic_params, cam_odo_q, cam_odo_t, odo_pos, odo_att, + observed_p)); + break; + } + break; + } + + return costFunction; +} + +ceres::CostFunction *CostFunctionFactory::generateCostFunction( + const CameraConstPtr &cameraL, const CameraConstPtr &cameraR, + const Eigen::Vector3d &observed_P, const Eigen::Vector2d &observed_p_l, + const Eigen::Vector2d &observed_p_r) const { + ceres::CostFunction *costFunction = 0; + + if (cameraL->modelType() != cameraR->modelType()) { + return costFunction; + } + + switch (cameraL->modelType()) { + case Camera::KANNALA_BRANDT: + costFunction = new ceres::AutoDiffCostFunction< + StereoReprojectionError, 4, 8, 8, 4, 3, 4, 3>( + new StereoReprojectionError( + observed_P, observed_p_l, observed_p_r)); + break; + case Camera::PINHOLE: + costFunction = new ceres::AutoDiffCostFunction< + StereoReprojectionError, 4, 8, 8, 4, 3, 4, 3>( + new StereoReprojectionError( + observed_P, observed_p_l, observed_p_r)); + break; + case Camera::MEI: + costFunction = + new ceres::AutoDiffCostFunction, + 4, 9, 9, 4, 3, 4, 3>( + new StereoReprojectionError( + observed_P, observed_p_l, observed_p_r)); + break; + case Camera::SCARAMUZZA: + costFunction = new ceres::AutoDiffCostFunction< + StereoReprojectionError, 4, SCARAMUZZA_CAMERA_NUM_PARAMS, + SCARAMUZZA_CAMERA_NUM_PARAMS, 4, 3, 4, 3>( + new StereoReprojectionError( + observed_P, observed_p_l, observed_p_r)); + break; + } + + return costFunction; +} +} diff --git a/src/mynteye/api/camodocal/src/camera_models/EquidistantCamera.cc b/src/mynteye/api/camodocal/src/camera_models/EquidistantCamera.cc new file mode 100644 index 0000000..bea48f9 --- /dev/null +++ b/src/mynteye/api/camodocal/src/camera_models/EquidistantCamera.cc @@ -0,0 +1,733 @@ +#include "camodocal/camera_models/EquidistantCamera.h" +#include +#include +#include +#include "eigen3/Eigen/Dense" +#include +#include +#include +#include +#include + +#include "camodocal/gpl/gpl.h" + +namespace camodocal { +#define PI M_PI +#define PI_2 1.5707963 +float ApproxAtan2(float y, float x) +{ + const float n1 = 0.97239411f; + const float n2 = -0.19194795f; + float result = 0.0f; + if (x != 0.0f) + { + const union { float flVal; std::uint32_t nVal; } tYSign = { y }; + const union { float flVal; std::uint32_t nVal; } tXSign = { x }; + if (fabsf(x) >= fabsf(y)) + { + union { float flVal; std::uint32_t nVal; } tOffset = { PI }; + // Add or subtract PI based on y's sign. + tOffset.nVal |= tYSign.nVal & 0x80000000u; + // No offset if x is positive, so multiply by 0 or based on x's sign. + tOffset.nVal *= tXSign.nVal >> 31; + result = tOffset.flVal; + const float z = y / x; + result += (n1 + n2 * z * z) * z; + } + else // Use atan(y/x) = pi/2 - atan(x/y) if |y/x| > 1. + { + union { float flVal; std::uint32_t nVal; } tOffset = { PI_2 }; + // Add or subtract PI/2 based on y's sign. + tOffset.nVal |= tYSign.nVal & 0x80000000u; + result = tOffset.flVal; + const float z = x / y; + result -= (n1 + n2 * z * z) * z; + } + } + else if (y > 0.0f) + { + result = PI_2; + } + else if (y < 0.0f) + { + result = -PI_2; + } + return result; +} + +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 ¶ms) { + 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 ¶ms) + : mParameters(params) { + // Inverse camera projection matrix parameters + m_inv_K11 = 1.0 / mParameters.mu(); + m_inv_K13 = -mParameters.u0() / mParameters.mu(); + m_inv_K22 = 1.0 / mParameters.mv(); + m_inv_K23 = -mParameters.v0() / mParameters.mv(); +} + +Camera::ModelType EquidistantCamera::modelType(void) const { + return mParameters.modelType(); +} + +const std::string &EquidistantCamera::cameraName(void) const { + return mParameters.cameraName(); +} + +int EquidistantCamera::imageWidth(void) const { + return mParameters.imageWidth(); +} + +int EquidistantCamera::imageHeight(void) const { + return mParameters.imageHeight(); +} + +void EquidistantCamera::estimateIntrinsics( + const cv::Size &boardSize, + const std::vector > &objectPoints, + const std::vector > &imagePoints) { + Parameters params = getParameters(); + + double u0 = params.imageWidth() / 2.0; + double v0 = params.imageHeight() / 2.0; + + double minReprojErr = std::numeric_limits::max(); + + std::vector rvecs, tvecs; + rvecs.assign(objectPoints.size(), cv::Mat()); + tvecs.assign(objectPoints.size(), cv::Mat()); + + params.k2() = 0.0; + params.k3() = 0.0; + params.k4() = 0.0; + params.k5() = 0.0; + params.u0() = u0; + params.v0() = v0; + + // Initialize focal length + // C. Hughes, P. Denny, M. Glavin, and E. Jones, + // Equidistant Fish-Eye Calibration and Rectification by Vanishing Point + // Extraction, PAMI 2010 + // Find circles from rows of chessboard corners, and for each pair + // of circles, find vanishing points: v1 and v2. + // f = ||v1 - v2|| / PI; + double f0 = 0.0; + for (size_t i = 0; i < imagePoints.size(); ++i) { + std::vector center(boardSize.height); + double radius[boardSize.height]; + for (int r = 0; r < boardSize.height; ++r) { + std::vector circle; + for (int c = 0; c < boardSize.width; ++c) { + circle.push_back(imagePoints.at(i).at(r * boardSize.width + c)); + } + + fitCircle(circle, center[r](0), center[r](1), radius[r]); + } + + 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(0.5); + //double theta = 0.5; +// double phi = 0.5; +// Eigen::Vector2d p_u = r(mParameters.k2(), mParameters.k3(), mParameters.k4(), +// mParameters.k5(), theta) * +// Eigen::Vector2d(cos(0.5), sin(0.5)); + double theta = acos(P(2) / P.norm()); + double phi = atan2(P(1), P(0)); + //double phi = ApproxAtan2(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 ¶meters) { + 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 ¶meterVec) { + 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 ¶meterVec) 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/src/mynteye/api/camodocal/src/camera_models/PinholeCamera.cc b/src/mynteye/api/camodocal/src/camera_models/PinholeCamera.cc new file mode 100644 index 0000000..e6cb3f5 --- /dev/null +++ b/src/mynteye/api/camodocal/src/camera_models/PinholeCamera.cc @@ -0,0 +1,752 @@ +#include "camodocal/camera_models/PinholeCamera.h" + +#include +#include +#include "eigen3/Eigen/Dense" +#include +#include +#include +#include + +#include "camodocal/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 ¶ms) { + 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 ¶ms) + : 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 > &objectPoints, + const std::vector > &imagePoints) { + // Z. Zhang, A Flexible New Technique for Camera Calibration, PAMI 2000 + + Parameters params = getParameters(); + + params.k1() = 0.0; + params.k2() = 0.0; + params.p1() = 0.0; + params.p2() = 0.0; + + double cx = params.imageWidth() / 2.0; + double cy = params.imageHeight() / 2.0; + params.cx() = cx; + params.cy() = cy; + + size_t nImages = imagePoints.size(); + + cv::Mat A(nImages * 2, 2, CV_64F); + cv::Mat b(nImages * 2, 1, CV_64F); + + 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 ¶meters) { + 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 ¶meterVec) { + 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 ¶meterVec) 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/src/mynteye/api/camodocal/src/camera_models/ScaramuzzaCamera.cc b/src/mynteye/api/camodocal/src/camera_models/ScaramuzzaCamera.cc new file mode 100644 index 0000000..1392873 --- /dev/null +++ b/src/mynteye/api/camodocal/src/camera_models/ScaramuzzaCamera.cc @@ -0,0 +1,802 @@ +#include "camodocal/camera_models/ScaramuzzaCamera.h" + +#include +#include +#include +#include +#include "eigen3/Eigen/Dense" +#include "eigen3/Eigen/SVD" +#include +#include +#include +#include +#include + +#include "camodocal/gpl/gpl.h" + +Eigen::VectorXd polyfit( + Eigen::VectorXd &xVec, Eigen::VectorXd &yVec, int poly_order) { + assert(poly_order > 0); + assert(xVec.size() > poly_order); + assert(xVec.size() == yVec.size()); + + Eigen::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 (!boost::iequals(sModelType, "scaramuzza")) { + return false; + } + } + + m_modelType = SCARAMUZZA; + fs["camera_name"] >> m_cameraName; + m_imageWidth = static_cast(fs["image_width"]); + m_imageHeight = static_cast(fs["image_height"]); + + cv::FileNode n = fs["poly_parameters"]; + for (int i = 0; i < SCARAMUZZA_POLY_SIZE; i++) + m_poly[i] = static_cast( + n[std::string("p") + boost::lexical_cast(i)]); + + n = fs["inv_poly_parameters"]; + for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++) + m_inv_poly[i] = static_cast( + n[std::string("p") + boost::lexical_cast(i)]); + + n = fs["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") + boost::lexical_cast(i) << m_poly[i]; + fs << "}"; + + fs << "inv_poly_parameters"; + fs << "{"; + for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++) + fs << std::string("p") + boost::lexical_cast(i) + << m_inv_poly[i]; + fs << "}"; + + fs << "affine_parameters"; + fs << "{" + << "ac" << m_C << "ad" << m_D << "ae" << m_E << "cx" << m_center_x << "cy" + << m_center_y << "}"; + + fs.release(); +} + +OCAMCamera::Parameters &OCAMCamera::Parameters::operator=( + const OCAMCamera::Parameters &other) { + if (this != &other) { + m_modelType = other.m_modelType; + m_cameraName = other.m_cameraName; + m_imageWidth = other.m_imageWidth; + m_imageHeight = other.m_imageHeight; + m_C = other.m_C; + m_D = other.m_D; + m_E = other.m_E; + m_center_x = other.m_center_x; + m_center_y = other.m_center_y; + + memcpy(m_poly, other.m_poly, sizeof(double) * SCARAMUZZA_POLY_SIZE); + memcpy( + m_inv_poly, other.m_inv_poly, + sizeof(double) * SCARAMUZZA_INV_POLY_SIZE); + } + + return *this; +} + +std::ostream &operator<<( + std::ostream &out, const OCAMCamera::Parameters ¶ms) { + out << "Camera Parameters:" << std::endl; + out << " model_type " + << "scaramuzza" << std::endl; + out << " camera_name " << params.m_cameraName << std::endl; + out << " image_width " << params.m_imageWidth << std::endl; + out << " image_height " << params.m_imageHeight << std::endl; + + out << std::fixed << std::setprecision(10); + + out << "Poly Parameters" << std::endl; + for (int i = 0; i < SCARAMUZZA_POLY_SIZE; i++) + out << std::string("p") + boost::lexical_cast(i) << ": " + << params.m_poly[i] << std::endl; + + out << "Inverse Poly Parameters" << std::endl; + for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++) + out << std::string("p") + boost::lexical_cast(i) << ": " + << params.m_inv_poly[i] << std::endl; + + out << "Affine Parameters" << std::endl; + out << " ac " << params.m_C << std::endl + << " ad " << params.m_D << std::endl + << " ae " << params.m_E << std::endl; + out << " cx " << params.m_center_x << std::endl + << " cy " << params.m_center_y << std::endl; + + return out; +} + +OCAMCamera::OCAMCamera() : m_inv_scale(0.0) {} + +OCAMCamera::OCAMCamera(const OCAMCamera::Parameters ¶ms) + : 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 > &objectPoints, + const std::vector > &imagePoints) { + // std::cout << "OCAMCamera::estimateIntrinsics - NOT IMPLEMENTED" << + // std::endl; + // throw std::string("OCAMCamera::estimateIntrinsics - NOT IMPLEMENTED"); + + // Reference: Page 30 of + // " Scaramuzza, D. Omnidirectional Vision: from Calibration to Robot Motion + // Estimation, ETH Zurich. Thesis no. 17635." + // http://e-collection.library.ethz.ch/eserv/eth:30301/eth-30301-02.pdf + // Matlab code: calibrate.m + + // First, estimate every image's extrinsics parameters + std::vector RList; + std::vector TList; + + RList.reserve(imagePoints.size()); + TList.reserve(imagePoints.size()); + + // i-th image + for (size_t image_index = 0; image_index < imagePoints.size(); + ++image_index) { + const std::vector &objPts = objectPoints.at(image_index); + const std::vector &imgPts = imagePoints.at(image_index); + + assert(objPts.size() == imgPts.size()); + assert( + objPts.size() == + static_cast(boardSize.width * boardSize.height)); + + Eigen::MatrixXd M(objPts.size(), 6); + + for (size_t corner_index = 0; corner_index < objPts.size(); + ++corner_index) { + double X = objPts.at(corner_index).x; + double Y = objPts.at(corner_index).y; + assert(objPts.at(corner_index).z == 0.0); + + double u = imgPts.at(corner_index).x; + double v = imgPts.at(corner_index).y; + + M(corner_index, 0) = -v * X; + M(corner_index, 1) = -v * Y; + M(corner_index, 2) = u * X; + M(corner_index, 3) = u * Y; + M(corner_index, 4) = -v; + M(corner_index, 5) = u; + } + + 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 < sr31_values.size(); ++i) { + const double sr31 = sr31_values.at(i); + const double sr32 = sr32_values.at(i); + const double lambda = 1.0 / sqrt(sr11 * sr11 + sr21 * sr21 + sr31 * sr31); + Eigen::Matrix3d H; + H.setZero(); + H(0, 0) = sr11; + H(0, 1) = sr12; + H(0, 2) = st1; + H(1, 0) = sr21; + H(1, 1) = sr22; + H(1, 2) = st2; + H(2, 0) = sr31; + H(2, 1) = sr32; + H(2, 2) = 0; + + H_values.push_back(lambda * H); + H_values.push_back(-lambda * H); + } + + for (auto &H : H_values) { + // std::cout << "H=\n" << H << std::endl; + Eigen::Matrix3d R; + R.col(0) = H.col(0); + R.col(1) = H.col(1); + R.col(2) = H.col(0).cross(H.col(1)); + // std::cout << "R33 = " << R(2,2) << std::endl; + } + + std::vector H_candidates; + + for (auto &H : H_values) { + Eigen::MatrixXd A_mat(2 * imagePoints.at(image_index).size(), 4); + Eigen::VectorXd B_vec(2 * imagePoints.at(image_index).size()); + A_mat.setZero(); + B_vec.setZero(); + + size_t line_index = 0; + + // iterate images + const double &r11 = H(0, 0); + const double &r12 = H(0, 1); + // const double& r13 = H(0,2); + const double &r21 = H(1, 0); + const double &r22 = H(1, 1); + // const double& r23 = H(1,2); + const double &r31 = H(2, 0); + const double &r32 = H(2, 1); + // const double& r33 = H(2,2); + const double &t1 = H(0); + const double &t2 = H(1); + + // iterate chessboard corners in the image + for (size_t j = 0; j < imagePoints.at(image_index).size(); ++j) { + assert(line_index == 2 * j); + + const double &X = objectPoints.at(image_index).at(j).x; + const double &Y = objectPoints.at(image_index).at(j).y; + const double &u = imagePoints.at(image_index).at(j).x; + const double &v = imagePoints.at(image_index).at(j).y; + + double A = r21 * X + r22 * Y + t2; + double B = v * (r31 * X + r32 * Y); + double C = r11 * X + r12 * Y + t1; + double D = u * (r31 * X + r32 * Y); + double rou = std::sqrt(u * u + v * v); + + A_mat(line_index + 0, 0) = A; + A_mat(line_index + 1, 0) = C; + A_mat(line_index + 0, 1) = A * rou; + A_mat(line_index + 1, 1) = C * rou; + A_mat(line_index + 0, 2) = A * rou * rou; + A_mat(line_index + 1, 2) = C * rou * rou; + + A_mat(line_index + 0, 3) = -v; + A_mat(line_index + 1, 3) = -u; + B_vec(line_index + 0) = B; + B_vec(line_index + 1) = D; + + line_index += 2; + } + + assert(line_index == static_cast(A_mat.rows())); + + // pseudo-inverse for polynomial parameters and all t3s + { + Eigen::JacobiSVD svd( + A_mat, Eigen::ComputeThinU | Eigen::ComputeThinV); + + Eigen::VectorXd x = svd.solve(B_vec); + + // std::cout << "x(poly and t3) = " << x << std::endl; + + if (x(2) > 0 && x(3) > 0) { + H_candidates.push_back(H); + } + } + } + + // printf("H_candidates.size()=%zu\n", H_candidates.size()); + assert(H_candidates.size() == 1); + + Eigen::Matrix3d &H = H_candidates.front(); + + Eigen::Matrix3d R; + R.col(0) = H.col(0); + R.col(1) = H.col(1); + R.col(2) = H.col(0).cross(H.col(1)); + + Eigen::Vector3d T = H.col(2); + RList.push_back(R); + TList.push_back(T); + + // std::cout << "#" << image_index << " frame" << " R =" << R << " \nT = " + // << T.transpose() << std::endl; + } + + // Second, estimate camera intrinsic parameters and all t3 + Eigen::MatrixXd A_mat( + 2 * imagePoints.size() * imagePoints.at(0).size(), + SCARAMUZZA_POLY_SIZE - 1 + imagePoints.size()); + Eigen::VectorXd B_vec(2 * imagePoints.size() * imagePoints.at(0).size()); + A_mat.setZero(); + B_vec.setZero(); + + size_t line_index = 0; + + // iterate images + for (size_t i = 0; i < imagePoints.size(); ++i) { + const double &r11 = RList.at(i)(0, 0); + const double &r12 = RList.at(i)(0, 1); + // const double& r13 = RList.at(i)(0,2); + const double &r21 = RList.at(i)(1, 0); + const double &r22 = RList.at(i)(1, 1); + // const double& r23 = RList.at(i)(1,2); + const double &r31 = RList.at(i)(2, 0); + const double &r32 = RList.at(i)(2, 1); + // const double& r33 = RList.at(i)(2,2); + const double &t1 = TList.at(i)(0); + const double &t2 = TList.at(i)(1); + + // iterate chessboard corners in the image + for (size_t j = 0; j < imagePoints.at(i).size(); ++j) { + assert(line_index == 2 * (i * imagePoints.at(0).size() + j)); + + const double &X = objectPoints.at(i).at(j).x; + const double &Y = objectPoints.at(i).at(j).y; + const double &u = imagePoints.at(i).at(j).x; + const double &v = imagePoints.at(i).at(j).y; + + double A = r21 * X + r22 * Y + t2; + double B = v * (r31 * X + r32 * Y); + double C = r11 * X + r12 * Y + t1; + double D = u * (r31 * X + r32 * Y); + double rou = std::sqrt(u * u + v * v); + + for (int k = 1; k <= SCARAMUZZA_POLY_SIZE - 1; ++k) { + double pow_rou = 0.0; + if (k == 1) { + pow_rou = 1.0; + } else { + pow_rou = std::pow(rou, k); + } + + A_mat(line_index + 0, k - 1) = A * pow_rou; + A_mat(line_index + 1, k - 1) = C * pow_rou; + } + + A_mat(line_index + 0, SCARAMUZZA_POLY_SIZE - 1 + i) = -v; + A_mat(line_index + 1, SCARAMUZZA_POLY_SIZE - 1 + i) = -u; + B_vec(line_index + 0) = B; + B_vec(line_index + 1) = D; + + line_index += 2; + } + } + + assert(line_index == static_cast(A_mat.rows())); + + Eigen::Matrix poly_coeff; + // pseudo-inverse for polynomial parameters and all t3s + { + Eigen::JacobiSVD svd( + A_mat, Eigen::ComputeThinU | Eigen::ComputeThinV); + + Eigen::VectorXd x = svd.solve(B_vec); + + poly_coeff[0] = x(0); + poly_coeff[1] = 0.0; + for (int i = 1; i < poly_coeff.size() - 1; ++i) { + poly_coeff[i + 1] = x(i); + } + assert( + x.size() == + static_cast(SCARAMUZZA_POLY_SIZE - 1 + TList.size())); + } + + Parameters params = getParameters(); + + // Affine matrix A is constructed as [C D; E 1] + params.C() = 1.0; + params.D() = 0.0; + params.E() = 0.0; + + params.center_x() = params.imageWidth() / 2.0; + params.center_y() = params.imageHeight() / 2.0; + + for (size_t i = 0; i < SCARAMUZZA_POLY_SIZE; ++i) { + params.poly(i) = poly_coeff[i]; + } + + // params.poly(0) = -216.9657476318; + // params.poly(1) = 0.0; + // params.poly(2) = 0.0017866911; + // params.poly(3) = -0.0000019866; + // params.poly(4) = 0.0000000077; + + // inv_poly + { + std::vector rou_vec; + std::vector z_vec; + for (double rou = 0.0; + rou <= (params.imageWidth() + params.imageHeight()) / 2; rou += 0.1) { + double rou_pow_k = 1.0; + double z = 0.0; + + for (int k = 0; k < SCARAMUZZA_POLY_SIZE; k++) { + z += rou_pow_k * params.poly(k); + rou_pow_k *= rou; + } + + rou_vec.push_back(rou); + z_vec.push_back(z); + } + + assert(rou_vec.size() == z_vec.size()); + Eigen::VectorXd xVec(rou_vec.size()); + Eigen::VectorXd yVec(rou_vec.size()); + + for (size_t i = 0; i < rou_vec.size(); ++i) { + xVec(i) = std::atan2(-z_vec.at(i), rou_vec.at(i)); + yVec(i) = rou_vec.at(i); + } + + // use lower order poly to eliminate over-fitting cause by noisy/inaccurate + // data + const int poly_fit_order = 4; + Eigen::VectorXd inv_poly_coeff = polyfit(xVec, yVec, poly_fit_order); + + for (int i = 0; i <= poly_fit_order; ++i) { + params.inv_poly(i) = inv_poly_coeff(i); + } + } + + setParameters(params); + + std::cout << "initial params:\n" << params << std::endl; +} + +/** + * \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 OCAMCamera::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 OCAMCamera::liftProjective( + const Eigen::Vector2d &p, Eigen::Vector3d &P) const { + // Relative to Center + Eigen::Vector2d xc( + p[0] - mParameters.center_x(), p[1] - mParameters.center_y()); + + // Affine Transformation + // xc_a = inv(A) * xc; + Eigen::Vector2d xc_a( + m_inv_scale * (xc[0] - mParameters.D() * xc[1]), + m_inv_scale * (-mParameters.E() * xc[0] + mParameters.C() * xc[1])); + + double phi = std::sqrt(xc_a[0] * xc_a[0] + xc_a[1] * xc_a[1]); + double phi_i = 1.0; + double z = 0.0; + + for (int i = 0; i < SCARAMUZZA_POLY_SIZE; i++) { + z += phi_i * mParameters.poly(i); + phi_i *= phi; + } + + P << xc[0], xc[1], -z; +} + +/** + * \brief Project a 3D point (\a x,\a y,\a z) to the image plane in (\a u,\a v) + * + * \param P 3D point coordinates + * \param p return value, contains the image point coordinates + */ +void OCAMCamera::spaceToPlane( + const Eigen::Vector3d &P, Eigen::Vector2d &p) const { + double norm = std::sqrt(P[0] * P[0] + P[1] * P[1]); + double theta = std::atan2(-P[2], norm); + double rho = 0.0; + double theta_i = 1.0; + + for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++) { + rho += theta_i * mParameters.inv_poly(i); + theta_i *= theta; + } + + double invNorm = 1.0 / norm; + Eigen::Vector2d xn(P[0] * invNorm * rho, P[1] * invNorm * rho); + + p << xn[0] * mParameters.C() + xn[1] * mParameters.D() + + mParameters.center_x(), + xn[0] * mParameters.E() + xn[1] + mParameters.center_y(); +} + +/** + * \brief Projects an undistorted 2D point p_u to the image plane + * + * \param p_u 2D point coordinates + * \return image point coordinates + */ +void OCAMCamera::undistToPlane( + const Eigen::Vector2d &p_u, Eigen::Vector2d &p) const { + Eigen::Vector3d P(p_u[0], p_u[1], 1.0); + spaceToPlane(P, p); +} + +#if 0 +void +OCAMCamera::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); +} +#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 ¶meters) { + mParameters = parameters; + + m_inv_scale = 1.0 / (parameters.C() - parameters.D() * parameters.E()); +} + +void OCAMCamera::readParameters(const std::vector ¶meterVec) { + 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 ¶meterVec) 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/src/mynteye/api/camodocal/src/chessboard/Chessboard.cc b/src/mynteye/api/camodocal/src/chessboard/Chessboard.cc new file mode 100644 index 0000000..37a7418 --- /dev/null +++ b/src/mynteye/api/camodocal/src/chessboard/Chessboard.cc @@ -0,0 +1,1814 @@ +#include "camodocal/chessboard/Chessboard.h" + +#include +#include + +#include "camodocal/chessboard/ChessboardQuad.h" +#include "camodocal/chessboard/Spline.h" + +#define MAX_CONTOUR_APPROX 7 + +namespace camodocal { + +Chessboard::Chessboard(cv::Size boardSize, cv::Mat &image) + : mBoardSize(boardSize), mCornersFound(false) { + if (image.channels() == 1) { + cv::cvtColor(image, mSketch, CV_GRAY2BGR); + image.copyTo(mImage); + } else { + image.copyTo(mSketch); + cv::cvtColor(image, mImage, CV_BGR2GRAY); + } +} + +void Chessboard::findCorners(bool useOpenCV) { + mCornersFound = findChessboardCorners( + mImage, mBoardSize, mCorners, + CV_CALIB_CB_ADAPTIVE_THRESH + CV_CALIB_CB_NORMALIZE_IMAGE + + CV_CALIB_CB_FILTER_QUADS + CV_CALIB_CB_FAST_CHECK, + useOpenCV); + + if (mCornersFound) { + // draw chessboard corners + cv::drawChessboardCorners(mSketch, mBoardSize, mCorners, mCornersFound); + } +} + +const std::vector &Chessboard::getCorners(void) const { + return mCorners; +} + +bool Chessboard::cornersFound(void) const { + return mCornersFound; +} + +const cv::Mat &Chessboard::getImage(void) const { + return mImage; +} + +const cv::Mat &Chessboard::getSketch(void) const { + return mSketch; +} + +bool Chessboard::findChessboardCorners( + const cv::Mat &image, const cv::Size &patternSize, + std::vector &corners, int flags, bool useOpenCV) { + if (useOpenCV) { + return cv::findChessboardCorners(image, patternSize, corners, flags); + } else { + return findChessboardCornersImproved(image, patternSize, corners, flags); + } +} + +bool Chessboard::findChessboardCornersImproved( + const cv::Mat &image, const cv::Size &patternSize, + std::vector &corners, int flags) { + /************************************************************************************\ + This is improved variant of chessboard corner detection algorithm that + uses a graph of connected quads. It is based on the code contributed + by Vladimir Vezhnevets and Philip Gruebele. + Here is the copyright notice from the original Vladimir's code: + =============================================================== + + The algorithms developed and implemented by Vezhnevets Vldimir + aka Dead Moroz (vvp@graphics.cs.msu.ru) + See http://graphics.cs.msu.su/en/research/calibration/opencv.html + for detailed information. + + Reliability additions and modifications made by Philip Gruebele. + pgruebele@cox.net + + Some improvements were made by: + 1) Martin Rufli - increased chance of correct corner matching + Rufli, M., Scaramuzza, D., and Siegwart, R. (2008), + Automatic Detection of Checkerboards on Blurred and Distorted Images, + Proceedings of the IEEE/RSJ International Conference on + Intelligent Robots and Systems (IROS 2008), Nice, France, September + 2008. + 2) Lionel Heng - post-detection checks and better thresholding + + \************************************************************************************/ + + // int bestDilation = -1; + const int minDilations = 0; + const int maxDilations = 7; + + std::vector outputQuadGroup; + + if (image.depth() != CV_8U || image.channels() == 2) { + return false; + } + + if (patternSize.width < 2 || patternSize.height < 2) { + return false; + } + + if (patternSize.width > 127 || patternSize.height > 127) { + return false; + } + + cv::Mat img = image; + + // Image histogram normalization and + // BGR to Grayscale image conversion (if applicable) + // MARTIN: Set to "false" + if (image.channels() != 1 || (flags & CV_CALIB_CB_NORMALIZE_IMAGE)) { + cv::Mat norm_img(image.rows, image.cols, CV_8UC1); + + if (image.channels() != 1) { + cv::cvtColor(image, norm_img, CV_BGR2GRAY); + img = norm_img; + } + + if (flags & CV_CALIB_CB_NORMALIZE_IMAGE) { + cv::equalizeHist(image, norm_img); + img = norm_img; + } + } + + if (flags & CV_CALIB_CB_FAST_CHECK) { + if (!checkChessboard(img, patternSize)) { + return false; + } + } + + // PART 1: FIND LARGEST PATTERN + //----------------------------------------------------------------------- + // Checker patterns are tried to be found by dilating the background and + // then applying a canny edge finder on the closed contours (checkers). + // Try one dilation run, but if the pattern is not found, repeat until + // max_dilations is reached. + + int prevSqrSize = 0; + bool found = false; + std::vector outputCorners; + + for (int k = 0; k < 6; ++k) { + for (int dilations = minDilations; dilations <= maxDilations; ++dilations) { + if (found) { + break; + } + + cv::Mat thresh_img; + + // convert the input grayscale image to binary (black-n-white) + if (flags & CV_CALIB_CB_ADAPTIVE_THRESH) { + int blockSize = + lround( + prevSqrSize == 0 + ? std::min(img.cols, img.rows) * (k % 2 == 0 ? 0.2 : 0.1) + : prevSqrSize * 2) | + 1; + + // convert to binary + cv::adaptiveThreshold( + img, thresh_img, 255, CV_ADAPTIVE_THRESH_MEAN_C, CV_THRESH_BINARY, + blockSize, (k / 2) * 5); + } else { + // empiric threshold level + double mean = (cv::mean(img))[0]; + int thresh_level = lround(mean - 10); + thresh_level = std::max(thresh_level, 10); + + cv::threshold(img, thresh_img, thresh_level, 255, CV_THRESH_BINARY); + } + + // MARTIN's Code + // Use both a rectangular and a cross kernel. In this way, a more + // homogeneous dilation is performed, which is crucial for small, + // distorted checkers. Use the CROSS kernel first, since its action + // on the image is more subtle + cv::Mat kernel1 = cv::getStructuringElement( + CV_SHAPE_CROSS, cv::Size(3, 3), cv::Point(1, 1)); + cv::Mat kernel2 = cv::getStructuringElement( + CV_SHAPE_RECT, cv::Size(3, 3), cv::Point(1, 1)); + + if (dilations >= 1) + cv::dilate(thresh_img, thresh_img, kernel1); + if (dilations >= 2) + cv::dilate(thresh_img, thresh_img, kernel2); + if (dilations >= 3) + cv::dilate(thresh_img, thresh_img, kernel1); + if (dilations >= 4) + cv::dilate(thresh_img, thresh_img, kernel2); + if (dilations >= 5) + cv::dilate(thresh_img, thresh_img, kernel1); + if (dilations >= 6) + cv::dilate(thresh_img, thresh_img, kernel2); + + // In order to find rectangles that go to the edge, we draw a white + // line around the image edge. Otherwise FindContours will miss those + // clipped rectangle contours. The border color will be the image mean, + // because otherwise we risk screwing up filters like cvSmooth() + cv::rectangle( + thresh_img, cv::Point(0, 0), + cv::Point(thresh_img.cols - 1, thresh_img.rows - 1), + CV_RGB(255, 255, 255), 3, 8); + + // Generate quadrangles in the following function + std::vector quads; + + generateQuads(quads, thresh_img, flags, dilations, true); + if (quads.empty()) { + continue; + } + + // The following function finds and assigns neighbor quads to every + // quadrangle in the immediate vicinity fulfilling certain + // prerequisites + findQuadNeighbors(quads, dilations); + + // The connected quads will be organized in groups. The following loop + // increases a "group_idx" identifier. + // The function "findConnectedQuads assigns all connected quads + // a unique group ID. + // If more quadrangles were assigned to a given group (i.e. connected) + // than are expected by the input variable "patternSize", the + // function "cleanFoundConnectedQuads" erases the surplus + // quadrangles by minimizing the convex hull of the remaining pattern. + + for (int group_idx = 0;; ++group_idx) { + std::vector quadGroup; + + findConnectedQuads(quads, quadGroup, group_idx, dilations); + + if (quadGroup.empty()) { + break; + } + + cleanFoundConnectedQuads(quadGroup, patternSize); + + // The following function labels all corners of every quad + // with a row and column entry. + // "count" specifies the number of found quads in "quad_group" + // with group identifier "group_idx" + // The last parameter is set to "true", because this is the + // first function call and some initializations need to be + // made. + labelQuadGroup(quadGroup, patternSize, true); + + found = checkQuadGroup(quadGroup, outputCorners, patternSize); + + float sumDist = 0; + int total = 0; + + for (int i = 0; i < (int)outputCorners.size(); ++i) { + int ni = 0; + float avgi = outputCorners.at(i)->meanDist(ni); + sumDist += avgi * ni; + total += ni; + } + prevSqrSize = lround(sumDist / std::max(total, 1)); + + if (found && !checkBoardMonotony(outputCorners, patternSize)) { + found = false; + } + } + } + } + + if (!found) { + return false; + } else { + corners.clear(); + corners.reserve(outputCorners.size()); + for (size_t i = 0; i < outputCorners.size(); ++i) { + corners.push_back(outputCorners.at(i)->pt); + } + + cv::cornerSubPix( + image, corners, cv::Size(11, 11), cv::Size(-1, -1), + cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1)); + + return true; + } +} + +//=========================================================================== +// ERASE OVERHEAD +//=========================================================================== +// If we found too many connected quads, remove those which probably do not +// belong. +void Chessboard::cleanFoundConnectedQuads( + std::vector &quadGroup, cv::Size patternSize) { + cv::Point2f center(0.0f, 0.0f); + + // Number of quads this pattern should contain + int count = ((patternSize.width + 1) * (patternSize.height + 1) + 1) / 2; + + // If we have more quadrangles than we should, try to eliminate duplicates + // or ones which don't belong to the pattern rectangle. Else go to the end + // of the function + if ((int)quadGroup.size() <= count) { + return; + } + + // Create an array of quadrangle centers + std::vector centers; + centers.resize(quadGroup.size()); + + for (size_t i = 0; i < quadGroup.size(); ++i) { + cv::Point2f ci(0.0f, 0.0f); + ChessboardQuadPtr &q = quadGroup[i]; + + for (int j = 0; j < 4; ++j) { + ci += q->corners[j]->pt; + } + + ci *= 0.25f; + + // Centers(i), is the geometric center of quad(i) + // Center, is the center of all found quads + centers[i] = ci; + + center += ci; + } + + center *= 1.0f / quadGroup.size(); + + // If we have more quadrangles than we should, we try to eliminate bad + // ones based on minimizing the bounding box. We iteratively remove the + // point which reduces the size of the bounding box of the blobs the most + // (since we want the rectangle to be as small as possible) remove the + // quadrange that causes the biggest reduction in pattern size until we + // have the correct number + while ((int)quadGroup.size() > count) { + double minBoxArea = DBL_MAX; + int minBoxAreaIndex = -1; + + // For each point, calculate box area without that point + for (size_t skip = 0; skip < quadGroup.size(); ++skip) { + // get bounding rectangle + cv::Point2f temp = centers[skip]; + centers[skip] = center; + + std::vector hull; + cv::convexHull(centers, hull, true, true); + centers[skip] = temp; + + double hull_area = fabs(cv::contourArea(hull)); + + // remember smallest box area + if (hull_area < minBoxArea) { + minBoxArea = hull_area; + minBoxAreaIndex = skip; + } + } + + ChessboardQuadPtr &q0 = quadGroup[minBoxAreaIndex]; + + // remove any references to this quad as a neighbor + for (size_t i = 0; i < quadGroup.size(); ++i) { + ChessboardQuadPtr &q = quadGroup.at(i); + for (int j = 0; j < 4; ++j) { + if (q->neighbors[j] == q0) { + q->neighbors[j].reset(); + q->count--; + for (int k = 0; k < 4; ++k) { + if (q0->neighbors[k] == q) { + q0->neighbors[k].reset(); + q0->count--; + break; + } + } + break; + } + } + } + // remove the quad + quadGroup.at(minBoxAreaIndex) = quadGroup.back(); + centers.at(minBoxAreaIndex) = centers.back(); + quadGroup.pop_back(); + centers.pop_back(); + } +} + +//=========================================================================== +// FIND COONECTED QUADS +//=========================================================================== +void Chessboard::findConnectedQuads( + std::vector &quads, + std::vector &group, int group_idx, int dilation) { + ChessboardQuadPtr q; + + // Scan the array for a first unlabeled quad + for (size_t i = 0; i < quads.size(); ++i) { + ChessboardQuadPtr &quad = quads.at(i); + + if (quad->count > 0 && quad->group_idx < 0) { + q = quad; + break; + } + } + + if (q.get() == 0) { + return; + } + + // Recursively find a group of connected quads starting from the seed quad + + std::vector stack; + stack.push_back(q); + + group.push_back(q); + q->group_idx = group_idx; + + while (!stack.empty()) { + q = stack.back(); + stack.pop_back(); + + for (int i = 0; i < 4; ++i) { + ChessboardQuadPtr &neighbor = q->neighbors[i]; + + // If he neighbor exists and the neighbor has more than 0 + // neighbors and the neighbor has not been classified yet. + if (neighbor.get() && neighbor->count > 0 && neighbor->group_idx < 0) { + stack.push_back(neighbor); + group.push_back(neighbor); + neighbor->group_idx = group_idx; + } + } + } +} + +void Chessboard::labelQuadGroup( + std::vector &quadGroup, cv::Size patternSize, + bool firstRun) { + // If this is the first function call, a seed quad needs to be selected + if (firstRun) { + // Search for the (first) quad with the maximum number of neighbors + // (usually 4). This will be our starting point. + int mark = -1; + int maxNeighborCount = 0; + for (size_t i = 0; i < quadGroup.size(); ++i) { + ChessboardQuadPtr &q = quadGroup.at(i); + if (q->count > maxNeighborCount) { + mark = i; + maxNeighborCount = q->count; + + if (maxNeighborCount == 4) { + break; + } + } + } + + // Mark the starting quad's (per definition) upper left corner with + //(0,0) and then proceed clockwise + // The following labeling sequence enshures a "right coordinate system" + ChessboardQuadPtr &q = quadGroup.at(mark); + + q->labeled = true; + + q->corners[0]->row = 0; + q->corners[0]->column = 0; + q->corners[1]->row = 0; + q->corners[1]->column = 1; + q->corners[2]->row = 1; + q->corners[2]->column = 1; + q->corners[3]->row = 1; + q->corners[3]->column = 0; + } + + // Mark all other corners with their respective row and column + bool flagChanged = true; + while (flagChanged) { + // First reset the flag to "false" + flagChanged = false; + + // Go through all quads top down is faster, since unlabeled quads will + // be inserted at the end of the list + for (int i = quadGroup.size() - 1; i >= 0; --i) { + ChessboardQuadPtr &quad = quadGroup.at(i); + + // Check whether quad "i" has been labeled already + if (!quad->labeled) { + // Check its neighbors, whether some of them have been labeled + // already + for (int j = 0; j < 4; j++) { + // Check whether the neighbor exists (i.e. is not the NULL + // pointer) + if (quad->neighbors[j]) { + ChessboardQuadPtr &quadNeighbor = quad->neighbors[j]; + + // Only proceed, if neighbor "j" was labeled + if (quadNeighbor->labeled) { + // For every quad it could happen to pass here + // multiple times. We therefore "break" later. + // Check whitch of the neighbors corners is + // connected to the current quad + int connectedNeighborCornerId = -1; + for (int k = 0; k < 4; ++k) { + if (quadNeighbor->neighbors[k] == quad) { + connectedNeighborCornerId = k; + + // there is only one, therefore + break; + } + } + + // For the following calculations we need the row + // and column of the connected neighbor corner and + // all other corners of the connected quad "j", + // clockwise (CW) + ChessboardCornerPtr &conCorner = + quadNeighbor->corners[connectedNeighborCornerId]; + ChessboardCornerPtr &conCornerCW1 = + quadNeighbor->corners[(connectedNeighborCornerId + 1) % 4]; + ChessboardCornerPtr &conCornerCW2 = + quadNeighbor->corners[(connectedNeighborCornerId + 2) % 4]; + ChessboardCornerPtr &conCornerCW3 = + quadNeighbor->corners[(connectedNeighborCornerId + 3) % 4]; + + quad->corners[j]->row = conCorner->row; + quad->corners[j]->column = conCorner->column; + quad->corners[(j + 1) % 4]->row = + conCorner->row - conCornerCW2->row + conCornerCW3->row; + quad->corners[(j + 1) % 4]->column = conCorner->column - + conCornerCW2->column + + conCornerCW3->column; + quad->corners[(j + 2) % 4]->row = + conCorner->row + conCorner->row - conCornerCW2->row; + quad->corners[(j + 2) % 4]->column = + conCorner->column + conCorner->column - conCornerCW2->column; + quad->corners[(j + 3) % 4]->row = + conCorner->row - conCornerCW2->row + conCornerCW1->row; + quad->corners[(j + 3) % 4]->column = conCorner->column - + conCornerCW2->column + + conCornerCW1->column; + + // Mark this quad as labeled + quad->labeled = true; + + // Changes have taken place, set the flag + flagChanged = true; + + // once is enough! + break; + } + } + } + } + } + } + + // All corners are marked with row and column + // Record the minimal and maximal row and column indices + // It is unlikely that more than 8bit checkers are used per dimension, if + // there are + // an error would have been thrown at the beginning of + // "cvFindChessboardCorners2" + int min_row = 127; + int max_row = -127; + int min_column = 127; + int max_column = -127; + + for (int i = 0; i < (int)quadGroup.size(); ++i) { + ChessboardQuadPtr &q = quadGroup.at(i); + + for (int j = 0; j < 4; ++j) { + ChessboardCornerPtr &c = q->corners[j]; + + if (c->row > max_row) { + max_row = c->row; + } + if (c->row < min_row) { + min_row = c->row; + } + if (c->column > max_column) { + max_column = c->column; + } + if (c->column < min_column) { + min_column = c->column; + } + } + } + + // Label all internal corners with "needsNeighbor" = false + // Label all external corners with "needsNeighbor" = true, + // except if in a given dimension the pattern size is reached + for (int i = min_row; i <= max_row; ++i) { + for (int j = min_column; j <= max_column; ++j) { + // A flag that indicates, whether a row/column combination is + // executed multiple times + bool flag = false; + + // Remember corner and quad + int cornerID; + int quadID; + + for (int k = 0; k < (int)quadGroup.size(); ++k) { + ChessboardQuadPtr &q = quadGroup.at(k); + + for (int l = 0; l < 4; ++l) { + if ((q->corners[l]->row == i) && (q->corners[l]->column == j)) { + if (flag) { + // Passed at least twice through here + q->corners[l]->needsNeighbor = false; + quadGroup[quadID]->corners[cornerID]->needsNeighbor = false; + } else { + // Mark with needs a neighbor, but note the + // address + q->corners[l]->needsNeighbor = true; + cornerID = l; + quadID = k; + } + + // set the flag to true + flag = true; + } + } + } + } + } + + // Complete Linking: + // sometimes not all corners were properly linked in "findQuadNeighbors", + // but after labeling each corner with its respective row and column, it is + // possible to match them anyway. + for (int i = min_row; i <= max_row; ++i) { + for (int j = min_column; j <= max_column; ++j) { + // the following "number" indicates the number of corners which + // correspond to the given (i,j) value + // 1 is a border corner or a conrer which still needs a neighbor + // 2 is a fully connected internal corner + // >2 something went wrong during labeling, report a warning + int number = 1; + + // remember corner and quad + int cornerID; + int quadID; + + for (int k = 0; k < (int)quadGroup.size(); ++k) { + ChessboardQuadPtr &q = quadGroup.at(k); + + for (int l = 0; l < 4; ++l) { + if ((q->corners[l]->row == i) && (q->corners[l]->column == j)) { + if (number == 1) { + // First corner, note its ID + cornerID = l; + quadID = k; + } + + else if (number == 2) { + // Second corner, check wheter this and the + // first one have equal coordinates, else + // interpolate + cv::Point2f delta = + q->corners[l]->pt - quadGroup[quadID]->corners[cornerID]->pt; + + if (delta.x != 0.0f || delta.y != 0.0f) { + // Interpolate + q->corners[l]->pt -= delta * 0.5f; + + quadGroup[quadID]->corners[cornerID]->pt += delta * 0.5f; + } + } else if (number > 2) { + // Something went wrong during row/column labeling + // Report a Warning + // ->Implemented in the function "mrWriteCorners" + } + + // increase the number by one + ++number; + } + } + } + } + } + + // Bordercorners don't need any neighbors, if the pattern size in the + // respective direction is reached + // The only time we can make shure that the target pattern size is reached in + // a given + // dimension, is when the larger side has reached the target size in the + // maximal + // direction, or if the larger side is larger than the smaller target size and + // the + // smaller side equals the smaller target size + int largerDimPattern = std::max(patternSize.height, patternSize.width); + int smallerDimPattern = std::min(patternSize.height, patternSize.width); + bool flagSmallerDim1 = false; + bool flagSmallerDim2 = false; + + if ((largerDimPattern + 1) == max_column - min_column) { + flagSmallerDim1 = true; + // We found out that in the column direction the target pattern size is + // reached + // Therefore border column corners do not need a neighbor anymore + // Go through all corners + for (int k = 0; k < (int)quadGroup.size(); ++k) { + ChessboardQuadPtr &q = quadGroup.at(k); + + for (int l = 0; l < 4; ++l) { + ChessboardCornerPtr &c = q->corners[l]; + + if (c->column == min_column || c->column == max_column) { + // Needs no neighbor anymore + c->needsNeighbor = false; + } + } + } + } + + if ((largerDimPattern + 1) == max_row - min_row) { + flagSmallerDim2 = true; + // We found out that in the column direction the target pattern size is + // reached + // Therefore border column corners do not need a neighbor anymore + // Go through all corners + for (int k = 0; k < (int)quadGroup.size(); ++k) { + ChessboardQuadPtr &q = quadGroup.at(k); + + for (int l = 0; l < 4; ++l) { + ChessboardCornerPtr &c = q->corners[l]; + + if (c->row == min_row || c->row == max_row) { + // Needs no neighbor anymore + c->needsNeighbor = false; + } + } + } + } + + // Check the two flags: + // - If one is true and the other false, then the pattern target + // size was reached in in one direction -> We can check, whether the + // target + // pattern size is also reached in the other direction + // - If both are set to true, then we deal with a square board -> do + // nothing + // - If both are set to false -> There is a possibility that the larger + // side is + // larger than the smaller target size -> Check and if true, then check + // whether + // the other side has the same size as the smaller target size + if ((flagSmallerDim1 == false && flagSmallerDim2 == true)) { + // Larger target pattern size is in row direction, check wheter smaller + // target + // pattern size is reached in column direction + if ((smallerDimPattern + 1) == max_column - min_column) { + for (int k = 0; k < (int)quadGroup.size(); ++k) { + ChessboardQuadPtr &q = quadGroup.at(k); + + for (int l = 0; l < 4; ++l) { + ChessboardCornerPtr &c = q->corners[l]; + + if (c->column == min_column || c->column == max_column) { + // Needs no neighbor anymore + c->needsNeighbor = false; + } + } + } + } + } + + if ((flagSmallerDim1 == true && flagSmallerDim2 == false)) { + // Larger target pattern size is in column direction, check wheter smaller + // target + // pattern size is reached in row direction + if ((smallerDimPattern + 1) == max_row - min_row) { + for (int k = 0; k < (int)quadGroup.size(); ++k) { + ChessboardQuadPtr &q = quadGroup.at(k); + + for (int l = 0; l < 4; ++l) { + ChessboardCornerPtr &c = q->corners[l]; + + if (c->row == min_row || c->row == max_row) { + // Needs no neighbor anymore + c->needsNeighbor = false; + } + } + } + } + } + + if ((flagSmallerDim1 == false && flagSmallerDim2 == false) && + smallerDimPattern + 1 < max_column - min_column) { + // Larger target pattern size is in column direction, check wheter smaller + // target + // pattern size is reached in row direction + if ((smallerDimPattern + 1) == max_row - min_row) { + for (int k = 0; k < (int)quadGroup.size(); ++k) { + ChessboardQuadPtr &q = quadGroup.at(k); + + for (int l = 0; l < 4; ++l) { + ChessboardCornerPtr &c = q->corners[l]; + + if (c->row == min_row || c->row == max_row) { + // Needs no neighbor anymore + c->needsNeighbor = false; + } + } + } + } + } + + if ((flagSmallerDim1 == false && flagSmallerDim2 == false) && + smallerDimPattern + 1 < max_row - min_row) { + // Larger target pattern size is in row direction, check wheter smaller + // target + // pattern size is reached in column direction + if ((smallerDimPattern + 1) == max_column - min_column) { + for (int k = 0; k < (int)quadGroup.size(); ++k) { + ChessboardQuadPtr &q = quadGroup.at(k); + + for (int l = 0; l < 4; ++l) { + ChessboardCornerPtr &c = q->corners[l]; + + if (c->column == min_column || c->column == max_column) { + // Needs no neighbor anymore + c->needsNeighbor = false; + } + } + } + } + } +} + +//=========================================================================== +// GIVE A GROUP IDX +//=========================================================================== +void Chessboard::findQuadNeighbors( + std::vector &quads, int dilation) { + // Thresh dilation is used to counter the effect of dilation on the + // distance between 2 neighboring corners. Since the distance below is + // computed as its square, we do here the same. Additionally, we take the + // conservative assumption that dilation was performed using the 3x3 CROSS + // kernel, which coresponds to the 4-neighborhood. + const float thresh_dilation = (float)(2 * dilation + 3) * (2 * dilation + 3) * + 2; // the "*2" is for the x and y component + // the "3" is for initial corner mismatch + + // Find quad neighbors + for (size_t idx = 0; idx < quads.size(); ++idx) { + ChessboardQuadPtr &curQuad = quads.at(idx); + + // Go through all quadrangles and label them in groups + // For each corner of this quadrangle + for (int i = 0; i < 4; ++i) { + float minDist = FLT_MAX; + int closestCornerIdx = -1; + ChessboardQuadPtr closestQuad; + + if (curQuad->neighbors[i]) { + continue; + } + + cv::Point2f pt = curQuad->corners[i]->pt; + + // Find the closest corner in all other quadrangles + for (size_t k = 0; k < quads.size(); ++k) { + if (k == idx) { + continue; + } + + ChessboardQuadPtr &quad = quads.at(k); + + for (int j = 0; j < 4; ++j) { + // If it already has a neighbor + if (quad->neighbors[j]) { + continue; + } + + cv::Point2f dp = pt - quad->corners[j]->pt; + float dist = dp.dot(dp); + + // The following "if" checks, whether "dist" is the + // shortest so far and smaller than the smallest + // edge length of the current and target quads + if (dist < minDist && dist <= (curQuad->edge_len + thresh_dilation) && + dist <= (quad->edge_len + thresh_dilation)) { + // Check whether conditions are fulfilled + if (matchCorners(curQuad, i, quad, j)) { + closestCornerIdx = j; + closestQuad = quad; + minDist = dist; + } + } + } + } + + // Have we found a matching corner point? + if (closestCornerIdx >= 0 && minDist < FLT_MAX) { + ChessboardCornerPtr closestCorner = + closestQuad->corners[closestCornerIdx]; + + // Make sure that the closest quad does not have the current + // quad as neighbor already + bool valid = true; + for (int j = 0; j < 4; ++j) { + if (closestQuad->neighbors[j] == curQuad) { + valid = false; + break; + } + } + if (!valid) { + continue; + } + + // We've found one more corner - remember it + closestCorner->pt = (pt + closestCorner->pt) * 0.5f; + + curQuad->count++; + curQuad->neighbors[i] = closestQuad; + curQuad->corners[i] = closestCorner; + + closestQuad->count++; + closestQuad->neighbors[closestCornerIdx] = curQuad; + closestQuad->corners[closestCornerIdx] = closestCorner; + } + } + } +} + +//=========================================================================== +// AUGMENT PATTERN WITH ADDITIONAL QUADS +//=========================================================================== +// The first part of the function is basically a copy of +// "findQuadNeighbors" +// The comparisons between two points and two lines could be computed in their +// own function +int Chessboard::augmentBestRun( + std::vector &candidateQuads, int candidateDilation, + std::vector &existingQuads, int existingDilation) { + // thresh dilation is used to counter the effect of dilation on the + // distance between 2 neighboring corners. Since the distance below is + // computed as its square, we do here the same. Additionally, we take the + // conservative assumption that dilation was performed using the 3x3 CROSS + // kernel, which coresponds to the 4-neighborhood. + const float thresh_dilation = (2 * candidateDilation + 3) * + (2 * existingDilation + 3) * + 2; // the "*2" is for the x and y component + + // Search all old quads which have a neighbor that needs to be linked + for (size_t idx = 0; idx < existingQuads.size(); ++idx) { + ChessboardQuadPtr &curQuad = existingQuads.at(idx); + + // For each corner of this quadrangle + for (int i = 0; i < 4; ++i) { + float minDist = FLT_MAX; + int closestCornerIdx = -1; + ChessboardQuadPtr closestQuad; + + // If curQuad corner[i] is already linked, continue + if (!curQuad->corners[i]->needsNeighbor) { + continue; + } + + cv::Point2f pt = curQuad->corners[i]->pt; + + // Look for a match in all candidateQuads' corners + for (size_t k = 0; k < candidateQuads.size(); ++k) { + ChessboardQuadPtr &candidateQuad = candidateQuads.at(k); + + // Only look at unlabeled new quads + if (candidateQuad->labeled) { + continue; + } + + for (int j = 0; j < 4; ++j) { + // Only proceed if they are less than dist away from each + // other + cv::Point2f dp = pt - candidateQuad->corners[j]->pt; + float dist = dp.dot(dp); + + if ((dist < minDist) && + dist <= (curQuad->edge_len + thresh_dilation) && + dist <= (candidateQuad->edge_len + thresh_dilation)) { + if (matchCorners(curQuad, i, candidateQuad, j)) { + closestCornerIdx = j; + closestQuad = candidateQuad; + minDist = dist; + } + } + } + } + + // Have we found a matching corner point? + if (closestCornerIdx >= 0 && minDist < FLT_MAX) { + ChessboardCornerPtr closestCorner = + closestQuad->corners[closestCornerIdx]; + closestCorner->pt = (pt + closestCorner->pt) * 0.5f; + + // We've found one more corner - remember it + // ATTENTION: write the corner x and y coordinates separately, + // else the crucial row/column entries will be overwritten !!! + curQuad->corners[i]->pt = closestCorner->pt; + curQuad->neighbors[i] = closestQuad; + closestQuad->corners[closestCornerIdx]->pt = closestCorner->pt; + + // Label closest quad as labeled. In this way we exclude it + // being considered again during the next loop iteration + closestQuad->labeled = true; + + // We have a new member of the final pattern, copy it over + ChessboardQuadPtr newQuad(new ChessboardQuad); + newQuad->count = 1; + newQuad->edge_len = closestQuad->edge_len; + newQuad->group_idx = curQuad->group_idx; // the same as the current + // quad + newQuad->labeled = false; // do it right afterwards + + curQuad->neighbors[i] = newQuad; + + // We only know one neighbor for sure + newQuad->neighbors[closestCornerIdx] = curQuad; + + for (int j = 0; j < 4; j++) { + newQuad->corners[j].reset(new ChessboardCorner); + newQuad->corners[j]->pt = closestQuad->corners[j]->pt; + } + + existingQuads.push_back(newQuad); + + // Start the function again + return -1; + } + } + } + + // Finished, don't start the function again + return 1; +} + +//=========================================================================== +// GENERATE QUADRANGLES +//=========================================================================== +void Chessboard::generateQuads( + std::vector &quads, cv::Mat &image, int flags, + int dilation, bool firstRun) { + // Empirical lower bound for the size of allowable quadrangles. + // MARTIN, modified: Added "*0.1" in order to find smaller quads. + int minSize = lround(image.cols * image.rows * .03 * 0.01 * 0.92 * 0.1); + + std::vector > contours; + std::vector hierarchy; + + // Initialize contour retrieving routine + cv::findContours( + image, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE); + + std::vector > quadContours; + + for (size_t i = 0; i < contours.size(); ++i) { + // Reject contours with a too small perimeter and contours which are + // completely surrounded by another contour + // MARTIN: If this function is called during PART 1, then the parameter + // "first run" + // is set to "true". This guarantees, that only "nice" squares are detected. + // During PART 2, we allow the polygonal approximation function below to + // approximate more freely, which can result in recognized "squares" that + // are in + // reality multiple blurred and sticked together squares. + std::vector &contour = contours.at(i); + + if (hierarchy[i][3] == -1 || cv::contourArea(contour) < minSize) { + continue; + } + + int min_approx_level = 1, max_approx_level; + if (firstRun) { + max_approx_level = 3; + } else { + max_approx_level = MAX_CONTOUR_APPROX; + } + + std::vector approxContour; + for (int approx_level = min_approx_level; approx_level <= max_approx_level; + approx_level++) { + cv::approxPolyDP(contour, approxContour, approx_level, true); + + if (approxContour.size() == 4) { + break; + } + } + + // Reject non-quadrangles + if (approxContour.size() == 4 && cv::isContourConvex(approxContour)) { + double p = cv::arcLength(approxContour, true); + double area = cv::contourArea(approxContour); + + cv::Point pt[4]; + for (int i = 0; i < 4; i++) { + pt[i] = approxContour[i]; + } + + cv::Point dp = pt[0] - pt[2]; + double d1 = sqrt(dp.dot(dp)); + + dp = pt[1] - pt[3]; + double d2 = sqrt(dp.dot(dp)); + + // PHILIPG: Only accept those quadrangles which are more + // square than rectangular and which are big enough + dp = pt[0] - pt[1]; + double d3 = sqrt(dp.dot(dp)); + dp = pt[1] - pt[2]; + double d4 = sqrt(dp.dot(dp)); + + if (!(flags & CV_CALIB_CB_FILTER_QUADS) || + (d3 * 4 > d4 && d4 * 4 > d3 && d3 * d4 < area * 1.5 && + area > minSize && d1 >= 0.15 * p && d2 >= 0.15 * p)) { + quadContours.push_back(approxContour); + } + } + } + + // Allocate quad & corner buffers + quads.resize(quadContours.size()); + + // Create array of quads structures + for (size_t idx = 0; idx < quadContours.size(); ++idx) { + ChessboardQuadPtr &q = quads.at(idx); + std::vector &contour = quadContours.at(idx); + + // Reset group ID + q.reset(new ChessboardQuad); + assert(contour.size() == 4); + + for (int i = 0; i < 4; ++i) { + cv::Point2f pt = contour.at(i); + ChessboardCornerPtr corner(new ChessboardCorner); + corner->pt = pt; + q->corners[i] = corner; + } + + for (int i = 0; i < 4; ++i) { + cv::Point2f dp = q->corners[i]->pt - q->corners[(i + 1) & 3]->pt; + float d = dp.dot(dp); + if (q->edge_len > d) { + q->edge_len = d; + } + } + } +} + +bool Chessboard::checkQuadGroup( + std::vector &quads, + std::vector &corners, cv::Size patternSize) { + // Initialize + bool flagRow = false; + bool flagColumn = false; + int height = -1; + int width = -1; + + // Compute the minimum and maximum row / column ID + // (it is unlikely that more than 8bit checkers are used per dimension) + int min_row = 127; + int max_row = -127; + int min_col = 127; + int max_col = -127; + + for (size_t i = 0; i < quads.size(); ++i) { + ChessboardQuadPtr &q = quads.at(i); + + for (int j = 0; j < 4; ++j) { + ChessboardCornerPtr &c = q->corners[j]; + + if (c->row > max_row) { + max_row = c->row; + } + if (c->row < min_row) { + min_row = c->row; + } + if (c->column > max_col) { + max_col = c->column; + } + if (c->column < min_col) { + min_col = c->column; + } + } + } + + // If in a given direction the target pattern size is reached, we know exactly + // how + // the checkerboard is oriented. + // Else we need to prepare enough "dummy" corners for the worst case. + for (size_t i = 0; i < quads.size(); ++i) { + ChessboardQuadPtr &q = quads.at(i); + + for (int j = 0; j < 4; ++j) { + ChessboardCornerPtr &c = q->corners[j]; + + if (c->column == max_col && c->row != min_row && c->row != max_row && + !c->needsNeighbor) { + flagColumn = true; + } + if (c->row == max_row && c->column != min_col && c->column != max_col && + !c->needsNeighbor) { + flagRow = true; + } + } + } + + if (flagColumn) { + if (max_col - min_col == patternSize.width + 1) { + width = patternSize.width; + height = patternSize.height; + } else { + width = patternSize.height; + height = patternSize.width; + } + } else if (flagRow) { + if (max_row - min_row == patternSize.width + 1) { + height = patternSize.width; + width = patternSize.height; + } else { + height = patternSize.height; + width = patternSize.width; + } + } else { + // If target pattern size is not reached in at least one of the two + // directions, then we do not know where the remaining corners are + // located. Account for this. + width = std::max(patternSize.width, patternSize.height); + height = std::max(patternSize.width, patternSize.height); + } + + ++min_row; + ++min_col; + max_row = min_row + height - 1; + max_col = min_col + width - 1; + + corners.clear(); + + int linkedBorderCorners = 0; + + // Write the corners in increasing order to the output file + for (int i = min_row; i <= max_row; ++i) { + for (int j = min_col; j <= max_col; ++j) { + // Reset the iterator + int iter = 1; + + for (int k = 0; k < (int)quads.size(); ++k) { + ChessboardQuadPtr &quad = quads.at(k); + + for (int l = 0; l < 4; ++l) { + ChessboardCornerPtr &c = quad->corners[l]; + + if (c->row == i && c->column == j) { + bool boardEdge = false; + if (i == min_row || i == max_row || j == min_col || j == max_col) { + boardEdge = true; + } + + if ((iter == 1 && boardEdge) || (iter == 2 && !boardEdge)) { + // The respective row and column have been found + corners.push_back(quad->corners[l]); + } + + if (iter == 2 && boardEdge) { + ++linkedBorderCorners; + } + + // If the iterator is larger than two, this means that more than + // two corners have the same row / column entries. Then some + // linking errors must have occured and we should not use the found + // pattern + if (iter > 2) { + return false; + } + + iter++; + } + } + } + } + } + + if ((int)corners.size() != patternSize.width * patternSize.height || + linkedBorderCorners < + (patternSize.width * 2 + patternSize.height * 2 - 2) * 0.75f) { + return false; + } + + // check that no corners lie at image boundary + float border = 5.0f; + for (int i = 0; i < (int)corners.size(); ++i) { + ChessboardCornerPtr &c = corners.at(i); + + if (c->pt.x < border || c->pt.x > mImage.cols - border || + c->pt.y < border || c->pt.y > mImage.rows - border) { + return false; + } + } + + // check if we need to transpose the board + if (width != patternSize.width) { + std::swap(width, height); + + std::vector outputCorners; + outputCorners.resize(corners.size()); + + for (int i = 0; i < height; ++i) { + for (int j = 0; j < width; ++j) { + outputCorners.at(i * width + j) = corners.at(j * height + i); + } + } + + corners = outputCorners; + } + + // check if we need to revert the order in each row + cv::Point2f p0 = corners.at(0)->pt; + cv::Point2f p1 = corners.at(width - 1)->pt; + cv::Point2f p2 = corners.at(width)->pt; + + if ((p1 - p0).cross(p2 - p0) < 0.0f) { + for (int i = 0; i < height; ++i) { + for (int j = 0; j < width / 2; ++j) { + std::swap( + corners.at(i * width + j), corners.at(i * width + width - j - 1)); + } + } + } + + p0 = corners.at(0)->pt; + p2 = corners.at(width)->pt; + + // check if we need to rotate the board + if (p2.y < p0.y) { + std::vector outputCorners; + outputCorners.resize(corners.size()); + + for (int i = 0; i < height; ++i) { + for (int j = 0; j < width; ++j) { + outputCorners.at(i * width + j) = + corners.at((height - i - 1) * width + width - j - 1); + } + } + + corners = outputCorners; + } + + return true; +} + +void Chessboard::getQuadrangleHypotheses( + const std::vector > &contours, + std::vector > &quads, int classId) const { + const float minAspectRatio = 0.2f; + const float maxAspectRatio = 5.0f; + const float minBoxSize = 10.0f; + + for (size_t i = 0; i < contours.size(); ++i) { + cv::RotatedRect box = cv::minAreaRect(contours.at(i)); + float boxSize = std::max(box.size.width, box.size.height); + if (boxSize < minBoxSize) { + continue; + } + + float aspectRatio = box.size.width / std::max(box.size.height, 1.0f); + + if (aspectRatio < minAspectRatio || aspectRatio > maxAspectRatio) { + continue; + } + + quads.push_back(std::pair(boxSize, classId)); + } +} + +bool less_pred( + const std::pair &p1, const std::pair &p2) { + return p1.first < p2.first; +} + +void countClasses( + const std::vector > &pairs, size_t idx1, size_t idx2, + std::vector &counts) { + counts.assign(2, 0); + for (size_t i = idx1; i != idx2; ++i) { + counts[pairs[i].second]++; + } +} + +bool Chessboard::checkChessboard( + const cv::Mat &image, cv::Size patternSize) const { + const int erosionCount = 1; + const float blackLevel = 20.f; + const float whiteLevel = 130.f; + const float blackWhiteGap = 70.f; + + cv::Mat white = image.clone(); + + cv::Mat black = image.clone(); + + cv::erode(white, white, cv::Mat(), cv::Point(-1, -1), erosionCount); + cv::dilate(black, black, cv::Mat(), cv::Point(-1, -1), erosionCount); + + cv::Mat thresh(image.rows, image.cols, CV_8UC1); + + bool result = false; + for (float threshLevel = blackLevel; threshLevel < whiteLevel && !result; + threshLevel += 20.0f) { + cv::threshold( + white, thresh, threshLevel + blackWhiteGap, 255, CV_THRESH_BINARY); + + std::vector > contours; + std::vector hierarchy; + + // Initialize contour retrieving routine + cv::findContours( + thresh, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE); + + std::vector > quads; + getQuadrangleHypotheses(contours, quads, 1); + + cv::threshold(black, thresh, threshLevel, 255, CV_THRESH_BINARY_INV); + + cv::findContours( + thresh, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE); + getQuadrangleHypotheses(contours, quads, 0); + + const size_t min_quads_count = patternSize.width * patternSize.height / 2; + std::sort(quads.begin(), quads.end(), less_pred); + + // now check if there are many hypotheses with similar sizes + // do this by floodfill-style algorithm + const float sizeRelDev = 0.4f; + + for (size_t i = 0; i < quads.size(); ++i) { + size_t j = i + 1; + for (; j < quads.size(); ++j) { + if (quads[j].first / quads[i].first > 1.0f + sizeRelDev) { + break; + } + } + + if (j + 1 > min_quads_count + i) { + // check the number of black and white squares + std::vector counts; + countClasses(quads, i, j, counts); + const int blackCount = lroundf( + ceilf(patternSize.width / 2.0f) * ceilf(patternSize.height / 2.0f)); + const int whiteCount = lroundf( + floorf(patternSize.width / 2.0f) * + floorf(patternSize.height / 2.0f)); + if (counts[0] < blackCount * 0.75f || counts[1] < whiteCount * 0.75f) { + continue; + } + result = true; + break; + } + } + } + + return result; +} + +bool Chessboard::checkBoardMonotony( + std::vector &corners, cv::Size patternSize) { + const float threshFactor = 0.2f; + + Spline splineXY, splineYX; + splineXY.setLowBC(Spline::PARABOLIC_RUNOUT_BC); + splineXY.setHighBC(Spline::PARABOLIC_RUNOUT_BC); + splineYX.setLowBC(Spline::PARABOLIC_RUNOUT_BC); + splineYX.setHighBC(Spline::PARABOLIC_RUNOUT_BC); + + // check if each row of corners approximates a cubic spline + for (int i = 0; i < patternSize.height; ++i) { + splineXY.clear(); + splineYX.clear(); + + cv::Point2f p[3]; + p[0] = corners.at(i * patternSize.width)->pt; + p[1] = corners.at(i * patternSize.width + patternSize.width / 2)->pt; + p[2] = corners.at(i * patternSize.width + patternSize.width - 1)->pt; + + for (int j = 0; j < 3; ++j) { + splineXY.addPoint(p[j].x, p[j].y); + splineYX.addPoint(p[j].y, p[j].x); + } + + for (int j = 1; j < patternSize.width - 1; ++j) { + cv::Point2f &p_j = corners.at(i * patternSize.width + j)->pt; + + float thresh = std::numeric_limits::max(); + + // up-neighbor + if (i > 0) { + cv::Point2f &neighbor = corners.at((i - 1) * patternSize.width + j)->pt; + thresh = fminf(thresh, cv::norm(neighbor - p_j)); + } + // down-neighbor + if (i < patternSize.height - 1) { + cv::Point2f &neighbor = corners.at((i + 1) * patternSize.width + j)->pt; + thresh = fminf(thresh, cv::norm(neighbor - p_j)); + } + // left-neighbor + { + cv::Point2f &neighbor = corners.at(i * patternSize.width + j - 1)->pt; + thresh = fminf(thresh, cv::norm(neighbor - p_j)); + } + // right-neighbor + { + cv::Point2f &neighbor = corners.at(i * patternSize.width + j + 1)->pt; + thresh = fminf(thresh, cv::norm(neighbor - p_j)); + } + + thresh *= threshFactor; + + if (fminf( + fabsf(splineXY(p_j.x) - p_j.y), fabsf(splineYX(p_j.y) - p_j.x)) > + thresh) { + return false; + } + } + } + + // check if each column of corners approximates a cubic spline + for (int j = 0; j < patternSize.width; ++j) { + splineXY.clear(); + splineYX.clear(); + + cv::Point2f p[3]; + p[0] = corners.at(j)->pt; + p[1] = corners.at(patternSize.height / 2 * patternSize.width + j)->pt; + p[2] = corners.at((patternSize.height - 1) * patternSize.width + j)->pt; + + for (int i = 0; i < 3; ++i) { + splineXY.addPoint(p[i].x, p[i].y); + splineYX.addPoint(p[i].y, p[i].x); + } + + for (int i = 1; i < patternSize.height - 1; ++i) { + cv::Point2f &p_i = corners.at(i * patternSize.width + j)->pt; + + float thresh = std::numeric_limits::max(); + + // up-neighbor + { + cv::Point2f &neighbor = corners.at((i - 1) * patternSize.width + j)->pt; + thresh = fminf(thresh, cv::norm(neighbor - p_i)); + } + // down-neighbor + { + cv::Point2f &neighbor = corners.at((i + 1) * patternSize.width + j)->pt; + thresh = fminf(thresh, cv::norm(neighbor - p_i)); + } + // left-neighbor + if (j > 0) { + cv::Point2f &neighbor = corners.at(i * patternSize.width + j - 1)->pt; + thresh = fminf(thresh, cv::norm(neighbor - p_i)); + } + // right-neighbor + if (j < patternSize.width - 1) { + cv::Point2f &neighbor = corners.at(i * patternSize.width + j + 1)->pt; + thresh = fminf(thresh, cv::norm(neighbor - p_i)); + } + + thresh *= threshFactor; + + if (fminf( + fabsf(splineXY(p_i.x) - p_i.y), fabsf(splineYX(p_i.y) - p_i.x)) > + thresh) { + return false; + } + } + } + + return true; +} + +bool Chessboard::matchCorners( + ChessboardQuadPtr &quad1, int corner1, ChessboardQuadPtr &quad2, + int corner2) const { + // First Check everything from the viewpoint of the + // current quad compute midpoints of "parallel" quad + // sides 1 + float x1 = (quad1->corners[corner1]->pt.x + + quad1->corners[(corner1 + 1) % 4]->pt.x) / + 2; + float y1 = (quad1->corners[corner1]->pt.y + + quad1->corners[(corner1 + 1) % 4]->pt.y) / + 2; + float x2 = (quad1->corners[(corner1 + 2) % 4]->pt.x + + quad1->corners[(corner1 + 3) % 4]->pt.x) / + 2; + float y2 = (quad1->corners[(corner1 + 2) % 4]->pt.y + + quad1->corners[(corner1 + 3) % 4]->pt.y) / + 2; + // compute midpoints of "parallel" quad sides 2 + float x3 = (quad1->corners[corner1]->pt.x + + quad1->corners[(corner1 + 3) % 4]->pt.x) / + 2; + float y3 = (quad1->corners[corner1]->pt.y + + quad1->corners[(corner1 + 3) % 4]->pt.y) / + 2; + float x4 = (quad1->corners[(corner1 + 1) % 4]->pt.x + + quad1->corners[(corner1 + 2) % 4]->pt.x) / + 2; + float y4 = (quad1->corners[(corner1 + 1) % 4]->pt.y + + quad1->corners[(corner1 + 2) % 4]->pt.y) / + 2; + + // MARTIN: Heuristic + // For corner2 of quad2 to be considered, + // it needs to be on the same side of the two lines as + // corner1. This is given, if the cross product has + // the same sign for both computations below: + float a1 = x1 - x2; + float b1 = y1 - y2; + // the current corner + float c11 = quad1->corners[corner1]->pt.x - x2; + float d11 = quad1->corners[corner1]->pt.y - y2; + // the candidate corner + float c12 = quad2->corners[corner2]->pt.x - x2; + float d12 = quad2->corners[corner2]->pt.y - y2; + float sign11 = a1 * d11 - c11 * b1; + float sign12 = a1 * d12 - c12 * b1; + + float a2 = x3 - x4; + float b2 = y3 - y4; + // the current corner + float c21 = quad1->corners[corner1]->pt.x - x4; + float d21 = quad1->corners[corner1]->pt.y - y4; + // the candidate corner + float c22 = quad2->corners[corner2]->pt.x - x4; + float d22 = quad2->corners[corner2]->pt.y - y4; + float sign21 = a2 * d21 - c21 * b2; + float sign22 = a2 * d22 - c22 * b2; + + // Also make shure that two border quads of the same row or + // column don't link. Check from the current corner's view, + // whether the corner diagonal from the candidate corner + // is also on the same side of the two lines as the current + // corner and the candidate corner. + float c13 = quad2->corners[(corner2 + 2) % 4]->pt.x - x2; + float d13 = quad2->corners[(corner2 + 2) % 4]->pt.y - y2; + float c23 = quad2->corners[(corner2 + 2) % 4]->pt.x - x4; + float d23 = quad2->corners[(corner2 + 2) % 4]->pt.y - y4; + float sign13 = a1 * d13 - c13 * b1; + float sign23 = a2 * d23 - c23 * b2; + + // Second: Then check everything from the viewpoint of + // the candidate quad. Compute midpoints of "parallel" + // quad sides 1 + float u1 = (quad2->corners[corner2]->pt.x + + quad2->corners[(corner2 + 1) % 4]->pt.x) / + 2; + float v1 = (quad2->corners[corner2]->pt.y + + quad2->corners[(corner2 + 1) % 4]->pt.y) / + 2; + float u2 = (quad2->corners[(corner2 + 2) % 4]->pt.x + + quad2->corners[(corner2 + 3) % 4]->pt.x) / + 2; + float v2 = (quad2->corners[(corner2 + 2) % 4]->pt.y + + quad2->corners[(corner2 + 3) % 4]->pt.y) / + 2; + // compute midpoints of "parallel" quad sides 2 + float u3 = (quad2->corners[corner2]->pt.x + + quad2->corners[(corner2 + 3) % 4]->pt.x) / + 2; + float v3 = (quad2->corners[corner2]->pt.y + + quad2->corners[(corner2 + 3) % 4]->pt.y) / + 2; + float u4 = (quad2->corners[(corner2 + 1) % 4]->pt.x + + quad2->corners[(corner2 + 2) % 4]->pt.x) / + 2; + float v4 = (quad2->corners[(corner2 + 1) % 4]->pt.y + + quad2->corners[(corner2 + 2) % 4]->pt.y) / + 2; + + // MARTIN: Heuristic + // For corner2 of quad2 to be considered, + // it needs to be on the same side of the two lines as + // corner1. This is given, if the cross product has + // the same sign for both computations below: + float a3 = u1 - u2; + float b3 = v1 - v2; + // the current corner + float c31 = quad1->corners[corner1]->pt.x - u2; + float d31 = quad1->corners[corner1]->pt.y - v2; + // the candidate corner + float c32 = quad2->corners[corner2]->pt.x - u2; + float d32 = quad2->corners[corner2]->pt.y - v2; + float sign31 = a3 * d31 - c31 * b3; + float sign32 = a3 * d32 - c32 * b3; + + float a4 = u3 - u4; + float b4 = v3 - v4; + // the current corner + float c41 = quad1->corners[corner1]->pt.x - u4; + float d41 = quad1->corners[corner1]->pt.y - v4; + // the candidate corner + float c42 = quad2->corners[corner2]->pt.x - u4; + float d42 = quad2->corners[corner2]->pt.y - v4; + float sign41 = a4 * d41 - c41 * b4; + float sign42 = a4 * d42 - c42 * b4; + + // Also make sure that two border quads of the same row or + // column don't link. Check from the candidate corner's view, + // whether the corner diagonal from the current corner + // is also on the same side of the two lines as the current + // corner and the candidate corner. + float c33 = quad1->corners[(corner1 + 2) % 4]->pt.x - u2; + float d33 = quad1->corners[(corner1 + 2) % 4]->pt.y - v2; + float c43 = quad1->corners[(corner1 + 2) % 4]->pt.x - u4; + float d43 = quad1->corners[(corner1 + 2) % 4]->pt.y - v4; + float sign33 = a3 * d33 - c33 * b3; + float sign43 = a4 * d43 - c43 * b4; + + // This time we also need to make shure, that no quad + // is linked to a quad of another dilation run which + // may lie INSIDE it!!! + // Third: Therefore check everything from the viewpoint + // of the current quad compute midpoints of "parallel" + // quad sides 1 + float x5 = quad1->corners[corner1]->pt.x; + float y5 = quad1->corners[corner1]->pt.y; + float x6 = quad1->corners[(corner1 + 1) % 4]->pt.x; + float y6 = quad1->corners[(corner1 + 1) % 4]->pt.y; + // compute midpoints of "parallel" quad sides 2 + float x7 = x5; + float y7 = y5; + float x8 = quad1->corners[(corner1 + 3) % 4]->pt.x; + float y8 = quad1->corners[(corner1 + 3) % 4]->pt.y; + + // MARTIN: Heuristic + // For corner2 of quad2 to be considered, + // it needs to be on the other side of the two lines than + // corner1. This is given, if the cross product has + // a different sign for both computations below: + float a5 = x6 - x5; + float b5 = y6 - y5; + // the current corner + float c51 = quad1->corners[(corner1 + 2) % 4]->pt.x - x5; + float d51 = quad1->corners[(corner1 + 2) % 4]->pt.y - y5; + // the candidate corner + float c52 = quad2->corners[corner2]->pt.x - x5; + float d52 = quad2->corners[corner2]->pt.y - y5; + float sign51 = a5 * d51 - c51 * b5; + float sign52 = a5 * d52 - c52 * b5; + + float a6 = x8 - x7; + float b6 = y8 - y7; + // the current corner + float c61 = quad1->corners[(corner1 + 2) % 4]->pt.x - x7; + float d61 = quad1->corners[(corner1 + 2) % 4]->pt.y - y7; + // the candidate corner + float c62 = quad2->corners[corner2]->pt.x - x7; + float d62 = quad2->corners[corner2]->pt.y - y7; + float sign61 = a6 * d61 - c61 * b6; + float sign62 = a6 * d62 - c62 * b6; + + // Fourth: Then check everything from the viewpoint of + // the candidate quad compute midpoints of "parallel" + // quad sides 1 + float u5 = quad2->corners[corner2]->pt.x; + float v5 = quad2->corners[corner2]->pt.y; + float u6 = quad2->corners[(corner2 + 1) % 4]->pt.x; + float v6 = quad2->corners[(corner2 + 1) % 4]->pt.y; + // compute midpoints of "parallel" quad sides 2 + float u7 = u5; + float v7 = v5; + float u8 = quad2->corners[(corner2 + 3) % 4]->pt.x; + float v8 = quad2->corners[(corner2 + 3) % 4]->pt.y; + + // MARTIN: Heuristic + // For corner2 of quad2 to be considered, + // it needs to be on the other side of the two lines than + // corner1. This is given, if the cross product has + // a different sign for both computations below: + float a7 = u6 - u5; + float b7 = v6 - v5; + // the current corner + float c71 = quad1->corners[corner1]->pt.x - u5; + float d71 = quad1->corners[corner1]->pt.y - v5; + // the candidate corner + float c72 = quad2->corners[(corner2 + 2) % 4]->pt.x - u5; + float d72 = quad2->corners[(corner2 + 2) % 4]->pt.y - v5; + float sign71 = a7 * d71 - c71 * b7; + float sign72 = a7 * d72 - c72 * b7; + + float a8 = u8 - u7; + float b8 = v8 - v7; + // the current corner + float c81 = quad1->corners[corner1]->pt.x - u7; + float d81 = quad1->corners[corner1]->pt.y - v7; + // the candidate corner + float c82 = quad2->corners[(corner2 + 2) % 4]->pt.x - u7; + float d82 = quad2->corners[(corner2 + 2) % 4]->pt.y - v7; + float sign81 = a8 * d81 - c81 * b8; + float sign82 = a8 * d82 - c82 * b8; + + // Check whether conditions are fulfilled + if (((sign11 < 0 && sign12 < 0) || (sign11 > 0 && sign12 > 0)) && + ((sign21 < 0 && sign22 < 0) || (sign21 > 0 && sign22 > 0)) && + ((sign31 < 0 && sign32 < 0) || (sign31 > 0 && sign32 > 0)) && + ((sign41 < 0 && sign42 < 0) || (sign41 > 0 && sign42 > 0)) && + ((sign11 < 0 && sign13 < 0) || (sign11 > 0 && sign13 > 0)) && + ((sign21 < 0 && sign23 < 0) || (sign21 > 0 && sign23 > 0)) && + ((sign31 < 0 && sign33 < 0) || (sign31 > 0 && sign33 > 0)) && + ((sign41 < 0 && sign43 < 0) || (sign41 > 0 && sign43 > 0)) && + ((sign51 < 0 && sign52 > 0) || (sign51 > 0 && sign52 < 0)) && + ((sign61 < 0 && sign62 > 0) || (sign61 > 0 && sign62 < 0)) && + ((sign71 < 0 && sign72 > 0) || (sign71 > 0 && sign72 < 0)) && + ((sign81 < 0 && sign82 > 0) || (sign81 > 0 && sign82 < 0))) { + return true; + } else { + return false; + } +} +} diff --git a/src/mynteye/api/camodocal/src/gpl/EigenQuaternionParameterization.cc b/src/mynteye/api/camodocal/src/gpl/EigenQuaternionParameterization.cc new file mode 100644 index 0000000..6693dd4 --- /dev/null +++ b/src/mynteye/api/camodocal/src/gpl/EigenQuaternionParameterization.cc @@ -0,0 +1,43 @@ +#include "camodocal/gpl/EigenQuaternionParameterization.h" + +#include + +namespace camodocal { + +bool EigenQuaternionParameterization::Plus( + const double *x, const double *delta, double *x_plus_delta) const { + const double norm_delta = + sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]); + if (norm_delta > 0.0) { + const double sin_delta_by_delta = (sin(norm_delta) / norm_delta); + double q_delta[4]; + q_delta[0] = sin_delta_by_delta * delta[0]; + q_delta[1] = sin_delta_by_delta * delta[1]; + q_delta[2] = sin_delta_by_delta * delta[2]; + q_delta[3] = cos(norm_delta); + EigenQuaternionProduct(q_delta, x, x_plus_delta); + } else { + for (int i = 0; i < 4; ++i) { + x_plus_delta[i] = x[i]; + } + } + return true; +} + +bool EigenQuaternionParameterization::ComputeJacobian( + const double *x, double *jacobian) const { + jacobian[0] = x[3]; + jacobian[1] = x[2]; + jacobian[2] = -x[1]; // NOLINT + jacobian[3] = -x[2]; + jacobian[4] = x[3]; + jacobian[5] = x[0]; // NOLINT + jacobian[6] = x[1]; + jacobian[7] = -x[0]; + jacobian[8] = x[3]; // NOLINT + jacobian[9] = -x[0]; + jacobian[10] = -x[1]; + jacobian[11] = -x[2]; // NOLINT + return true; +} +} diff --git a/src/mynteye/api/camodocal/src/gpl/gpl.cc b/src/mynteye/api/camodocal/src/gpl/gpl.cc new file mode 100644 index 0000000..4100e97 --- /dev/null +++ b/src/mynteye/api/camodocal/src/gpl/gpl.cc @@ -0,0 +1,754 @@ +#include "camodocal/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; +} + +#ifdef _WIN32 +#include +#include +#include +LARGE_INTEGER +getFILETIMEoffset() { + SYSTEMTIME s; + FILETIME f; + LARGE_INTEGER t; + + s.wYear = 1970; + s.wMonth = 1; + s.wDay = 1; + s.wHour = 0; + s.wMinute = 0; + s.wSecond = 0; + s.wMilliseconds = 0; + SystemTimeToFileTime(&s, &f); + t.QuadPart = f.dwHighDateTime; + t.QuadPart <<= 32; + t.QuadPart |= f.dwLowDateTime; + return (t); +} + +int clock_gettime(int X, struct timespec *tp) { + LARGE_INTEGER t; + FILETIME f; + double microseconds; + static LARGE_INTEGER offset; + static double frequencyToMicroseconds; + static int initialized = 0; + static BOOL usePerformanceCounter = 0; + + if (!initialized) { + LARGE_INTEGER performanceFrequency; + initialized = 1; + usePerformanceCounter = QueryPerformanceFrequency(&performanceFrequency); + if (usePerformanceCounter) { + QueryPerformanceCounter(&offset); + frequencyToMicroseconds = + (double)performanceFrequency.QuadPart / 1000000.; + } else { + offset = getFILETIMEoffset(); + frequencyToMicroseconds = 10.; + } + } + if (usePerformanceCounter) + QueryPerformanceCounter(&t); + else { + GetSystemTimeAsFileTime(&f); + t.QuadPart = f.dwHighDateTime; + t.QuadPart <<= 32; + t.QuadPart |= f.dwLowDateTime; + } + + t.QuadPart -= offset.QuadPart; + microseconds = (double)t.QuadPart / frequencyToMicroseconds; + t.QuadPart = microseconds; + tp->tv_sec = t.QuadPart / 1000000; + tp->tv_nsec = (t.QuadPart % 1000000) * 1000; + return (0); +} +#endif + +unsigned long long timeInMicroseconds(void) { + struct timespec tp; +#ifdef __APPLE__ + tp = orwl_gettime(); +#else + clock_gettime(CLOCK_REALTIME, &tp); +#endif + + return tp.tv_sec * 1000000 + tp.tv_nsec / 1000; +} + +double timeInSeconds(void) { + struct timespec tp; +#ifdef __APPLE__ + tp = orwl_gettime(); +#else + clock_gettime(CLOCK_REALTIME, &tp); +#endif + + return static_cast(tp.tv_sec) + + static_cast(tp.tv_nsec) / 1000000000.0; +} + +float colormapAutumn[128][3] = { + {1.0f, 0.f, 0.f}, {1.0f, 0.007874f, 0.f}, {1.0f, 0.015748f, 0.f}, + {1.0f, 0.023622f, 0.f}, {1.0f, 0.031496f, 0.f}, {1.0f, 0.03937f, 0.f}, + {1.0f, 0.047244f, 0.f}, {1.0f, 0.055118f, 0.f}, {1.0f, 0.062992f, 0.f}, + {1.0f, 0.070866f, 0.f}, {1.0f, 0.07874f, 0.f}, {1.0f, 0.086614f, 0.f}, + {1.0f, 0.094488f, 0.f}, {1.0f, 0.10236f, 0.f}, {1.0f, 0.11024f, 0.f}, + {1.0f, 0.11811f, 0.f}, {1.0f, 0.12598f, 0.f}, {1.0f, 0.13386f, 0.f}, + {1.0f, 0.14173f, 0.f}, {1.0f, 0.14961f, 0.f}, {1.0f, 0.15748f, 0.f}, + {1.0f, 0.16535f, 0.f}, {1.0f, 0.17323f, 0.f}, {1.0f, 0.1811f, 0.f}, + {1.0f, 0.18898f, 0.f}, {1.0f, 0.19685f, 0.f}, {1.0f, 0.20472f, 0.f}, + {1.0f, 0.2126f, 0.f}, {1.0f, 0.22047f, 0.f}, {1.0f, 0.22835f, 0.f}, + {1.0f, 0.23622f, 0.f}, {1.0f, 0.24409f, 0.f}, {1.0f, 0.25197f, 0.f}, + {1.0f, 0.25984f, 0.f}, {1.0f, 0.26772f, 0.f}, {1.0f, 0.27559f, 0.f}, + {1.0f, 0.28346f, 0.f}, {1.0f, 0.29134f, 0.f}, {1.0f, 0.29921f, 0.f}, + {1.0f, 0.30709f, 0.f}, {1.0f, 0.31496f, 0.f}, {1.0f, 0.32283f, 0.f}, + {1.0f, 0.33071f, 0.f}, {1.0f, 0.33858f, 0.f}, {1.0f, 0.34646f, 0.f}, + {1.0f, 0.35433f, 0.f}, {1.0f, 0.3622f, 0.f}, {1.0f, 0.37008f, 0.f}, + {1.0f, 0.37795f, 0.f}, {1.0f, 0.38583f, 0.f}, {1.0f, 0.3937f, 0.f}, + {1.0f, 0.40157f, 0.f}, {1.0f, 0.40945f, 0.f}, {1.0f, 0.41732f, 0.f}, + {1.0f, 0.4252f, 0.f}, {1.0f, 0.43307f, 0.f}, {1.0f, 0.44094f, 0.f}, + {1.0f, 0.44882f, 0.f}, {1.0f, 0.45669f, 0.f}, {1.0f, 0.46457f, 0.f}, + {1.0f, 0.47244f, 0.f}, {1.0f, 0.48031f, 0.f}, {1.0f, 0.48819f, 0.f}, + {1.0f, 0.49606f, 0.f}, {1.0f, 0.50394f, 0.f}, {1.0f, 0.51181f, 0.f}, + {1.0f, 0.51969f, 0.f}, {1.0f, 0.52756f, 0.f}, {1.0f, 0.53543f, 0.f}, + {1.0f, 0.54331f, 0.f}, {1.0f, 0.55118f, 0.f}, {1.0f, 0.55906f, 0.f}, + {1.0f, 0.56693f, 0.f}, {1.0f, 0.5748f, 0.f}, {1.0f, 0.58268f, 0.f}, + {1.0f, 0.59055f, 0.f}, {1.0f, 0.59843f, 0.f}, {1.0f, 0.6063f, 0.f}, + {1.0f, 0.61417f, 0.f}, {1.0f, 0.62205f, 0.f}, {1.0f, 0.62992f, 0.f}, + {1.0f, 0.6378f, 0.f}, {1.0f, 0.64567f, 0.f}, {1.0f, 0.65354f, 0.f}, + {1.0f, 0.66142f, 0.f}, {1.0f, 0.66929f, 0.f}, {1.0f, 0.67717f, 0.f}, + {1.0f, 0.68504f, 0.f}, {1.0f, 0.69291f, 0.f}, {1.0f, 0.70079f, 0.f}, + {1.0f, 0.70866f, 0.f}, {1.0f, 0.71654f, 0.f}, {1.0f, 0.72441f, 0.f}, + {1.0f, 0.73228f, 0.f}, {1.0f, 0.74016f, 0.f}, {1.0f, 0.74803f, 0.f}, + {1.0f, 0.75591f, 0.f}, {1.0f, 0.76378f, 0.f}, {1.0f, 0.77165f, 0.f}, + {1.0f, 0.77953f, 0.f}, {1.0f, 0.7874f, 0.f}, {1.0f, 0.79528f, 0.f}, + {1.0f, 0.80315f, 0.f}, {1.0f, 0.81102f, 0.f}, {1.0f, 0.8189f, 0.f}, + {1.0f, 0.82677f, 0.f}, {1.0f, 0.83465f, 0.f}, {1.0f, 0.84252f, 0.f}, + {1.0f, 0.85039f, 0.f}, {1.0f, 0.85827f, 0.f}, {1.0f, 0.86614f, 0.f}, + {1.0f, 0.87402f, 0.f}, {1.0f, 0.88189f, 0.f}, {1.0f, 0.88976f, 0.f}, + {1.0f, 0.89764f, 0.f}, {1.0f, 0.90551f, 0.f}, {1.0f, 0.91339f, 0.f}, + {1.0f, 0.92126f, 0.f}, {1.0f, 0.92913f, 0.f}, {1.0f, 0.93701f, 0.f}, + {1.0f, 0.94488f, 0.f}, {1.0f, 0.95276f, 0.f}, {1.0f, 0.96063f, 0.f}, + {1.0f, 0.9685f, 0.f}, {1.0f, 0.97638f, 0.f}, {1.0f, 0.98425f, 0.f}, + {1.0f, 0.99213f, 0.f}, {1.0f, 1.0f, 0.0f}}; + +float colormapJet[128][3] = { + {0.0f, 0.0f, 0.53125f}, {0.0f, 0.0f, 0.5625f}, + {0.0f, 0.0f, 0.59375f}, {0.0f, 0.0f, 0.625f}, + {0.0f, 0.0f, 0.65625f}, {0.0f, 0.0f, 0.6875f}, + {0.0f, 0.0f, 0.71875f}, {0.0f, 0.0f, 0.75f}, + {0.0f, 0.0f, 0.78125f}, {0.0f, 0.0f, 0.8125f}, + {0.0f, 0.0f, 0.84375f}, {0.0f, 0.0f, 0.875f}, + {0.0f, 0.0f, 0.90625f}, {0.0f, 0.0f, 0.9375f}, + {0.0f, 0.0f, 0.96875f}, {0.0f, 0.0f, 1.0f}, + {0.0f, 0.03125f, 1.0f}, {0.0f, 0.0625f, 1.0f}, + {0.0f, 0.09375f, 1.0f}, {0.0f, 0.125f, 1.0f}, + {0.0f, 0.15625f, 1.0f}, {0.0f, 0.1875f, 1.0f}, + {0.0f, 0.21875f, 1.0f}, {0.0f, 0.25f, 1.0f}, + {0.0f, 0.28125f, 1.0f}, {0.0f, 0.3125f, 1.0f}, + {0.0f, 0.34375f, 1.0f}, {0.0f, 0.375f, 1.0f}, + {0.0f, 0.40625f, 1.0f}, {0.0f, 0.4375f, 1.0f}, + {0.0f, 0.46875f, 1.0f}, {0.0f, 0.5f, 1.0f}, + {0.0f, 0.53125f, 1.0f}, {0.0f, 0.5625f, 1.0f}, + {0.0f, 0.59375f, 1.0f}, {0.0f, 0.625f, 1.0f}, + {0.0f, 0.65625f, 1.0f}, {0.0f, 0.6875f, 1.0f}, + {0.0f, 0.71875f, 1.0f}, {0.0f, 0.75f, 1.0f}, + {0.0f, 0.78125f, 1.0f}, {0.0f, 0.8125f, 1.0f}, + {0.0f, 0.84375f, 1.0f}, {0.0f, 0.875f, 1.0f}, + {0.0f, 0.90625f, 1.0f}, {0.0f, 0.9375f, 1.0f}, + {0.0f, 0.96875f, 1.0f}, {0.0f, 1.0f, 1.0f}, + {0.03125f, 1.0f, 0.96875f}, {0.0625f, 1.0f, 0.9375f}, + {0.09375f, 1.0f, 0.90625f}, {0.125f, 1.0f, 0.875f}, + {0.15625f, 1.0f, 0.84375f}, {0.1875f, 1.0f, 0.8125f}, + {0.21875f, 1.0f, 0.78125f}, {0.25f, 1.0f, 0.75f}, + {0.28125f, 1.0f, 0.71875f}, {0.3125f, 1.0f, 0.6875f}, + {0.34375f, 1.0f, 0.65625f}, {0.375f, 1.0f, 0.625f}, + {0.40625f, 1.0f, 0.59375f}, {0.4375f, 1.0f, 0.5625f}, + {0.46875f, 1.0f, 0.53125f}, {0.5f, 1.0f, 0.5f}, + {0.53125f, 1.0f, 0.46875f}, {0.5625f, 1.0f, 0.4375f}, + {0.59375f, 1.0f, 0.40625f}, {0.625f, 1.0f, 0.375f}, + {0.65625f, 1.0f, 0.34375f}, {0.6875f, 1.0f, 0.3125f}, + {0.71875f, 1.0f, 0.28125f}, {0.75f, 1.0f, 0.25f}, + {0.78125f, 1.0f, 0.21875f}, {0.8125f, 1.0f, 0.1875f}, + {0.84375f, 1.0f, 0.15625f}, {0.875f, 1.0f, 0.125f}, + {0.90625f, 1.0f, 0.09375f}, {0.9375f, 1.0f, 0.0625f}, + {0.96875f, 1.0f, 0.03125f}, {1.0f, 1.0f, 0.0f}, + {1.0f, 0.96875f, 0.0f}, {1.0f, 0.9375f, 0.0f}, + {1.0f, 0.90625f, 0.0f}, {1.0f, 0.875f, 0.0f}, + {1.0f, 0.84375f, 0.0f}, {1.0f, 0.8125f, 0.0f}, + {1.0f, 0.78125f, 0.0f}, {1.0f, 0.75f, 0.0f}, + {1.0f, 0.71875f, 0.0f}, {1.0f, 0.6875f, 0.0f}, + {1.0f, 0.65625f, 0.0f}, {1.0f, 0.625f, 0.0f}, + {1.0f, 0.59375f, 0.0f}, {1.0f, 0.5625f, 0.0f}, + {1.0f, 0.53125f, 0.0f}, {1.0f, 0.5f, 0.0f}, + {1.0f, 0.46875f, 0.0f}, {1.0f, 0.4375f, 0.0f}, + {1.0f, 0.40625f, 0.0f}, {1.0f, 0.375f, 0.0f}, + {1.0f, 0.34375f, 0.0f}, {1.0f, 0.3125f, 0.0f}, + {1.0f, 0.28125f, 0.0f}, {1.0f, 0.25f, 0.0f}, + {1.0f, 0.21875f, 0.0f}, {1.0f, 0.1875f, 0.0f}, + {1.0f, 0.15625f, 0.0f}, {1.0f, 0.125f, 0.0f}, + {1.0f, 0.09375f, 0.0f}, {1.0f, 0.0625f, 0.0f}, + {1.0f, 0.03125f, 0.0f}, {1.0f, 0.0f, 0.0f}, + {0.96875f, 0.0f, 0.0f}, {0.9375f, 0.0f, 0.0f}, + {0.90625f, 0.0f, 0.0f}, {0.875f, 0.0f, 0.0f}, + {0.84375f, 0.0f, 0.0f}, {0.8125f, 0.0f, 0.0f}, + {0.78125f, 0.0f, 0.0f}, {0.75f, 0.0f, 0.0f}, + {0.71875f, 0.0f, 0.0f}, {0.6875f, 0.0f, 0.0f}, + {0.65625f, 0.0f, 0.0f}, {0.625f, 0.0f, 0.0f}, + {0.59375f, 0.0f, 0.0f}, {0.5625f, 0.0f, 0.0f}, + {0.53125f, 0.0f, 0.0f}, {0.5f, 0.0f, 0.0f}}; + +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 > 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 ¢erX, double ¢erY, + 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; +} + +long int timestampDiff(uint64_t t1, uint64_t t2) { + if (t2 > t1) { + uint64_t d = t2 - t1; + + if (d > std::numeric_limits::max()) { + return std::numeric_limits::max(); + } else { + return d; + } + } else { + uint64_t d = t1 - t2; + + if (d > std::numeric_limits::max()) { + return std::numeric_limits::min(); + } else { + return -static_cast(d); + } + } +} +} diff --git a/src/mynteye/api/camodocal/src/sparse_graph/Transform.cc b/src/mynteye/api/camodocal/src/sparse_graph/Transform.cc new file mode 100644 index 0000000..8bb568d --- /dev/null +++ b/src/mynteye/api/camodocal/src/sparse_graph/Transform.cc @@ -0,0 +1,55 @@ +#include + +namespace camodocal { + +Transform::Transform() { + m_q.setIdentity(); + m_t.setZero(); +} + +Transform::Transform(const Eigen::Matrix4d &H) { + m_q = Eigen::Quaterniond(H.block<3, 3>(0, 0)); + m_t = H.block<3, 1>(0, 3); +} + +Eigen::Quaterniond &Transform::rotation(void) { + return m_q; +} + +const Eigen::Quaterniond &Transform::rotation(void) const { + return m_q; +} + +double *Transform::rotationData(void) { + return m_q.coeffs().data(); +} + +const double *const Transform::rotationData(void) const { + return m_q.coeffs().data(); +} + +Eigen::Vector3d &Transform::translation(void) { + return m_t; +} + +const Eigen::Vector3d &Transform::translation(void) const { + return m_t; +} + +double *Transform::translationData(void) { + return m_t.data(); +} + +const double *const Transform::translationData(void) const { + return m_t.data(); +} + +Eigen::Matrix4d Transform::toMatrix(void) const { + Eigen::Matrix4d H; + H.setIdentity(); + H.block<3, 3>(0, 0) = m_q.toRotationMatrix(); + H.block<3, 1>(0, 3) = m_t; + + return H; +} +}