fixed vector size mismatch between matched and status vectors
This commit is contained in:
admin1
571
camera_models/src/calib/CameraCalibration.cc
Normal file
571
camera_models/src/calib/CameraCalibration.cc
Normal file
@@ -0,0 +1,571 @@
|
||||
#include "camodocal/calib/CameraCalibration.h"
|
||||
|
||||
#include <cstdio>
|
||||
#include <eigen3/Eigen/Dense>
|
||||
#include <iomanip>
|
||||
#include <iostream>
|
||||
#include <algorithm>
|
||||
#include <fstream>
|
||||
#include <opencv2/core/core.hpp>
|
||||
#include <opencv2/core/eigen.hpp>
|
||||
#include <opencv2/imgproc/imgproc.hpp>
|
||||
#include <opencv2/calib3d/calib3d.hpp>
|
||||
|
||||
#include "camodocal/camera_models/CameraFactory.h"
|
||||
#include "camodocal/sparse_graph/Transform.h"
|
||||
#include "camodocal/gpl/EigenQuaternionParameterization.h"
|
||||
#include "camodocal/gpl/EigenUtils.h"
|
||||
#include "camodocal/camera_models/CostFunctionFactory.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<cv::Point2f>& corners)
|
||||
{
|
||||
m_imagePoints.push_back(corners);
|
||||
|
||||
std::vector<cv::Point3f> 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<cv::Mat> rvecs;
|
||||
std::vector<cv::Mat> 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<double>(i,0) = rvecs.at(i).at<double>(0);
|
||||
m_cameraPoses.at<double>(i,1) = rvecs.at(i).at<double>(1);
|
||||
m_cameraPoses.at<double>(i,2) = rvecs.at(i).at<double>(2);
|
||||
m_cameraPoses.at<double>(i,3) = tvecs.at(i).at<double>(0);
|
||||
m_cameraPoses.at<double>(i,4) = tvecs.at(i).at<double>(1);
|
||||
m_cameraPoses.at<double>(i,5) = tvecs.at(i).at<double>(2);
|
||||
}
|
||||
|
||||
// Compute measurement covariance.
|
||||
std::vector<std::vector<cv::Point2f> > 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<cv::Point2f> 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<double>(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<double>(errCount);
|
||||
measurementCovariance(1,0) = measurementCovariance(0,1);
|
||||
|
||||
m_measurementCovariance = measurementCovariance;
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
int
|
||||
CameraCalibration::sampleCount(void) const
|
||||
{
|
||||
return m_imagePoints.size();
|
||||
}
|
||||
|
||||
std::vector<std::vector<cv::Point2f> >&
|
||||
CameraCalibration::imagePoints(void)
|
||||
{
|
||||
return m_imagePoints;
|
||||
}
|
||||
|
||||
const std::vector<std::vector<cv::Point2f> >&
|
||||
CameraCalibration::imagePoints(void) const
|
||||
{
|
||||
return m_imagePoints;
|
||||
}
|
||||
|
||||
std::vector<std::vector<cv::Point3f> >&
|
||||
CameraCalibration::scenePoints(void)
|
||||
{
|
||||
return m_scenePoints;
|
||||
}
|
||||
|
||||
const std::vector<std::vector<cv::Point3f> >&
|
||||
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<cv::Mat>& images) const
|
||||
{
|
||||
std::vector<cv::Mat> rvecs, tvecs;
|
||||
|
||||
for (size_t i = 0; i < images.size(); ++i)
|
||||
{
|
||||
cv::Mat rvec(3, 1, CV_64F);
|
||||
rvec.at<double>(0) = m_cameraPoses.at<double>(i,0);
|
||||
rvec.at<double>(1) = m_cameraPoses.at<double>(i,1);
|
||||
rvec.at<double>(2) = m_cameraPoses.at<double>(i,2);
|
||||
|
||||
cv::Mat tvec(3, 1, CV_64F);
|
||||
tvec.at<double>(0) = m_cameraPoses.at<double>(i,3);
|
||||
tvec.at<double>(1) = m_cameraPoses.at<double>(i,4);
|
||||
tvec.at<double>(2) = m_cameraPoses.at<double>(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<cv::Point2f> estImagePoints;
|
||||
m_camera->projectPoints(m_scenePoints.at(i), rvecs.at(i), tvecs.at(i),
|
||||
estImagePoints);
|
||||
|
||||
float errorSum = 0.0f;
|
||||
float errorMax = std::numeric_limits<float>::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<double>(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<double>(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<cv::Mat>& rvecs, std::vector<cv::Mat>& 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<cv::Mat>& rvecs, std::vector<cv::Mat>& tvecs) const
|
||||
{
|
||||
// Use ceres to do optimization
|
||||
ceres::Problem problem;
|
||||
|
||||
std::vector<Transform, Eigen::aligned_allocator<Transform> > 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<double>(0),
|
||||
tvecs[i].at<double>(1),
|
||||
tvecs[i].at<double>(2);
|
||||
}
|
||||
|
||||
std::vector<double> 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<double>(0) = transformVec.at(i).translation()(0);
|
||||
tvec.at<double>(1) = transformVec.at(i).translation()(1);
|
||||
tvec.at<double>(2) = transformVec.at(i).translation()(2);
|
||||
}
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
void
|
||||
CameraCalibration::readData(std::ifstream& ifs, T& data) const
|
||||
{
|
||||
char* buffer = new char[sizeof(T)];
|
||||
|
||||
ifs.read(buffer, sizeof(T));
|
||||
|
||||
data = *(reinterpret_cast<T*>(buffer));
|
||||
|
||||
delete[] buffer;
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
void
|
||||
CameraCalibration::writeData(std::ofstream& ofs, T data) const
|
||||
{
|
||||
char* pData = reinterpret_cast<char*>(&data);
|
||||
|
||||
ofs.write(pData, sizeof(T));
|
||||
}
|
||||
|
||||
}
|
||||
252
camera_models/src/camera_models/Camera.cc
Normal file
252
camera_models/src/camera_models/Camera.cc
Normal file
@@ -0,0 +1,252 @@
|
||||
#include "camodocal/camera_models/Camera.h"
|
||||
#include "camodocal/camera_models/ScaramuzzaCamera.h"
|
||||
|
||||
#include <opencv2/calib3d/calib3d.hpp>
|
||||
|
||||
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<cv::Point3f>& objectPoints,
|
||||
const std::vector<cv::Point2f>& imagePoints,
|
||||
cv::Mat& rvec, cv::Mat& tvec) const
|
||||
{
|
||||
std::vector<cv::Point2f> Ms(imagePoints.size());
|
||||
for (size_t i = 0; i < Ms.size(); ++i)
|
||||
{
|
||||
Eigen::Vector3d P;
|
||||
liftProjective(Eigen::Vector2d(imagePoints.at(i).x, imagePoints.at(i).y), P);
|
||||
|
||||
P /= P(2);
|
||||
|
||||
Ms.at(i).x = P(0);
|
||||
Ms.at(i).y = P(1);
|
||||
}
|
||||
|
||||
// assume unit focal length, zero principal point, and zero distortion
|
||||
cv::solvePnP(objectPoints, Ms, cv::Mat::eye(3, 3, CV_64F), cv::noArray(), rvec, tvec);
|
||||
}
|
||||
|
||||
double
|
||||
Camera::reprojectionDist(const Eigen::Vector3d& P1, const Eigen::Vector3d& P2) const
|
||||
{
|
||||
Eigen::Vector2d p1, p2;
|
||||
|
||||
spaceToPlane(P1, p1);
|
||||
spaceToPlane(P2, p2);
|
||||
|
||||
return (p1 - p2).norm();
|
||||
}
|
||||
|
||||
double
|
||||
Camera::reprojectionError(const std::vector< std::vector<cv::Point3f> >& objectPoints,
|
||||
const std::vector< std::vector<cv::Point2f> >& imagePoints,
|
||||
const std::vector<cv::Mat>& rvecs,
|
||||
const std::vector<cv::Mat>& tvecs,
|
||||
cv::OutputArray _perViewErrors) 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<cv::Point2f> 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<double>(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<cv::Point3f>& objectPoints,
|
||||
const cv::Mat& rvec,
|
||||
const cv::Mat& tvec,
|
||||
std::vector<cv::Point2f>& 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<double>(0,0), R0.at<double>(0,1), R0.at<double>(0,2),
|
||||
R0.at<double>(1,0), R0.at<double>(1,1), R0.at<double>(1,2),
|
||||
R0.at<double>(2,0), R0.at<double>(2,1), R0.at<double>(2,2);
|
||||
|
||||
Eigen::Vector3d t;
|
||||
t << tvec.at<double>(0), tvec.at<double>(1), tvec.at<double>(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)));
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
190
camera_models/src/camera_models/CameraFactory.cc
Normal file
190
camera_models/src/camera_models/CameraFactory.cc
Normal file
@@ -0,0 +1,190 @@
|
||||
#include "camodocal/camera_models/CameraFactory.h"
|
||||
|
||||
#include <boost/algorithm/string.hpp>
|
||||
|
||||
#include "camodocal/camera_models/CataCamera.h"
|
||||
#include "camodocal/camera_models/EquidistantCamera.h"
|
||||
#include "camodocal/camera_models/PinholeCamera.h"
|
||||
#include "camodocal/camera_models/PinholeFullCamera.h"
|
||||
#include "camodocal/camera_models/ScaramuzzaCamera.h"
|
||||
|
||||
#include "ceres/ceres.h"
|
||||
|
||||
namespace camodocal
|
||||
{
|
||||
|
||||
boost::shared_ptr< CameraFactory > CameraFactory::m_instance;
|
||||
|
||||
CameraFactory::CameraFactory( ) {}
|
||||
|
||||
boost::shared_ptr< CameraFactory >
|
||||
CameraFactory::instance( void )
|
||||
{
|
||||
if ( m_instance.get( ) == 0 )
|
||||
{
|
||||
m_instance.reset( new CameraFactory );
|
||||
}
|
||||
|
||||
return m_instance;
|
||||
}
|
||||
|
||||
CameraPtr
|
||||
CameraFactory::generateCamera( Camera::ModelType modelType, const std::string& cameraName, cv::Size imageSize ) const
|
||||
{
|
||||
switch ( modelType )
|
||||
{
|
||||
case Camera::KANNALA_BRANDT:
|
||||
{
|
||||
EquidistantCameraPtr camera( new EquidistantCamera );
|
||||
|
||||
EquidistantCamera::Parameters params = camera->getParameters( );
|
||||
params.cameraName( ) = cameraName;
|
||||
params.imageWidth( ) = imageSize.width;
|
||||
params.imageHeight( ) = imageSize.height;
|
||||
camera->setParameters( params );
|
||||
return camera;
|
||||
}
|
||||
case Camera::PINHOLE:
|
||||
{
|
||||
PinholeCameraPtr camera( new PinholeCamera );
|
||||
|
||||
PinholeCamera::Parameters params = camera->getParameters( );
|
||||
params.cameraName( ) = cameraName;
|
||||
params.imageWidth( ) = imageSize.width;
|
||||
params.imageHeight( ) = imageSize.height;
|
||||
camera->setParameters( params );
|
||||
return camera;
|
||||
}
|
||||
case Camera::PINHOLE_FULL:
|
||||
{
|
||||
PinholeFullCameraPtr camera( new PinholeFullCamera );
|
||||
|
||||
PinholeFullCamera::Parameters params = camera->getParameters( );
|
||||
params.cameraName( ) = cameraName;
|
||||
params.imageWidth( ) = imageSize.width;
|
||||
params.imageHeight( ) = imageSize.height;
|
||||
camera->setParameters( params );
|
||||
return camera;
|
||||
}
|
||||
case Camera::SCARAMUZZA:
|
||||
{
|
||||
OCAMCameraPtr camera( new OCAMCamera );
|
||||
|
||||
OCAMCamera::Parameters params = camera->getParameters( );
|
||||
params.cameraName( ) = cameraName;
|
||||
params.imageWidth( ) = imageSize.width;
|
||||
params.imageHeight( ) = imageSize.height;
|
||||
camera->setParameters( params );
|
||||
return camera;
|
||||
}
|
||||
case Camera::MEI:
|
||||
default:
|
||||
{
|
||||
CataCameraPtr camera( new CataCamera );
|
||||
|
||||
CataCamera::Parameters params = camera->getParameters( );
|
||||
params.cameraName( ) = cameraName;
|
||||
params.imageWidth( ) = imageSize.width;
|
||||
params.imageHeight( ) = imageSize.height;
|
||||
camera->setParameters( params );
|
||||
return camera;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
CameraPtr
|
||||
CameraFactory::generateCameraFromYamlFile( const std::string& filename )
|
||||
{
|
||||
cv::FileStorage fs( filename, cv::FileStorage::READ );
|
||||
|
||||
if ( !fs.isOpened( ) )
|
||||
{
|
||||
return CameraPtr( );
|
||||
}
|
||||
|
||||
Camera::ModelType modelType = Camera::MEI;
|
||||
if ( !fs["model_type"].isNone( ) )
|
||||
{
|
||||
std::string sModelType;
|
||||
fs["model_type"] >> sModelType;
|
||||
|
||||
if ( boost::iequals( sModelType, "kannala_brandt" ) )
|
||||
{
|
||||
modelType = Camera::KANNALA_BRANDT;
|
||||
}
|
||||
else if ( boost::iequals( sModelType, "mei" ) )
|
||||
{
|
||||
modelType = Camera::MEI;
|
||||
}
|
||||
else if ( boost::iequals( sModelType, "scaramuzza" ) )
|
||||
{
|
||||
modelType = Camera::SCARAMUZZA;
|
||||
}
|
||||
else if ( boost::iequals( sModelType, "pinhole" ) )
|
||||
{
|
||||
modelType = Camera::PINHOLE;
|
||||
}
|
||||
else if ( boost::iequals( sModelType, "PINHOLE_FULL" ) )
|
||||
{
|
||||
modelType = Camera::PINHOLE_FULL;
|
||||
}
|
||||
else
|
||||
{
|
||||
std::cerr << "# ERROR: Unknown camera model: " << sModelType << std::endl;
|
||||
return CameraPtr( );
|
||||
}
|
||||
}
|
||||
|
||||
switch ( modelType )
|
||||
{
|
||||
case Camera::KANNALA_BRANDT:
|
||||
{
|
||||
EquidistantCameraPtr camera( new EquidistantCamera );
|
||||
|
||||
EquidistantCamera::Parameters params = camera->getParameters( );
|
||||
params.readFromYamlFile( filename );
|
||||
camera->setParameters( params );
|
||||
return camera;
|
||||
}
|
||||
case Camera::PINHOLE:
|
||||
{
|
||||
PinholeCameraPtr camera( new PinholeCamera );
|
||||
|
||||
PinholeCamera::Parameters params = camera->getParameters( );
|
||||
params.readFromYamlFile( filename );
|
||||
camera->setParameters( params );
|
||||
return camera;
|
||||
}
|
||||
case Camera::PINHOLE_FULL:
|
||||
{
|
||||
PinholeFullCameraPtr camera( new PinholeFullCamera );
|
||||
|
||||
PinholeFullCamera::Parameters params = camera->getParameters( );
|
||||
params.readFromYamlFile( filename );
|
||||
camera->setParameters( params );
|
||||
return camera;
|
||||
}
|
||||
case Camera::SCARAMUZZA:
|
||||
{
|
||||
OCAMCameraPtr camera( new OCAMCamera );
|
||||
|
||||
OCAMCamera::Parameters params = camera->getParameters( );
|
||||
params.readFromYamlFile( filename );
|
||||
camera->setParameters( params );
|
||||
return camera;
|
||||
}
|
||||
case Camera::MEI:
|
||||
default:
|
||||
{
|
||||
CataCameraPtr camera( new CataCamera );
|
||||
|
||||
CataCamera::Parameters params = camera->getParameters( );
|
||||
params.readFromYamlFile( filename );
|
||||
camera->setParameters( params );
|
||||
return camera;
|
||||
}
|
||||
}
|
||||
|
||||
return CameraPtr( );
|
||||
}
|
||||
}
|
||||
1005
camera_models/src/camera_models/CataCamera.cc
Normal file
1005
camera_models/src/camera_models/CataCamera.cc
Normal file
File diff suppressed because it is too large
Load Diff
1384
camera_models/src/camera_models/CostFunctionFactory.cc
Normal file
1384
camera_models/src/camera_models/CostFunctionFactory.cc
Normal file
File diff suppressed because it is too large
Load Diff
820
camera_models/src/camera_models/EquidistantCamera.cc
Normal file
820
camera_models/src/camera_models/EquidistantCamera.cc
Normal file
@@ -0,0 +1,820 @@
|
||||
#include "camodocal/camera_models/EquidistantCamera.h"
|
||||
|
||||
#include <cmath>
|
||||
#include <cstdio>
|
||||
#include <eigen3/Eigen/Dense>
|
||||
#include <iomanip>
|
||||
#include <iostream>
|
||||
#include <opencv2/calib3d/calib3d.hpp>
|
||||
#include <opencv2/core/eigen.hpp>
|
||||
#include <opencv2/imgproc/imgproc.hpp>
|
||||
|
||||
#include "camodocal/gpl/gpl.h"
|
||||
|
||||
namespace camodocal
|
||||
{
|
||||
|
||||
EquidistantCamera::Parameters::Parameters()
|
||||
: Camera::Parameters(KANNALA_BRANDT)
|
||||
, m_k2(0.0)
|
||||
, m_k3(0.0)
|
||||
, m_k4(0.0)
|
||||
, m_k5(0.0)
|
||||
, m_mu(0.0)
|
||||
, m_mv(0.0)
|
||||
, m_u0(0.0)
|
||||
, m_v0(0.0)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
EquidistantCamera::Parameters::Parameters(const std::string& cameraName,
|
||||
int w, int h,
|
||||
double k2, double k3, double k4, double k5,
|
||||
double mu, double mv,
|
||||
double u0, double v0)
|
||||
: Camera::Parameters(KANNALA_BRANDT, cameraName, w, h)
|
||||
, m_k2(k2)
|
||||
, m_k3(k3)
|
||||
, m_k4(k4)
|
||||
, m_k5(k5)
|
||||
, m_mu(mu)
|
||||
, m_mv(mv)
|
||||
, m_u0(u0)
|
||||
, m_v0(v0)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
double&
|
||||
EquidistantCamera::Parameters::k2(void)
|
||||
{
|
||||
return m_k2;
|
||||
}
|
||||
|
||||
double&
|
||||
EquidistantCamera::Parameters::k3(void)
|
||||
{
|
||||
return m_k3;
|
||||
}
|
||||
|
||||
double&
|
||||
EquidistantCamera::Parameters::k4(void)
|
||||
{
|
||||
return m_k4;
|
||||
}
|
||||
|
||||
double&
|
||||
EquidistantCamera::Parameters::k5(void)
|
||||
{
|
||||
return m_k5;
|
||||
}
|
||||
|
||||
double&
|
||||
EquidistantCamera::Parameters::mu(void)
|
||||
{
|
||||
return m_mu;
|
||||
}
|
||||
|
||||
double&
|
||||
EquidistantCamera::Parameters::mv(void)
|
||||
{
|
||||
return m_mv;
|
||||
}
|
||||
|
||||
double&
|
||||
EquidistantCamera::Parameters::u0(void)
|
||||
{
|
||||
return m_u0;
|
||||
}
|
||||
|
||||
double&
|
||||
EquidistantCamera::Parameters::v0(void)
|
||||
{
|
||||
return m_v0;
|
||||
}
|
||||
|
||||
double
|
||||
EquidistantCamera::Parameters::k2(void) const
|
||||
{
|
||||
return m_k2;
|
||||
}
|
||||
|
||||
double
|
||||
EquidistantCamera::Parameters::k3(void) const
|
||||
{
|
||||
return m_k3;
|
||||
}
|
||||
|
||||
double
|
||||
EquidistantCamera::Parameters::k4(void) const
|
||||
{
|
||||
return m_k4;
|
||||
}
|
||||
|
||||
double
|
||||
EquidistantCamera::Parameters::k5(void) const
|
||||
{
|
||||
return m_k5;
|
||||
}
|
||||
|
||||
double
|
||||
EquidistantCamera::Parameters::mu(void) const
|
||||
{
|
||||
return m_mu;
|
||||
}
|
||||
|
||||
double
|
||||
EquidistantCamera::Parameters::mv(void) const
|
||||
{
|
||||
return m_mv;
|
||||
}
|
||||
|
||||
double
|
||||
EquidistantCamera::Parameters::u0(void) const
|
||||
{
|
||||
return m_u0;
|
||||
}
|
||||
|
||||
double
|
||||
EquidistantCamera::Parameters::v0(void) const
|
||||
{
|
||||
return m_v0;
|
||||
}
|
||||
|
||||
bool
|
||||
EquidistantCamera::Parameters::readFromYamlFile(const std::string& filename)
|
||||
{
|
||||
cv::FileStorage fs(filename, cv::FileStorage::READ);
|
||||
|
||||
if (!fs.isOpened())
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!fs["model_type"].isNone())
|
||||
{
|
||||
std::string sModelType;
|
||||
fs["model_type"] >> sModelType;
|
||||
|
||||
if (sModelType.compare("KANNALA_BRANDT") != 0)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
m_modelType = KANNALA_BRANDT;
|
||||
fs["camera_name"] >> m_cameraName;
|
||||
m_imageWidth = static_cast<int>(fs["image_width"]);
|
||||
m_imageHeight = static_cast<int>(fs["image_height"]);
|
||||
|
||||
cv::FileNode n = fs["projection_parameters"];
|
||||
m_k2 = static_cast<double>(n["k2"]);
|
||||
m_k3 = static_cast<double>(n["k3"]);
|
||||
m_k4 = static_cast<double>(n["k4"]);
|
||||
m_k5 = static_cast<double>(n["k5"]);
|
||||
m_mu = static_cast<double>(n["mu"]);
|
||||
m_mv = static_cast<double>(n["mv"]);
|
||||
m_u0 = static_cast<double>(n["u0"]);
|
||||
m_v0 = static_cast<double>(n["v0"]);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void
|
||||
EquidistantCamera::Parameters::writeToYamlFile(const std::string& filename) const
|
||||
{
|
||||
cv::FileStorage fs(filename, cv::FileStorage::WRITE);
|
||||
|
||||
fs << "model_type" << "KANNALA_BRANDT";
|
||||
fs << "camera_name" << m_cameraName;
|
||||
fs << "image_width" << m_imageWidth;
|
||||
fs << "image_height" << m_imageHeight;
|
||||
|
||||
// projection: k2, k3, k4, k5, mu, mv, u0, v0
|
||||
fs << "projection_parameters";
|
||||
fs << "{" << "k2" << m_k2
|
||||
<< "k3" << m_k3
|
||||
<< "k4" << m_k4
|
||||
<< "k5" << m_k5
|
||||
<< "mu" << m_mu
|
||||
<< "mv" << m_mv
|
||||
<< "u0" << m_u0
|
||||
<< "v0" << m_v0 << "}";
|
||||
|
||||
fs.release();
|
||||
}
|
||||
|
||||
EquidistantCamera::Parameters&
|
||||
EquidistantCamera::Parameters::operator=(const EquidistantCamera::Parameters& other)
|
||||
{
|
||||
if (this != &other)
|
||||
{
|
||||
m_modelType = other.m_modelType;
|
||||
m_cameraName = other.m_cameraName;
|
||||
m_imageWidth = other.m_imageWidth;
|
||||
m_imageHeight = other.m_imageHeight;
|
||||
m_k2 = other.m_k2;
|
||||
m_k3 = other.m_k3;
|
||||
m_k4 = other.m_k4;
|
||||
m_k5 = other.m_k5;
|
||||
m_mu = other.m_mu;
|
||||
m_mv = other.m_mv;
|
||||
m_u0 = other.m_u0;
|
||||
m_v0 = other.m_v0;
|
||||
}
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
std::ostream&
|
||||
operator<< (std::ostream& out, const EquidistantCamera::Parameters& params)
|
||||
{
|
||||
out << "Camera Parameters:" << std::endl;
|
||||
out << " model_type " << "KANNALA_BRANDT" << std::endl;
|
||||
out << " camera_name " << params.m_cameraName << std::endl;
|
||||
out << " image_width " << params.m_imageWidth << std::endl;
|
||||
out << " image_height " << params.m_imageHeight << std::endl;
|
||||
|
||||
// projection: k2, k3, k4, k5, mu, mv, u0, v0
|
||||
out << "Projection Parameters" << std::endl;
|
||||
out << " k2 " << params.m_k2 << std::endl
|
||||
<< " k3 " << params.m_k3 << std::endl
|
||||
<< " k4 " << params.m_k4 << std::endl
|
||||
<< " k5 " << params.m_k5 << std::endl
|
||||
<< " mu " << params.m_mu << std::endl
|
||||
<< " mv " << params.m_mv << std::endl
|
||||
<< " u0 " << params.m_u0 << std::endl
|
||||
<< " v0 " << params.m_v0 << std::endl;
|
||||
|
||||
return out;
|
||||
}
|
||||
|
||||
EquidistantCamera::EquidistantCamera()
|
||||
: m_inv_K11(1.0)
|
||||
, m_inv_K13(0.0)
|
||||
, m_inv_K22(1.0)
|
||||
, m_inv_K23(0.0)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
EquidistantCamera::EquidistantCamera(const std::string& cameraName,
|
||||
int imageWidth, int imageHeight,
|
||||
double k2, double k3, double k4, double k5,
|
||||
double mu, double mv,
|
||||
double u0, double v0)
|
||||
: mParameters(cameraName, imageWidth, imageHeight,
|
||||
k2, k3, k4, k5, mu, mv, u0, v0)
|
||||
{
|
||||
// Inverse camera projection matrix parameters
|
||||
m_inv_K11 = 1.0 / mParameters.mu();
|
||||
m_inv_K13 = -mParameters.u0() / mParameters.mu();
|
||||
m_inv_K22 = 1.0 / mParameters.mv();
|
||||
m_inv_K23 = -mParameters.v0() / mParameters.mv();
|
||||
}
|
||||
|
||||
EquidistantCamera::EquidistantCamera(const EquidistantCamera::Parameters& params)
|
||||
: mParameters(params)
|
||||
{
|
||||
// Inverse camera projection matrix parameters
|
||||
m_inv_K11 = 1.0 / mParameters.mu();
|
||||
m_inv_K13 = -mParameters.u0() / mParameters.mu();
|
||||
m_inv_K22 = 1.0 / mParameters.mv();
|
||||
m_inv_K23 = -mParameters.v0() / mParameters.mv();
|
||||
}
|
||||
|
||||
Camera::ModelType
|
||||
EquidistantCamera::modelType(void) const
|
||||
{
|
||||
return mParameters.modelType();
|
||||
}
|
||||
|
||||
const std::string&
|
||||
EquidistantCamera::cameraName(void) const
|
||||
{
|
||||
return mParameters.cameraName();
|
||||
}
|
||||
|
||||
int
|
||||
EquidistantCamera::imageWidth(void) const
|
||||
{
|
||||
return mParameters.imageWidth();
|
||||
}
|
||||
|
||||
int
|
||||
EquidistantCamera::imageHeight(void) const
|
||||
{
|
||||
return mParameters.imageHeight();
|
||||
}
|
||||
|
||||
void
|
||||
EquidistantCamera::estimateIntrinsics(const cv::Size& boardSize,
|
||||
const std::vector< std::vector<cv::Point3f> >& objectPoints,
|
||||
const std::vector< std::vector<cv::Point2f> >& imagePoints)
|
||||
{
|
||||
Parameters params = getParameters();
|
||||
|
||||
double u0 = params.imageWidth() / 2.0;
|
||||
double v0 = params.imageHeight() / 2.0;
|
||||
|
||||
double minReprojErr = std::numeric_limits<double>::max();
|
||||
|
||||
std::vector<cv::Mat> rvecs, tvecs;
|
||||
rvecs.assign(objectPoints.size(), cv::Mat());
|
||||
tvecs.assign(objectPoints.size(), cv::Mat());
|
||||
|
||||
params.k2() = 0.0;
|
||||
params.k3() = 0.0;
|
||||
params.k4() = 0.0;
|
||||
params.k5() = 0.0;
|
||||
params.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<Eigen::Vector2d> center(boardSize.height);
|
||||
double radius[boardSize.height];
|
||||
for (int r = 0; r < boardSize.height; ++r)
|
||||
{
|
||||
std::vector<cv::Point2d> circle;
|
||||
for (int c = 0; c < boardSize.width; ++c)
|
||||
{
|
||||
circle.push_back(imagePoints.at(i).at(r * boardSize.width + c));
|
||||
}
|
||||
|
||||
fitCircle(circle, center[r](0), center[r](1), radius[r]);
|
||||
}
|
||||
|
||||
for (int j = 0; j < boardSize.height; ++j)
|
||||
{
|
||||
for (int k = j + 1; k < boardSize.height; ++k)
|
||||
{
|
||||
// find distance between pair of vanishing points which
|
||||
// correspond to intersection points of 2 circles
|
||||
std::vector<cv::Point2d> ipts;
|
||||
ipts = intersectCircles(center[j](0), center[j](1), radius[j],
|
||||
center[k](0), center[k](1), radius[k]);
|
||||
|
||||
if (ipts.size() < 2)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
double f = cv::norm(ipts.at(0) - ipts.at(1)) / M_PI;
|
||||
|
||||
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<double>::max())
|
||||
{
|
||||
std::cout << "[" << params.cameraName() << "] "
|
||||
<< "# INFO: kannala-Brandt model fails with given data. " << std::endl;
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
params.mu() = f0;
|
||||
params.mv() = f0;
|
||||
|
||||
setParameters(params);
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Lifts a point from the image plane to the unit sphere
|
||||
*
|
||||
* \param p image coordinates
|
||||
* \param P coordinates of the point on the sphere
|
||||
*/
|
||||
void
|
||||
EquidistantCamera::liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const
|
||||
{
|
||||
liftProjective(p, P);
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Lifts a point from the image plane to its projective ray
|
||||
*
|
||||
* \param p image coordinates
|
||||
* \param P coordinates of the projective ray
|
||||
*/
|
||||
void
|
||||
EquidistantCamera::liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const
|
||||
{
|
||||
// Lift points to normalised plane
|
||||
Eigen::Vector2d p_u;
|
||||
p_u << m_inv_K11 * p(0) + m_inv_K13,
|
||||
m_inv_K22 * p(1) + m_inv_K23;
|
||||
|
||||
// Obtain a projective ray
|
||||
double theta, phi;
|
||||
backprojectSymmetric(p_u, theta, phi);
|
||||
|
||||
P(0) = sin(theta) * cos(phi);
|
||||
P(1) = sin(theta) * sin(phi);
|
||||
P(2) = cos(theta);
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Project a 3D point (\a x,\a y,\a z) to the image plane in (\a u,\a v)
|
||||
*
|
||||
* \param P 3D point coordinates
|
||||
* \param p return value, contains the image point coordinates
|
||||
*/
|
||||
void
|
||||
EquidistantCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const
|
||||
{
|
||||
double theta = acos(P(2) / P.norm());
|
||||
double phi = atan2(P(1), P(0));
|
||||
|
||||
Eigen::Vector2d p_u = r(mParameters.k2(), mParameters.k3(), mParameters.k4(), mParameters.k5(), theta) * Eigen::Vector2d(cos(phi), sin(phi));
|
||||
|
||||
// Apply generalised projection matrix
|
||||
p << mParameters.mu() * p_u(0) + mParameters.u0(),
|
||||
mParameters.mv() * p_u(1) + mParameters.v0();
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* \brief Project a 3D point to the image plane and calculate Jacobian
|
||||
*
|
||||
* \param P 3D point coordinates
|
||||
* \param p return value, contains the image point coordinates
|
||||
*/
|
||||
void
|
||||
EquidistantCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p,
|
||||
Eigen::Matrix<double,2,3>& 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<float>(v,u) = p(0);
|
||||
mapY.at<float>(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<double>(), p);
|
||||
|
||||
mapX.at<float>(v,u) = p(0);
|
||||
mapY.at<float>(v,u) = p(1);
|
||||
}
|
||||
}
|
||||
|
||||
cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false);
|
||||
|
||||
cv::Mat K_rect_cv;
|
||||
cv::eigen2cv(K_rect, K_rect_cv);
|
||||
return K_rect_cv;
|
||||
}
|
||||
|
||||
int
|
||||
EquidistantCamera::parameterCount(void) const
|
||||
{
|
||||
return 8;
|
||||
}
|
||||
|
||||
const EquidistantCamera::Parameters&
|
||||
EquidistantCamera::getParameters(void) const
|
||||
{
|
||||
return mParameters;
|
||||
}
|
||||
|
||||
void
|
||||
EquidistantCamera::setParameters(const EquidistantCamera::Parameters& parameters)
|
||||
{
|
||||
mParameters = parameters;
|
||||
|
||||
// Inverse camera projection matrix parameters
|
||||
m_inv_K11 = 1.0 / mParameters.mu();
|
||||
m_inv_K13 = -mParameters.u0() / mParameters.mu();
|
||||
m_inv_K22 = 1.0 / mParameters.mv();
|
||||
m_inv_K23 = -mParameters.v0() / mParameters.mv();
|
||||
}
|
||||
|
||||
void
|
||||
EquidistantCamera::readParameters(const std::vector<double>& parameterVec)
|
||||
{
|
||||
if (parameterVec.size() != parameterCount())
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
Parameters params = getParameters();
|
||||
|
||||
params.k2() = parameterVec.at(0);
|
||||
params.k3() = parameterVec.at(1);
|
||||
params.k4() = parameterVec.at(2);
|
||||
params.k5() = parameterVec.at(3);
|
||||
params.mu() = parameterVec.at(4);
|
||||
params.mv() = parameterVec.at(5);
|
||||
params.u0() = parameterVec.at(6);
|
||||
params.v0() = parameterVec.at(7);
|
||||
|
||||
setParameters(params);
|
||||
}
|
||||
|
||||
void
|
||||
EquidistantCamera::writeParameters(std::vector<double>& parameterVec) const
|
||||
{
|
||||
parameterVec.resize(parameterCount());
|
||||
parameterVec.at(0) = mParameters.k2();
|
||||
parameterVec.at(1) = mParameters.k3();
|
||||
parameterVec.at(2) = mParameters.k4();
|
||||
parameterVec.at(3) = mParameters.k5();
|
||||
parameterVec.at(4) = mParameters.mu();
|
||||
parameterVec.at(5) = mParameters.mv();
|
||||
parameterVec.at(6) = mParameters.u0();
|
||||
parameterVec.at(7) = mParameters.v0();
|
||||
}
|
||||
|
||||
void
|
||||
EquidistantCamera::writeParametersToYamlFile(const std::string& filename) const
|
||||
{
|
||||
mParameters.writeToYamlFile(filename);
|
||||
}
|
||||
|
||||
std::string
|
||||
EquidistantCamera::parametersToString(void) const
|
||||
{
|
||||
std::ostringstream oss;
|
||||
oss << mParameters;
|
||||
|
||||
return oss.str();
|
||||
}
|
||||
|
||||
void
|
||||
EquidistantCamera::fitOddPoly(const std::vector<double>& x, const std::vector<double>& y,
|
||||
int n, std::vector<double>& coeffs) const
|
||||
{
|
||||
std::vector<int> 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<Eigen::MatrixXd> es(A);
|
||||
Eigen::MatrixXcd eigval = es.eigenvalues();
|
||||
|
||||
std::vector<double> 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());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
881
camera_models/src/camera_models/PinholeCamera.cc
Normal file
881
camera_models/src/camera_models/PinholeCamera.cc
Normal file
@@ -0,0 +1,881 @@
|
||||
#include "camodocal/camera_models/PinholeCamera.h"
|
||||
|
||||
#include <cmath>
|
||||
#include <cstdio>
|
||||
#include <eigen3/Eigen/Dense>
|
||||
#include <iomanip>
|
||||
#include <opencv2/calib3d/calib3d.hpp>
|
||||
#include <opencv2/core/eigen.hpp>
|
||||
#include <opencv2/imgproc/imgproc.hpp>
|
||||
|
||||
#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<int>(fs["image_width"]);
|
||||
m_imageHeight = static_cast<int>(fs["image_height"]);
|
||||
|
||||
cv::FileNode n = fs["distortion_parameters"];
|
||||
m_k1 = static_cast<double>(n["k1"]);
|
||||
m_k2 = static_cast<double>(n["k2"]);
|
||||
m_p1 = static_cast<double>(n["p1"]);
|
||||
m_p2 = static_cast<double>(n["p2"]);
|
||||
|
||||
n = fs["projection_parameters"];
|
||||
m_fx = static_cast<double>(n["fx"]);
|
||||
m_fy = static_cast<double>(n["fy"]);
|
||||
m_cx = static_cast<double>(n["cx"]);
|
||||
m_cy = static_cast<double>(n["cy"]);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void
|
||||
PinholeCamera::Parameters::writeToYamlFile(const std::string& filename) const
|
||||
{
|
||||
cv::FileStorage fs(filename, cv::FileStorage::WRITE);
|
||||
|
||||
fs << "model_type" << "PINHOLE";
|
||||
fs << "camera_name" << m_cameraName;
|
||||
fs << "image_width" << m_imageWidth;
|
||||
fs << "image_height" << m_imageHeight;
|
||||
|
||||
// radial distortion: k1, k2
|
||||
// tangential distortion: p1, p2
|
||||
fs << "distortion_parameters";
|
||||
fs << "{" << "k1" << m_k1
|
||||
<< "k2" << m_k2
|
||||
<< "p1" << m_p1
|
||||
<< "p2" << m_p2 << "}";
|
||||
|
||||
// projection: fx, fy, cx, cy
|
||||
fs << "projection_parameters";
|
||||
fs << "{" << "fx" << m_fx
|
||||
<< "fy" << m_fy
|
||||
<< "cx" << m_cx
|
||||
<< "cy" << m_cy << "}";
|
||||
|
||||
fs.release();
|
||||
}
|
||||
|
||||
PinholeCamera::Parameters&
|
||||
PinholeCamera::Parameters::operator=(const PinholeCamera::Parameters& other)
|
||||
{
|
||||
if (this != &other)
|
||||
{
|
||||
m_modelType = other.m_modelType;
|
||||
m_cameraName = other.m_cameraName;
|
||||
m_imageWidth = other.m_imageWidth;
|
||||
m_imageHeight = other.m_imageHeight;
|
||||
m_k1 = other.m_k1;
|
||||
m_k2 = other.m_k2;
|
||||
m_p1 = other.m_p1;
|
||||
m_p2 = other.m_p2;
|
||||
m_fx = other.m_fx;
|
||||
m_fy = other.m_fy;
|
||||
m_cx = other.m_cx;
|
||||
m_cy = other.m_cy;
|
||||
}
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
std::ostream&
|
||||
operator<< (std::ostream& out, const PinholeCamera::Parameters& params)
|
||||
{
|
||||
out << "Camera Parameters:" << std::endl;
|
||||
out << " model_type " << "PINHOLE" << std::endl;
|
||||
out << " camera_name " << params.m_cameraName << std::endl;
|
||||
out << " image_width " << params.m_imageWidth << std::endl;
|
||||
out << " image_height " << params.m_imageHeight << std::endl;
|
||||
|
||||
// radial distortion: k1, k2
|
||||
// tangential distortion: p1, p2
|
||||
out << "Distortion Parameters" << std::endl;
|
||||
out << " k1 " << params.m_k1 << std::endl
|
||||
<< " k2 " << params.m_k2 << std::endl
|
||||
<< " p1 " << params.m_p1 << std::endl
|
||||
<< " p2 " << params.m_p2 << std::endl;
|
||||
|
||||
// projection: fx, fy, cx, cy
|
||||
out << "Projection Parameters" << std::endl;
|
||||
out << " fx " << params.m_fx << std::endl
|
||||
<< " fy " << params.m_fy << std::endl
|
||||
<< " cx " << params.m_cx << std::endl
|
||||
<< " cy " << params.m_cy << std::endl;
|
||||
|
||||
return out;
|
||||
}
|
||||
|
||||
PinholeCamera::PinholeCamera()
|
||||
: m_inv_K11(1.0)
|
||||
, m_inv_K13(0.0)
|
||||
, m_inv_K22(1.0)
|
||||
, m_inv_K23(0.0)
|
||||
, m_noDistortion(true)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
PinholeCamera::PinholeCamera(const std::string& cameraName,
|
||||
int imageWidth, int imageHeight,
|
||||
double k1, double k2, double p1, double p2,
|
||||
double fx, double fy, double cx, double cy)
|
||||
: mParameters(cameraName, imageWidth, imageHeight,
|
||||
k1, k2, p1, p2, fx, fy, cx, cy)
|
||||
{
|
||||
if ((mParameters.k1() == 0.0) &&
|
||||
(mParameters.k2() == 0.0) &&
|
||||
(mParameters.p1() == 0.0) &&
|
||||
(mParameters.p2() == 0.0))
|
||||
{
|
||||
m_noDistortion = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
m_noDistortion = false;
|
||||
}
|
||||
|
||||
// Inverse camera projection matrix parameters
|
||||
m_inv_K11 = 1.0 / mParameters.fx();
|
||||
m_inv_K13 = -mParameters.cx() / mParameters.fx();
|
||||
m_inv_K22 = 1.0 / mParameters.fy();
|
||||
m_inv_K23 = -mParameters.cy() / mParameters.fy();
|
||||
}
|
||||
|
||||
PinholeCamera::PinholeCamera(const PinholeCamera::Parameters& params)
|
||||
: mParameters(params)
|
||||
{
|
||||
if ((mParameters.k1() == 0.0) &&
|
||||
(mParameters.k2() == 0.0) &&
|
||||
(mParameters.p1() == 0.0) &&
|
||||
(mParameters.p2() == 0.0))
|
||||
{
|
||||
m_noDistortion = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
m_noDistortion = false;
|
||||
}
|
||||
|
||||
// Inverse camera projection matrix parameters
|
||||
m_inv_K11 = 1.0 / mParameters.fx();
|
||||
m_inv_K13 = -mParameters.cx() / mParameters.fx();
|
||||
m_inv_K22 = 1.0 / mParameters.fy();
|
||||
m_inv_K23 = -mParameters.cy() / mParameters.fy();
|
||||
}
|
||||
|
||||
Camera::ModelType
|
||||
PinholeCamera::modelType(void) const
|
||||
{
|
||||
return mParameters.modelType();
|
||||
}
|
||||
|
||||
const std::string&
|
||||
PinholeCamera::cameraName(void) const
|
||||
{
|
||||
return mParameters.cameraName();
|
||||
}
|
||||
|
||||
int
|
||||
PinholeCamera::imageWidth(void) const
|
||||
{
|
||||
return mParameters.imageWidth();
|
||||
}
|
||||
|
||||
int
|
||||
PinholeCamera::imageHeight(void) const
|
||||
{
|
||||
return mParameters.imageHeight();
|
||||
}
|
||||
|
||||
void
|
||||
PinholeCamera::estimateIntrinsics(const cv::Size& boardSize,
|
||||
const std::vector< std::vector<cv::Point3f> >& objectPoints,
|
||||
const std::vector< std::vector<cv::Point2f> >& imagePoints)
|
||||
{
|
||||
// Z. Zhang, A Flexible New Technique for Camera Calibration, PAMI 2000
|
||||
|
||||
Parameters params = getParameters();
|
||||
|
||||
params.k1() = 0.0;
|
||||
params.k2() = 0.0;
|
||||
params.p1() = 0.0;
|
||||
params.p2() = 0.0;
|
||||
|
||||
double cx = params.imageWidth() / 2.0;
|
||||
double cy = params.imageHeight() / 2.0;
|
||||
params.cx() = cx;
|
||||
params.cy() = cy;
|
||||
|
||||
size_t nImages = imagePoints.size();
|
||||
|
||||
cv::Mat A(nImages * 2, 2, CV_64F);
|
||||
cv::Mat b(nImages * 2, 1, CV_64F);
|
||||
|
||||
for (size_t i = 0; i < nImages; ++i)
|
||||
{
|
||||
const std::vector<cv::Point3f>& oPoints = objectPoints.at(i);
|
||||
|
||||
std::vector<cv::Point2f> M(oPoints.size());
|
||||
for (size_t j = 0; j < M.size(); ++j)
|
||||
{
|
||||
M.at(j) = cv::Point2f(oPoints.at(j).x, oPoints.at(j).y);
|
||||
}
|
||||
|
||||
cv::Mat H = cv::findHomography(M, imagePoints.at(i));
|
||||
|
||||
H.at<double>(0,0) -= H.at<double>(2,0) * cx;
|
||||
H.at<double>(0,1) -= H.at<double>(2,1) * cx;
|
||||
H.at<double>(0,2) -= H.at<double>(2,2) * cx;
|
||||
H.at<double>(1,0) -= H.at<double>(2,0) * cy;
|
||||
H.at<double>(1,1) -= H.at<double>(2,1) * cy;
|
||||
H.at<double>(1,2) -= H.at<double>(2,2) * cy;
|
||||
|
||||
double h[3], v[3], d1[3], d2[3];
|
||||
double n[4] = {0,0,0,0};
|
||||
|
||||
for (int j = 0; j < 3; ++j)
|
||||
{
|
||||
double t0 = H.at<double>(j,0);
|
||||
double t1 = H.at<double>(j,1);
|
||||
h[j] = t0; v[j] = t1;
|
||||
d1[j] = (t0 + t1) * 0.5;
|
||||
d2[j] = (t0 - t1) * 0.5;
|
||||
n[0] += t0 * t0; n[1] += t1 * t1;
|
||||
n[2] += d1[j] * d1[j]; n[3] += d2[j] * d2[j];
|
||||
}
|
||||
|
||||
for (int j = 0; j < 4; ++j)
|
||||
{
|
||||
n[j] = 1.0 / sqrt(n[j]);
|
||||
}
|
||||
|
||||
for (int j = 0; j < 3; ++j)
|
||||
{
|
||||
h[j] *= n[0]; v[j] *= n[1];
|
||||
d1[j] *= n[2]; d2[j] *= n[3];
|
||||
}
|
||||
|
||||
A.at<double>(i * 2, 0) = h[0] * v[0];
|
||||
A.at<double>(i * 2, 1) = h[1] * v[1];
|
||||
A.at<double>(i * 2 + 1, 0) = d1[0] * d2[0];
|
||||
A.at<double>(i * 2 + 1, 1) = d1[1] * d2[1];
|
||||
b.at<double>(i * 2, 0) = -h[2] * v[2];
|
||||
b.at<double>(i * 2 + 1, 0) = -d1[2] * d2[2];
|
||||
}
|
||||
|
||||
cv::Mat f(2, 1, CV_64F);
|
||||
cv::solve(A, b, f, cv::DECOMP_NORMAL | cv::DECOMP_LU);
|
||||
|
||||
params.fx() = sqrt(fabs(1.0 / f.at<double>(0)));
|
||||
params.fy() = sqrt(fabs(1.0 / f.at<double>(1)));
|
||||
|
||||
setParameters(params);
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Lifts a point from the image plane to the unit sphere
|
||||
*
|
||||
* \param p image coordinates
|
||||
* \param P coordinates of the point on the sphere
|
||||
*/
|
||||
void
|
||||
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<double,2,3>& J) const
|
||||
{
|
||||
Eigen::Vector2d p_u, p_d;
|
||||
double norm, inv_denom;
|
||||
double dxdmx, dydmx, dxdmy, dydmy;
|
||||
|
||||
norm = P.norm();
|
||||
// Project points to the normalised plane
|
||||
inv_denom = 1.0 / P(2);
|
||||
p_u << inv_denom * P(0), inv_denom * P(1);
|
||||
|
||||
// Calculate jacobian
|
||||
double dudx = inv_denom;
|
||||
double dvdx = 0.0;
|
||||
double dudy = 0.0;
|
||||
double dvdy = inv_denom;
|
||||
inv_denom = - inv_denom * inv_denom;
|
||||
double dudz = P(0) * inv_denom;
|
||||
double dvdz = P(1) * inv_denom;
|
||||
|
||||
if (m_noDistortion)
|
||||
{
|
||||
p_d = p_u;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Apply distortion
|
||||
Eigen::Vector2d d_u;
|
||||
distortion(p_u, d_u);
|
||||
p_d = p_u + d_u;
|
||||
}
|
||||
|
||||
double fx = mParameters.fx();
|
||||
double fy = mParameters.fy();
|
||||
|
||||
// Make the product of the jacobians
|
||||
// and add projection matrix jacobian
|
||||
inv_denom = fx * (dudx * dxdmx + dvdx * dxdmy); // reuse
|
||||
dvdx = fy * (dudx * dydmx + dvdx * dydmy);
|
||||
dudx = inv_denom;
|
||||
|
||||
inv_denom = fx * (dudy * dxdmx + dvdy * dxdmy); // reuse
|
||||
dvdy = fy * (dudy * dydmx + dvdy * dydmy);
|
||||
dudy = inv_denom;
|
||||
|
||||
inv_denom = fx * (dudz * dxdmx + dvdz * dxdmy); // reuse
|
||||
dvdz = fy * (dudz * dydmx + dvdz * dydmy);
|
||||
dudz = inv_denom;
|
||||
|
||||
// Apply generalised projection matrix
|
||||
p << fx * p_d(0) + mParameters.cx(),
|
||||
fy * p_d(1) + mParameters.cy();
|
||||
|
||||
J << dudx, dudy, dudz,
|
||||
dvdx, dvdy, dvdz;
|
||||
}
|
||||
#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<float>(v,u) = p(0);
|
||||
mapY.at<float>(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<double>(), p);
|
||||
|
||||
mapX.at<float>(v,u) = p(0);
|
||||
mapY.at<float>(v,u) = p(1);
|
||||
}
|
||||
}
|
||||
|
||||
cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false);
|
||||
|
||||
cv::Mat K_rect_cv;
|
||||
cv::eigen2cv(K_rect, K_rect_cv);
|
||||
return K_rect_cv;
|
||||
}
|
||||
|
||||
int
|
||||
PinholeCamera::parameterCount(void) const
|
||||
{
|
||||
return 8;
|
||||
}
|
||||
|
||||
const PinholeCamera::Parameters&
|
||||
PinholeCamera::getParameters(void) const
|
||||
{
|
||||
return mParameters;
|
||||
}
|
||||
|
||||
void
|
||||
PinholeCamera::setParameters(const PinholeCamera::Parameters& parameters)
|
||||
{
|
||||
mParameters = parameters;
|
||||
|
||||
if ((mParameters.k1() == 0.0) &&
|
||||
(mParameters.k2() == 0.0) &&
|
||||
(mParameters.p1() == 0.0) &&
|
||||
(mParameters.p2() == 0.0))
|
||||
{
|
||||
m_noDistortion = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
m_noDistortion = false;
|
||||
}
|
||||
|
||||
m_inv_K11 = 1.0 / mParameters.fx();
|
||||
m_inv_K13 = -mParameters.cx() / mParameters.fx();
|
||||
m_inv_K22 = 1.0 / mParameters.fy();
|
||||
m_inv_K23 = -mParameters.cy() / mParameters.fy();
|
||||
}
|
||||
|
||||
void
|
||||
PinholeCamera::readParameters(const std::vector<double>& parameterVec)
|
||||
{
|
||||
if ((int)parameterVec.size() != parameterCount())
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
Parameters params = getParameters();
|
||||
|
||||
params.k1() = parameterVec.at(0);
|
||||
params.k2() = parameterVec.at(1);
|
||||
params.p1() = parameterVec.at(2);
|
||||
params.p2() = parameterVec.at(3);
|
||||
params.fx() = parameterVec.at(4);
|
||||
params.fy() = parameterVec.at(5);
|
||||
params.cx() = parameterVec.at(6);
|
||||
params.cy() = parameterVec.at(7);
|
||||
|
||||
setParameters(params);
|
||||
}
|
||||
|
||||
void
|
||||
PinholeCamera::writeParameters(std::vector<double>& parameterVec) const
|
||||
{
|
||||
parameterVec.resize(parameterCount());
|
||||
parameterVec.at(0) = mParameters.k1();
|
||||
parameterVec.at(1) = mParameters.k2();
|
||||
parameterVec.at(2) = mParameters.p1();
|
||||
parameterVec.at(3) = mParameters.p2();
|
||||
parameterVec.at(4) = mParameters.fx();
|
||||
parameterVec.at(5) = mParameters.fy();
|
||||
parameterVec.at(6) = mParameters.cx();
|
||||
parameterVec.at(7) = mParameters.cy();
|
||||
}
|
||||
|
||||
void
|
||||
PinholeCamera::writeParametersToYamlFile(const std::string& filename) const
|
||||
{
|
||||
mParameters.writeToYamlFile(filename);
|
||||
}
|
||||
|
||||
std::string
|
||||
PinholeCamera::parametersToString(void) const
|
||||
{
|
||||
std::ostringstream oss;
|
||||
oss << mParameters;
|
||||
|
||||
return oss.str();
|
||||
}
|
||||
|
||||
}
|
||||
1000
camera_models/src/camera_models/PinholeFullCamera.cc
Normal file
1000
camera_models/src/camera_models/PinholeFullCamera.cc
Normal file
File diff suppressed because it is too large
Load Diff
833
camera_models/src/camera_models/ScaramuzzaCamera.cc
Normal file
833
camera_models/src/camera_models/ScaramuzzaCamera.cc
Normal file
@@ -0,0 +1,833 @@
|
||||
#include "camodocal/camera_models/ScaramuzzaCamera.h"
|
||||
|
||||
#include <cmath>
|
||||
#include <cstdio>
|
||||
#include <eigen3/Eigen/Dense>
|
||||
#include <eigen3/Eigen/SVD>
|
||||
#include <iomanip>
|
||||
#include <iostream>
|
||||
#include <opencv2/calib3d/calib3d.hpp>
|
||||
#include <opencv2/core/eigen.hpp>
|
||||
#include <opencv2/imgproc/imgproc.hpp>
|
||||
#include <boost/lexical_cast.hpp>
|
||||
#include <boost/algorithm/string.hpp>
|
||||
|
||||
#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<Eigen::MatrixXd> 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<int>(fs["image_width"]);
|
||||
m_imageHeight = static_cast<int>(fs["image_height"]);
|
||||
|
||||
cv::FileNode n = fs["poly_parameters"];
|
||||
for(int i=0; i < SCARAMUZZA_POLY_SIZE; i++)
|
||||
m_poly[i] = static_cast<double>(n[std::string("p") + boost::lexical_cast<std::string>(i)]);
|
||||
|
||||
n = fs["inv_poly_parameters"];
|
||||
for(int i=0; i < SCARAMUZZA_INV_POLY_SIZE; i++)
|
||||
m_inv_poly[i] = static_cast<double>(n[std::string("p") + boost::lexical_cast<std::string>(i)]);
|
||||
|
||||
n = fs["affine_parameters"];
|
||||
m_C = static_cast<double>(n["ac"]);
|
||||
m_D = static_cast<double>(n["ad"]);
|
||||
m_E = static_cast<double>(n["ae"]);
|
||||
|
||||
m_center_x = static_cast<double>(n["cx"]);
|
||||
m_center_y = static_cast<double>(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<std::string>(i) << m_poly[i];
|
||||
fs << "}";
|
||||
|
||||
fs << "inv_poly_parameters";
|
||||
fs << "{";
|
||||
for(int i=0; i < SCARAMUZZA_INV_POLY_SIZE; i++)
|
||||
fs << std::string("p") + boost::lexical_cast<std::string>(i) << m_inv_poly[i];
|
||||
fs << "}";
|
||||
|
||||
fs << "affine_parameters";
|
||||
fs << "{" << "ac" << m_C
|
||||
<< "ad" << m_D
|
||||
<< "ae" << m_E
|
||||
<< "cx" << m_center_x
|
||||
<< "cy" << m_center_y << "}";
|
||||
|
||||
fs.release();
|
||||
}
|
||||
|
||||
OCAMCamera::Parameters&
|
||||
OCAMCamera::Parameters::operator=(const OCAMCamera::Parameters& other)
|
||||
{
|
||||
if (this != &other)
|
||||
{
|
||||
m_modelType = other.m_modelType;
|
||||
m_cameraName = other.m_cameraName;
|
||||
m_imageWidth = other.m_imageWidth;
|
||||
m_imageHeight = other.m_imageHeight;
|
||||
m_C = other.m_C;
|
||||
m_D = other.m_D;
|
||||
m_E = other.m_E;
|
||||
m_center_x = other.m_center_x;
|
||||
m_center_y = other.m_center_y;
|
||||
|
||||
memcpy(m_poly, other.m_poly, sizeof(double) * SCARAMUZZA_POLY_SIZE);
|
||||
memcpy(m_inv_poly, other.m_inv_poly, sizeof(double) * SCARAMUZZA_INV_POLY_SIZE);
|
||||
}
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
std::ostream&
|
||||
operator<< (std::ostream& out, const OCAMCamera::Parameters& params)
|
||||
{
|
||||
out << "Camera Parameters:" << std::endl;
|
||||
out << " model_type " << "scaramuzza" << std::endl;
|
||||
out << " camera_name " << params.m_cameraName << std::endl;
|
||||
out << " image_width " << params.m_imageWidth << std::endl;
|
||||
out << " image_height " << params.m_imageHeight << std::endl;
|
||||
|
||||
out << std::fixed << std::setprecision(10);
|
||||
|
||||
out << "Poly Parameters" << std::endl;
|
||||
for(int i=0; i < SCARAMUZZA_POLY_SIZE; i++)
|
||||
out << std::string("p") + boost::lexical_cast<std::string>(i) << ": " << params.m_poly[i] << std::endl;
|
||||
|
||||
out << "Inverse Poly Parameters" << std::endl;
|
||||
for(int i=0; i < SCARAMUZZA_INV_POLY_SIZE; i++)
|
||||
out << std::string("p") + boost::lexical_cast<std::string>(i) << ": " << params.m_inv_poly[i] << std::endl;
|
||||
|
||||
out << "Affine Parameters" << std::endl;
|
||||
out << " ac " << params.m_C << std::endl
|
||||
<< " ad " << params.m_D << std::endl
|
||||
<< " ae " << params.m_E << std::endl;
|
||||
out << " cx " << params.m_center_x << std::endl
|
||||
<< " cy " << params.m_center_y << std::endl;
|
||||
|
||||
return out;
|
||||
}
|
||||
|
||||
OCAMCamera::OCAMCamera()
|
||||
: m_inv_scale(0.0)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
OCAMCamera::OCAMCamera(const OCAMCamera::Parameters& params)
|
||||
: mParameters(params)
|
||||
{
|
||||
m_inv_scale = 1.0 / (params.C() - params.D() * params.E());
|
||||
}
|
||||
|
||||
Camera::ModelType
|
||||
OCAMCamera::modelType(void) const
|
||||
{
|
||||
return mParameters.modelType();
|
||||
}
|
||||
|
||||
const std::string&
|
||||
OCAMCamera::cameraName(void) const
|
||||
{
|
||||
return mParameters.cameraName();
|
||||
}
|
||||
|
||||
int
|
||||
OCAMCamera::imageWidth(void) const
|
||||
{
|
||||
return mParameters.imageWidth();
|
||||
}
|
||||
|
||||
int
|
||||
OCAMCamera::imageHeight(void) const
|
||||
{
|
||||
return mParameters.imageHeight();
|
||||
}
|
||||
|
||||
void
|
||||
OCAMCamera::estimateIntrinsics(const cv::Size& boardSize,
|
||||
const std::vector< std::vector<cv::Point3f> >& objectPoints,
|
||||
const std::vector< std::vector<cv::Point2f> >& imagePoints)
|
||||
{
|
||||
// std::cout << "OCAMCamera::estimateIntrinsics - NOT IMPLEMENTED" << std::endl;
|
||||
// throw std::string("OCAMCamera::estimateIntrinsics - NOT IMPLEMENTED");
|
||||
|
||||
// Reference: Page 30 of
|
||||
// " Scaramuzza, D. Omnidirectional Vision: from Calibration to Robot Motion Estimation, ETH Zurich. Thesis no. 17635."
|
||||
// http://e-collection.library.ethz.ch/eserv/eth:30301/eth-30301-02.pdf
|
||||
// Matlab code: calibrate.m
|
||||
|
||||
// First, estimate every image's extrinsics parameters
|
||||
std::vector< Eigen::Matrix3d > RList;
|
||||
std::vector< Eigen::Vector3d > TList;
|
||||
|
||||
RList.reserve(imagePoints.size());
|
||||
TList.reserve(imagePoints.size());
|
||||
|
||||
// i-th image
|
||||
for (size_t image_index = 0; image_index < imagePoints.size(); ++image_index)
|
||||
{
|
||||
const std::vector<cv::Point3f>& objPts = objectPoints.at(image_index);
|
||||
const std::vector<cv::Point2f>& imgPts = imagePoints.at(image_index);
|
||||
|
||||
assert(objPts.size() == imgPts.size());
|
||||
assert(objPts.size() == static_cast<unsigned int>(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<Eigen::MatrixXd> 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<double> 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<double> sr32_values;
|
||||
std::vector<double> sr31_values;
|
||||
for (auto sr32_squared : sr32_squared_values) {
|
||||
for(int sign = -1; sign <= 1; sign += 2) {
|
||||
const double sr32 = static_cast<double>(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<Eigen::Matrix3d> 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<Eigen::Matrix3d> 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<unsigned int>(A_mat.rows()));
|
||||
|
||||
// pseudo-inverse for polynomial parameters and all t3s
|
||||
{
|
||||
Eigen::JacobiSVD<Eigen::MatrixXd> 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<unsigned int>(A_mat.rows()));
|
||||
|
||||
Eigen::Matrix<double, SCARAMUZZA_POLY_SIZE, 1> poly_coeff;
|
||||
// pseudo-inverse for polynomial parameters and all t3s
|
||||
{
|
||||
Eigen::JacobiSVD<Eigen::MatrixXd> 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<unsigned int>(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<double> rou_vec;
|
||||
std::vector<double> 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<float>(v,u) = p(0);
|
||||
mapY.at<float>(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<double>(), p);
|
||||
|
||||
mapX.at<float>(v,u) = p(0);
|
||||
mapY.at<float>(v,u) = p(1);
|
||||
}
|
||||
}
|
||||
|
||||
cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false);
|
||||
|
||||
cv::Mat K_rect_cv;
|
||||
cv::eigen2cv(K_rect, K_rect_cv);
|
||||
return K_rect_cv;
|
||||
}
|
||||
|
||||
int
|
||||
OCAMCamera::parameterCount(void) const
|
||||
{
|
||||
return SCARAMUZZA_CAMERA_NUM_PARAMS;
|
||||
}
|
||||
|
||||
const OCAMCamera::Parameters&
|
||||
OCAMCamera::getParameters(void) const
|
||||
{
|
||||
return mParameters;
|
||||
}
|
||||
|
||||
void
|
||||
OCAMCamera::setParameters(const OCAMCamera::Parameters& parameters)
|
||||
{
|
||||
mParameters = parameters;
|
||||
|
||||
m_inv_scale = 1.0 / (parameters.C() - parameters.D() * parameters.E());
|
||||
}
|
||||
|
||||
void
|
||||
OCAMCamera::readParameters(const std::vector<double>& parameterVec)
|
||||
{
|
||||
if ((int)parameterVec.size() != parameterCount())
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
Parameters params = getParameters();
|
||||
|
||||
params.C() = parameterVec.at(0);
|
||||
params.D() = parameterVec.at(1);
|
||||
params.E() = parameterVec.at(2);
|
||||
params.center_x() = parameterVec.at(3);
|
||||
params.center_y() = parameterVec.at(4);
|
||||
for (int i=0; i < SCARAMUZZA_POLY_SIZE; i++)
|
||||
params.poly(i) = parameterVec.at(5+i);
|
||||
for (int i=0; i < SCARAMUZZA_INV_POLY_SIZE; i++)
|
||||
params.inv_poly(i) = parameterVec.at(5 + SCARAMUZZA_POLY_SIZE + i);
|
||||
|
||||
setParameters(params);
|
||||
}
|
||||
|
||||
void
|
||||
OCAMCamera::writeParameters(std::vector<double>& parameterVec) const
|
||||
{
|
||||
parameterVec.resize(parameterCount());
|
||||
parameterVec.at(0) = mParameters.C();
|
||||
parameterVec.at(1) = mParameters.D();
|
||||
parameterVec.at(2) = mParameters.E();
|
||||
parameterVec.at(3) = mParameters.center_x();
|
||||
parameterVec.at(4) = mParameters.center_y();
|
||||
for (int i=0; i < SCARAMUZZA_POLY_SIZE; i++)
|
||||
parameterVec.at(5+i) = mParameters.poly(i);
|
||||
for (int i=0; i < SCARAMUZZA_INV_POLY_SIZE; i++)
|
||||
parameterVec.at(5 + SCARAMUZZA_POLY_SIZE + i) = mParameters.inv_poly(i);
|
||||
}
|
||||
|
||||
void
|
||||
OCAMCamera::writeParametersToYamlFile(const std::string& filename) const
|
||||
{
|
||||
mParameters.writeToYamlFile(filename);
|
||||
}
|
||||
|
||||
std::string
|
||||
OCAMCamera::parametersToString(void) const
|
||||
{
|
||||
std::ostringstream oss;
|
||||
oss << mParameters;
|
||||
|
||||
return oss.str();
|
||||
}
|
||||
|
||||
}
|
||||
1982
camera_models/src/chessboard/Chessboard.cc
Normal file
1982
camera_models/src/chessboard/Chessboard.cc
Normal file
File diff suppressed because it is too large
Load Diff
46
camera_models/src/gpl/EigenQuaternionParameterization.cc
Normal file
46
camera_models/src/gpl/EigenQuaternionParameterization.cc
Normal file
@@ -0,0 +1,46 @@
|
||||
#include "camodocal/gpl/EigenQuaternionParameterization.h"
|
||||
|
||||
#include <cmath>
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
}
|
||||
928
camera_models/src/gpl/gpl.cc
Normal file
928
camera_models/src/gpl/gpl.cc
Normal file
@@ -0,0 +1,928 @@
|
||||
#include "camodocal/gpl/gpl.h"
|
||||
|
||||
#include <set>
|
||||
#ifdef _WIN32
|
||||
#include <winsock.h>
|
||||
#else
|
||||
#include <time.h>
|
||||
#endif
|
||||
|
||||
|
||||
// source: https://stackoverflow.com/questions/5167269/clock-gettime-alternative-in-mac-os-x
|
||||
#ifdef __APPLE__
|
||||
#include <mach/mach_time.h>
|
||||
#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 <sys/timeb.h>
|
||||
#include <sys/types.h>
|
||||
#include <winsock.h>
|
||||
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<double>(tp.tv_sec) +
|
||||
static_cast<double>(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<float>(i);
|
||||
unsigned char* pixel = imgColoredDepth.ptr<unsigned char>(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<cv::Point2i> 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<cv::Point2i> 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<cv::Point2i> bresCircle(int x0, int y0, int r)
|
||||
{
|
||||
// Bresenham's circle algorithm
|
||||
// Find cells intersected by circle with center (x0,y0) and radius r
|
||||
|
||||
std::vector< std::vector<bool> > 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<cv::Point2i> line;
|
||||
|
||||
line = bresLine(x0, y0 - r, x0, y0 + r);
|
||||
for (std::vector<cv::Point2i>::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<cv::Point2i>::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<cv::Point2i>::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<cv::Point2i>::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<cv::Point2i>::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<cv::Point2i>::iterator it = line.begin(); it != line.end(); ++it)
|
||||
{
|
||||
mask[it->x - x0 + r][it->y - y0 + r] = true;
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<cv::Point2i> 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<cv::Point2d>& points,
|
||||
double& centerX, double& centerY, double& radius)
|
||||
{
|
||||
// D. Umbach, and K. Jones, A Few Methods for Fitting Circles to Data,
|
||||
// IEEE Transactions on Instrumentation and Measurement, 2000
|
||||
// We use the modified least squares method.
|
||||
double sum_x = 0.0;
|
||||
double sum_y = 0.0;
|
||||
double sum_xx = 0.0;
|
||||
double sum_xy = 0.0;
|
||||
double sum_yy = 0.0;
|
||||
double sum_xxx = 0.0;
|
||||
double sum_xxy = 0.0;
|
||||
double sum_xyy = 0.0;
|
||||
double sum_yyy = 0.0;
|
||||
|
||||
int n = points.size();
|
||||
for (int i = 0; i < n; ++i)
|
||||
{
|
||||
double x = points.at(i).x;
|
||||
double y = points.at(i).y;
|
||||
|
||||
sum_x += x;
|
||||
sum_y += y;
|
||||
sum_xx += x * x;
|
||||
sum_xy += x * y;
|
||||
sum_yy += y * y;
|
||||
sum_xxx += x * x * x;
|
||||
sum_xxy += x * x * y;
|
||||
sum_xyy += x * y * y;
|
||||
sum_yyy += y * y * y;
|
||||
}
|
||||
|
||||
double A = n * sum_xx - square(sum_x);
|
||||
double B = n * sum_xy - sum_x * sum_y;
|
||||
double C = n * sum_yy - square(sum_y);
|
||||
double D = 0.5 * (n * sum_xyy - sum_x * sum_yy + n * sum_xxx - sum_x * sum_xx);
|
||||
double E = 0.5 * (n * sum_xxy - sum_y * sum_xx + n * sum_yyy - sum_y * sum_yy);
|
||||
|
||||
centerX = (D * C - B * E) / (A * C - square(B));
|
||||
centerY = (A * E - B * D) / (A * C - square(B));
|
||||
|
||||
double sum_r = 0.0;
|
||||
for (int i = 0; i < n; ++i)
|
||||
{
|
||||
double x = points.at(i).x;
|
||||
double y = points.at(i).y;
|
||||
|
||||
sum_r += hypot(x - centerX, y - centerY);
|
||||
}
|
||||
|
||||
radius = sum_r / n;
|
||||
}
|
||||
|
||||
std::vector<cv::Point2d>
|
||||
intersectCircles(double x1, double y1, double r1,
|
||||
double x2, double y2, double r2)
|
||||
{
|
||||
std::vector<cv::Point2d> 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<int>((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<double>((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<int>(ZoneLetter) - static_cast<int>('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<long int>::max())
|
||||
{
|
||||
return std::numeric_limits<long int>::max();
|
||||
}
|
||||
else
|
||||
{
|
||||
return d;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
uint64_t d = t1 - t2;
|
||||
|
||||
if (d > std::numeric_limits<long int>::max())
|
||||
{
|
||||
return std::numeric_limits<long int>::min();
|
||||
}
|
||||
else
|
||||
{
|
||||
return - static_cast<long int>(d);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
284
camera_models/src/intrinsic_calib.cc
Normal file
284
camera_models/src/intrinsic_calib.cc
Normal file
@@ -0,0 +1,284 @@
|
||||
#include <algorithm>
|
||||
#include <boost/algorithm/string.hpp>
|
||||
#include <boost/filesystem.hpp>
|
||||
#include <boost/program_options.hpp>
|
||||
#include <iomanip>
|
||||
#include <iostream>
|
||||
#include <opencv2/core/core.hpp>
|
||||
#include <opencv2/highgui/highgui.hpp>
|
||||
#include <opencv2/imgproc/imgproc.hpp>
|
||||
|
||||
#include "camodocal/calib/CameraCalibration.h"
|
||||
#include "camodocal/chessboard/Chessboard.h"
|
||||
#include "camodocal/gpl/gpl.h"
|
||||
|
||||
int
|
||||
main( int argc, char** argv )
|
||||
{
|
||||
cv::Size boardSize;
|
||||
float squareSize;
|
||||
std::string inputDir;
|
||||
std::string cameraModel;
|
||||
std::string cameraName;
|
||||
std::string prefix;
|
||||
std::string fileExtension;
|
||||
bool useOpenCV;
|
||||
bool viewResults;
|
||||
bool verbose;
|
||||
|
||||
//========= Handling Program options =========
|
||||
boost::program_options::options_description desc( "Allowed options" );
|
||||
desc.add_options( )( "help", "produce help message" )(
|
||||
"width,w",
|
||||
boost::program_options::value< int >( &boardSize.width )->default_value( 8 ),
|
||||
"Number of inner corners on the chessboard pattern in x direction" )(
|
||||
"height,h",
|
||||
boost::program_options::value< int >( &boardSize.height )->default_value( 12 ),
|
||||
"Number of inner corners on the chessboard pattern in y direction" )(
|
||||
"size,s",
|
||||
boost::program_options::value< float >( &squareSize )->default_value( 7.f ),
|
||||
"Size of one square in mm" )( "input,i",
|
||||
boost::program_options::value< std::string >( &inputDir )->default_value( "calibrationdata" ),
|
||||
"Input directory containing chessboard images" )(
|
||||
"prefix,p",
|
||||
boost::program_options::value< std::string >( &prefix )->default_value( "left-" ),
|
||||
"Prefix of images" )( "file-extension,e",
|
||||
boost::program_options::value< std::string >( &fileExtension )->default_value( ".png" ),
|
||||
"File extension of images" )(
|
||||
"camera-model",
|
||||
boost::program_options::value< std::string >( &cameraModel )->default_value( "mei" ),
|
||||
"Camera model: kannala-brandt | mei | pinhole" )(
|
||||
"camera-name",
|
||||
boost::program_options::value< std::string >( &cameraName )->default_value( "camera" ),
|
||||
"Name of camera" )( "opencv",
|
||||
boost::program_options::bool_switch( &useOpenCV )->default_value( true ),
|
||||
"Use OpenCV to detect corners" )(
|
||||
"view-results",
|
||||
boost::program_options::bool_switch( &viewResults )->default_value( false ),
|
||||
"View results" )( "verbose,v",
|
||||
boost::program_options::bool_switch( &verbose )->default_value( true ),
|
||||
"Verbose output" );
|
||||
|
||||
boost::program_options::positional_options_description pdesc;
|
||||
pdesc.add( "input", 1 );
|
||||
|
||||
boost::program_options::variables_map vm;
|
||||
boost::program_options::store( boost::program_options::command_line_parser( argc, argv )
|
||||
.options( desc )
|
||||
.positional( pdesc )
|
||||
.run( ),
|
||||
vm );
|
||||
boost::program_options::notify( vm );
|
||||
|
||||
if ( vm.count( "help" ) )
|
||||
{
|
||||
std::cout << desc << std::endl;
|
||||
return 1;
|
||||
}
|
||||
|
||||
if ( !boost::filesystem::exists( inputDir ) && !boost::filesystem::is_directory( inputDir ) )
|
||||
{
|
||||
std::cerr << "# ERROR: Cannot find input directory " << inputDir << "." << std::endl;
|
||||
return 1;
|
||||
}
|
||||
|
||||
camodocal::Camera::ModelType modelType;
|
||||
if ( boost::iequals( cameraModel, "kannala-brandt" ) )
|
||||
{
|
||||
modelType = camodocal::Camera::KANNALA_BRANDT;
|
||||
}
|
||||
else if ( boost::iequals( cameraModel, "mei" ) )
|
||||
{
|
||||
modelType = camodocal::Camera::MEI;
|
||||
}
|
||||
else if ( boost::iequals( cameraModel, "pinhole" ) )
|
||||
{
|
||||
modelType = camodocal::Camera::PINHOLE;
|
||||
}
|
||||
else if ( boost::iequals( cameraModel, "pinhole_full" ) )
|
||||
{
|
||||
modelType = camodocal::Camera::PINHOLE_FULL;
|
||||
}
|
||||
else if ( boost::iequals( cameraModel, "scaramuzza" ) )
|
||||
{
|
||||
modelType = camodocal::Camera::SCARAMUZZA;
|
||||
}
|
||||
else
|
||||
{
|
||||
std::cerr << "# ERROR: Unknown camera model: " << cameraModel << std::endl;
|
||||
return 1;
|
||||
}
|
||||
|
||||
switch ( modelType )
|
||||
{
|
||||
case camodocal::Camera::KANNALA_BRANDT:
|
||||
std::cout << "# INFO: Camera model: Kannala-Brandt" << std::endl;
|
||||
break;
|
||||
case camodocal::Camera::MEI:
|
||||
std::cout << "# INFO: Camera model: Mei" << std::endl;
|
||||
break;
|
||||
case camodocal::Camera::PINHOLE:
|
||||
std::cout << "# INFO: Camera model: Pinhole" << std::endl;
|
||||
break;
|
||||
case camodocal::Camera::PINHOLE_FULL:
|
||||
std::cout << "# INFO: Camera model: PinholeFull" << std::endl;
|
||||
break;
|
||||
case camodocal::Camera::SCARAMUZZA:
|
||||
std::cout << "# INFO: Camera model: Scaramuzza-Omnidirect" << std::endl;
|
||||
break;
|
||||
}
|
||||
|
||||
// look for images in input directory
|
||||
std::vector< std::string > imageFilenames;
|
||||
boost::filesystem::directory_iterator itr;
|
||||
for ( boost::filesystem::directory_iterator itr( inputDir );
|
||||
itr != boost::filesystem::directory_iterator( );
|
||||
++itr )
|
||||
{
|
||||
if ( !boost::filesystem::is_regular_file( itr->status( ) ) )
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
std::string filename = itr->path( ).filename( ).string( );
|
||||
|
||||
// check if prefix matches
|
||||
if ( !prefix.empty( ) )
|
||||
{
|
||||
if ( filename.compare( 0, prefix.length( ), prefix ) != 0 )
|
||||
{
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
||||
// check if file extension matches
|
||||
if ( filename.compare( filename.length( ) - fileExtension.length( ), fileExtension.length( ), fileExtension )
|
||||
!= 0 )
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
imageFilenames.push_back( itr->path( ).string( ) );
|
||||
|
||||
if ( verbose )
|
||||
{
|
||||
std::cerr << "# INFO: Adding " << imageFilenames.back( ) << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
if ( imageFilenames.empty( ) )
|
||||
{
|
||||
std::cerr << "# ERROR: No chessboard images found." << std::endl;
|
||||
return 1;
|
||||
}
|
||||
|
||||
if ( verbose )
|
||||
{
|
||||
std::cerr << "# INFO: # images: " << imageFilenames.size( ) << std::endl;
|
||||
}
|
||||
|
||||
std::sort( imageFilenames.begin( ), imageFilenames.end( ) );
|
||||
|
||||
cv::Mat image = cv::imread( imageFilenames.front( ), -1 );
|
||||
const cv::Size frameSize = image.size( );
|
||||
|
||||
camodocal::CameraCalibration calibration( modelType, cameraName, frameSize, boardSize, squareSize );
|
||||
calibration.setVerbose( verbose );
|
||||
|
||||
std::vector< bool > chessboardFound( imageFilenames.size( ), false );
|
||||
for ( size_t i = 0; i < imageFilenames.size( ); ++i )
|
||||
{
|
||||
image = cv::imread( imageFilenames.at( i ), -1 );
|
||||
|
||||
camodocal::Chessboard chessboard( boardSize, image );
|
||||
|
||||
chessboard.findCorners( useOpenCV );
|
||||
if ( chessboard.cornersFound( ) )
|
||||
{
|
||||
if ( verbose )
|
||||
{
|
||||
std::cerr << "# INFO: Detected chessboard in image " << i + 1 << ", "
|
||||
<< imageFilenames.at( i ) << std::endl;
|
||||
}
|
||||
|
||||
calibration.addChessboardData( chessboard.getCorners( ) );
|
||||
|
||||
cv::Mat sketch;
|
||||
chessboard.getSketch( ).copyTo( sketch );
|
||||
|
||||
cv::imshow( "Image", sketch );
|
||||
cv::waitKey( 50 );
|
||||
}
|
||||
else if ( verbose )
|
||||
{
|
||||
std::cerr << "# INFO: Did not detect chessboard in image " << i + 1 << std::endl;
|
||||
}
|
||||
chessboardFound.at( i ) = chessboard.cornersFound( );
|
||||
}
|
||||
cv::destroyWindow( "Image" );
|
||||
|
||||
if ( calibration.sampleCount( ) < 10 )
|
||||
{
|
||||
std::cerr << "# ERROR: Insufficient number of detected chessboards." << std::endl;
|
||||
return 1;
|
||||
}
|
||||
|
||||
if ( verbose )
|
||||
{
|
||||
std::cerr << "# INFO: Calibrating..." << std::endl;
|
||||
}
|
||||
|
||||
double startTime = camodocal::timeInSeconds( );
|
||||
|
||||
calibration.calibrate( );
|
||||
calibration.writeParams( cameraName + "_camera_calib.yaml" );
|
||||
calibration.writeChessboardData( cameraName + "_chessboard_data.dat" );
|
||||
|
||||
if ( verbose )
|
||||
{
|
||||
std::cout << "# INFO: Calibration took a total time of " << std::fixed
|
||||
<< std::setprecision( 3 ) << camodocal::timeInSeconds( ) - startTime << " sec.\n";
|
||||
}
|
||||
|
||||
if ( verbose )
|
||||
{
|
||||
std::cerr << "# INFO: Wrote calibration file to "
|
||||
<< cameraName + "_camera_calib.yaml" << std::endl;
|
||||
}
|
||||
|
||||
if ( viewResults )
|
||||
{
|
||||
std::vector< cv::Mat > cbImages;
|
||||
std::vector< std::string > cbImageFilenames;
|
||||
|
||||
for ( size_t i = 0; i < imageFilenames.size( ); ++i )
|
||||
{
|
||||
if ( !chessboardFound.at( i ) )
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
cbImages.push_back( cv::imread( imageFilenames.at( i ), -1 ) );
|
||||
cbImageFilenames.push_back( imageFilenames.at( i ) );
|
||||
}
|
||||
|
||||
// visualize observed and reprojected points
|
||||
calibration.drawResults( cbImages );
|
||||
|
||||
for ( size_t i = 0; i < cbImages.size( ); ++i )
|
||||
{
|
||||
cv::putText( cbImages.at( i ),
|
||||
cbImageFilenames.at( i ),
|
||||
cv::Point( 10, 20 ),
|
||||
cv::FONT_HERSHEY_COMPLEX,
|
||||
0.5,
|
||||
cv::Scalar( 255, 255, 255 ),
|
||||
1,
|
||||
CV_AA );
|
||||
cv::imshow( "Image", cbImages.at( i ) );
|
||||
cv::waitKey( 0 );
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
77
camera_models/src/sparse_graph/Transform.cc
Normal file
77
camera_models/src/sparse_graph/Transform.cc
Normal file
@@ -0,0 +1,77 @@
|
||||
#include <camodocal/sparse_graph/Transform.h>
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user