245 lines
7.0 KiB
C++
245 lines
7.0 KiB
C++
// Copyright 2018 Slightech Co., Ltd. All rights reserved.
|
|
//
|
|
// Licensed under the Apache License, Version 2.0 (the "License");
|
|
// you may not use this file except in compliance with the License.
|
|
// You may obtain a copy of the License at
|
|
//
|
|
// http://www.apache.org/licenses/LICENSE-2.0
|
|
//
|
|
// Unless required by applicable law or agreed to in writing, software
|
|
// distributed under the License is distributed on an "AS IS" BASIS,
|
|
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
// See the License for the specific language governing permissions and
|
|
// limitations under the License.
|
|
#ifndef MYNTEYE_CAMERA_MODELS_EQUIDISTANT_CAMERA_H_
|
|
#define MYNTEYE_CAMERA_MODELS_EQUIDISTANT_CAMERA_H_
|
|
|
|
#include <string>
|
|
#include <opencv2/core/core.hpp>
|
|
|
|
#include "camera.h"
|
|
|
|
MYNTEYE_BEGIN_NAMESPACE
|
|
|
|
namespace models {
|
|
|
|
/**
|
|
* J. Kannala, and S. Brandt, A Generic Camera Model and Calibration Method
|
|
* for Conventional, Wide-Angle, and Fish-Eye Lenses, PAMI 2006
|
|
*/
|
|
|
|
template <typename T> inline
|
|
void QuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) {
|
|
// 'scale' is 1 / norm(q).
|
|
const T scale = T(1) / sqrt(q[0] * q[0] +
|
|
q[1] * q[1] +
|
|
q[2] * q[2] +
|
|
q[3] * q[3]);
|
|
|
|
// Make unit-norm version of q.
|
|
const T unit[4] = {
|
|
scale * q[0],
|
|
scale * q[1],
|
|
scale * q[2],
|
|
scale * q[3],
|
|
};
|
|
|
|
UnitQuaternionRotatePoint(unit, pt, result);
|
|
}
|
|
|
|
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;
|
|
|
|
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
|
|
*/
|
|
explicit 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<std::vector<cv::Point3f> > &objectPoints,
|
|
const std::vector<std::vector<cv::Point2f> > &imagePoints);
|
|
|
|
// Lift points from the image plane to the projective space
|
|
void liftProjective(const ctain::Vector2d &p, ctain::Vector3d &P) const; // NOLINT
|
|
// %output P
|
|
|
|
// Projects 3D points to the image plane (Pi function)
|
|
void spaceToPlane(const ctain::Vector3d &P, ctain::Vector2d &p) const; // NOLINT
|
|
// %output p
|
|
|
|
|
|
// Projects 3D points to the image plane (Pi function)
|
|
// and calculates jacobian
|
|
void spaceToPlane(
|
|
const ctain::Vector3d &P,ctain::Vector2d &p, // NOLINT
|
|
ctain::Matrix23d &J) const; // NOLINT
|
|
// %output p
|
|
// %output J
|
|
|
|
template <typename T>
|
|
static void spaceToPlane(
|
|
const T *const params, const T *const q, const T *const t,
|
|
const ctain::Matrix<T> &P, ctain::Matrix<T> &p); // NOLINT
|
|
|
|
void initUndistortMap(
|
|
cv::Mat &map1, cv::Mat &map2, double fScale = 1.0) const; // NOLINT
|
|
|
|
cv::Mat initUndistortRectifyMap(
|
|
cv::Mat &map1, cv::Mat &map2, float fx = -1.0f, float fy = -1.0f, // NOLINT
|
|
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<double> ¶meterVec);
|
|
void writeParameters(std::vector<double> ¶meterVec) const; // NOLINT
|
|
|
|
std::string parametersToString(void) const;
|
|
|
|
private:
|
|
template <typename T>
|
|
static T r(T k2, T k3, T k4, T k5, T theta);
|
|
|
|
void fitOddPoly(
|
|
const std::vector<double> &x, const std::vector<double> &y, int n,
|
|
std::vector<double> &coeffs) const; // NOLINT
|
|
|
|
void backprojectSymmetric(
|
|
const ctain::Vector2d &p_u, double &theta, double &phi) const; // NOLINT
|
|
|
|
Parameters mParameters;
|
|
|
|
double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23;
|
|
};
|
|
|
|
typedef std::shared_ptr<EquidistantCamera> EquidistantCameraPtr;
|
|
typedef std::shared_ptr<const EquidistantCamera> EquidistantCameraConstPtr; // NOLINT
|
|
|
|
template <typename T>
|
|
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 <typename T>
|
|
void spaceToPlane(
|
|
const T *const params, const T *const q, const T *const t,
|
|
const ctain::Matrix<T> &P, ctain::Matrix<T> &p) { // NOLINT
|
|
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];
|
|
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]);
|
|
|
|
ctain::Matrix<T> p_u(2, 1), tmp(2, 1);
|
|
tmp(0) = cos(phi);
|
|
tmp(1) = sin(phi);
|
|
p_u = r(k2, k3, k4, k5, theta) * tmp;
|
|
|
|
p(0) = mu * p_u(0) + u0;
|
|
p(1) = mv * p_u(1) + v0;
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
MYNTEYE_END_NAMESPACE
|
|
|
|
#endif // MYNTEYE_CAMERA_MODELS_EQUIDISTANT_CAMERA_H_
|
|
|