/*M///////////////////////////////////////////////////////////////////////////////////////
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2015, Baisheng Lai (laibaisheng@gmail.com), Zhejiang University,
// all rights reserved.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of the copyright holders may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/
/**
* This module was accepted as a GSoC 2015 project for OpenCV, authored by
* Baisheng Lai, mentored by Bo Li.
*
* The omnidirectional camera in this module is denoted by the catadioptric
* model. Please refer to Mei's paper for details of the camera model:
*
* C. Mei and P. Rives, "Single view point omnidirectional camera
* calibration from planar grids", in ICRA 2007.
*
* The implementation of the calibration part is based on Li's calibration
* toolbox:
*
* B. Li, L. Heng, K. Kevin and M. Pollefeys, "A Multiple-Camera System
* Calibration Toolbox Using A Feature Descriptor-Based Calibration
* Pattern", in IROS 2013.
*/
#include "precomp.hpp"
#include "opencv2/ccalib/multicalib.hpp"
#include "opencv2/core.hpp"
#include <string>
#include <vector>
#include <queue>
#include <iostream>
namespace cv { namespace multicalib {
MultiCameraCalibration::MultiCameraCalibration(int cameraType, int nCameras, const std::string& fileName,
float patternWidth, float patternHeight, int verbose, int showExtration, int nMiniMatches, int flags, TermCriteria criteria,
Ptr<FeatureDetector> detector, Ptr<DescriptorExtractor> descriptor,
Ptr<DescriptorMatcher> matcher)
{
_camType = cameraType;
_nCamera = nCameras;
_flags = flags;
_nMiniMatches = nMiniMatches;
_filename = fileName;
_patternWidth = patternWidth;
_patternHeight = patternHeight;
_criteria = criteria;
_showExtraction = showExtration;
_objectPointsForEachCamera.resize(_nCamera);
_imagePointsForEachCamera.resize(_nCamera);
_cameraMatrix.resize(_nCamera);
_distortCoeffs.resize(_nCamera);
_xi.resize(_nCamera);
_omEachCamera.resize(_nCamera);
_tEachCamera.resize(_nCamera);
_detector = detector;
_descriptor = descriptor;
_matcher = matcher;
_verbose = verbose;
for (int i = 0; i < _nCamera; ++i)
{
_vertexList.push_back(vertex());
}
}
double MultiCameraCalibration::run()
{
loadImages();
initialize();
double error = optimizeExtrinsics();
return error;
}
std::vector<std::string> MultiCameraCalibration::readStringList()
{
std::vector<std::string> l;
l.resize(0);
FileStorage fs(_filename, FileStorage::READ);
FileNode n = fs.getFirstTopLevelNode();
FileNodeIterator it = n.begin(), it_end = n.end();
for( ; it != it_end; ++it )
l.push_back((std::string)*it);
return l;
}
void MultiCameraCalibration::loadImages()
{
std::vector<std::string> file_list;
file_list = readStringList();
Ptr<FeatureDetector> detector = _detector;
Ptr<DescriptorExtractor> descriptor = _descriptor;
Ptr<DescriptorMatcher> matcher = _matcher;
randpattern::RandomPatternCornerFinder finder(_patternWidth, _patternHeight, _nMiniMatches, CV_32F, _verbose, this->_showExtraction, detector, descriptor, matcher);
Mat pattern = cv::imread(file_list[0]);
finder.loadPattern(pattern);
std::vector<std::vector<std::string> > filesEachCameraFull(_nCamera);
std::vector<std::vector<int> > timestampFull(_nCamera);
std::vector<std::vector<int> > timestampAvailable(_nCamera);
for (int i = 1; i < (int)file_list.size(); ++i)
{
int cameraVertex, timestamp;
std::string filename = file_list[i].substr(0, file_list[i].rfind('.'));
size_t spritPosition1 = filename.rfind('/');
size_t spritPosition2 = filename.rfind('\\');
if (spritPosition1!=std::string::npos)
{
filename = filename.substr(spritPosition1+1, filename.size() - 1);
}
else if(spritPosition2!= std::string::npos)
{
filename = filename.substr(spritPosition2+1, filename.size() - 1);
}
sscanf(filename.c_str(), "%d-%d", &cameraVertex, ×tamp);
filesEachCameraFull[cameraVertex].push_back(file_list[i]);
timestampFull[cameraVertex].push_back(timestamp);
}
// calibrate each camera individually
for (int camera = 0; camera < _nCamera; ++camera)
{
Mat image, cameraMatrix, distortCoeffs;
// find image and object points
for (int imgIdx = 0; imgIdx < (int)filesEachCameraFull[camera].size(); ++imgIdx)
{
image = imread(filesEachCameraFull[camera][imgIdx], IMREAD_GRAYSCALE);
if (!image.empty() && _verbose)
{
std::cout << "open image " << filesEachCameraFull[camera][imgIdx] << " successfully" << std::endl;
}
else if (image.empty() && _verbose)
{
std::cout << "open image" << filesEachCameraFull[camera][imgIdx] << " failed" << std::endl;
}
std::vector<Mat> imgObj = finder.computeObjectImagePointsForSingle(image);
if ((int)imgObj[0].total() > _nMiniMatches)
{
_imagePointsForEachCamera[camera].push_back(imgObj[0]);
_objectPointsForEachCamera[camera].push_back(imgObj[1]);
timestampAvailable[camera].push_back(timestampFull[camera][imgIdx]);
}
else if ((int)imgObj[0].total() <= _nMiniMatches && _verbose)
{
std::cout << "image " << filesEachCameraFull[camera][imgIdx] <<" has too few matched points "<< std::endl;
}
}
// calibrate
Mat idx;
double rms = 0.0;
if (_camType == PINHOLE)
{
rms = cv::calibrateCamera(_objectPointsForEachCamera[camera], _imagePointsForEachCamera[camera],
image.size(), _cameraMatrix[camera], _distortCoeffs[camera], _omEachCamera[camera],
_tEachCamera[camera],_flags);
idx = Mat(1, (int)_omEachCamera[camera].size(), CV_32S);
for (int i = 0; i < (int)idx.total(); ++i)
{
idx.at<int>(i) = i;
}
}
//else if (_camType == FISHEYE)
//{
// rms = cv::fisheye::calibrate(_objectPointsForEachCamera[camera], _imagePointsForEachCamera[camera],
// image.size(), _cameraMatrix[camera], _distortCoeffs[camera], _omEachCamera[camera],
// _tEachCamera[camera], _flags);
// idx = Mat(1, (int)_omEachCamera[camera].size(), CV_32S);
// for (int i = 0; i < (int)idx.total(); ++i)
// {
// idx.at<int>(i) = i;
// }
//}
else if (_camType == OMNIDIRECTIONAL)
{
rms = cv::omnidir::calibrate(_objectPointsForEachCamera[camera], _imagePointsForEachCamera[camera],
image.size(), _cameraMatrix[camera], _xi[camera], _distortCoeffs[camera], _omEachCamera[camera],
_tEachCamera[camera], _flags, TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 300, 1e-7),
idx);
}
_cameraMatrix[camera].convertTo(_cameraMatrix[camera], CV_32F);
_distortCoeffs[camera].convertTo(_distortCoeffs[camera], CV_32F);
_xi[camera].convertTo(_xi[camera], CV_32F);
//else
//{
// CV_Error_(CV_StsOutOfRange, "Unknown camera type, use PINHOLE or OMNIDIRECTIONAL");
//}
for (int i = 0; i < (int)_omEachCamera[camera].size(); ++i)
{
int cameraVertex, timestamp, photoVertex;
cameraVertex = camera;
timestamp = timestampAvailable[camera][idx.at<int>(i)];
photoVertex = this->getPhotoVertex(timestamp);
if (_omEachCamera[camera][i].type()!=CV_32F)
{
_omEachCamera[camera][i].convertTo(_omEachCamera[camera][i], CV_32F);
}
if (_tEachCamera[camera][i].type()!=CV_32F)
{
_tEachCamera[camera][i].convertTo(_tEachCamera[camera][i], CV_32F);
}
Mat transform = Mat::eye(4, 4, CV_32F);
Mat R, T;
Rodrigues(_omEachCamera[camera][i], R);
T = (_tEachCamera[camera][i]).reshape(1, 3);
R.copyTo(transform.rowRange(0, 3).colRange(0, 3));
T.copyTo(transform.rowRange(0, 3).col(3));
this->_edgeList.push_back(edge(cameraVertex, photoVertex, idx.at<int>(i), transform));
}
std::cout << "initialized for camera " << camera << " rms = " << rms << std::endl;
std::cout << "initialized camera matrix for camera " << camera << " is" << std::endl;
std::cout << _cameraMatrix[camera] << std::endl;
std::cout << "xi for camera " << camera << " is " << _xi[camera] << std::endl;
}
}
int MultiCameraCalibration::getPhotoVertex(int timestamp)
{
int photoVertex = INVALID;
// find in existing photo vertex
for (int i = 0; i < (int)_vertexList.size(); ++i)
{
if (_vertexList[i].timestamp == timestamp)
{
photoVertex = i;
break;
}
}
// add a new photo vertex
if (photoVertex == INVALID)
{
_vertexList.push_back(vertex(Mat::eye(4, 4, CV_32F), timestamp));
photoVertex = (int)_vertexList.size() - 1;
}
return photoVertex;
}
void MultiCameraCalibration::initialize()
{
int nVertices = (int)_vertexList.size();
int nEdges = (int) _edgeList.size();
// build graph
Mat G = Mat::zeros(nVertices, nVertices, CV_32S);
for (int edgeIdx = 0; edgeIdx < nEdges; ++edgeIdx)
{
G.at<int>(this->_edgeList[edgeIdx].cameraVertex, this->_edgeList[edgeIdx].photoVertex) = edgeIdx + 1;
}
G = G + G.t();
// traverse the graph
std::vector<int> pre, order;
graphTraverse(G, 0, order, pre);
for (int i = 0; i < _nCamera; ++i)
{
if (pre[i] == INVALID)
{
std::cout << "camera" << i << "is not connected" << std::endl;
}
}
for (int i = 1; i < (int)order.size(); ++i)
{
int vertexIdx = order[i];
Mat prePose = this->_vertexList[pre[vertexIdx]].pose;
int edgeIdx = G.at<int>(vertexIdx, pre[vertexIdx]) - 1;
Mat transform = this->_edgeList[edgeIdx].transform;
if (vertexIdx < _nCamera)
{
this->_vertexList[vertexIdx].pose = transform * prePose.inv();
this->_vertexList[vertexIdx].pose.convertTo(this->_vertexList[vertexIdx].pose, CV_32F);
if (_verbose)
{
std::cout << "initial pose for camera " << vertexIdx << " is " << std::endl;
std::cout << this->_vertexList[vertexIdx].pose << std::endl;
}
}
else
{
this->_vertexList[vertexIdx].pose = prePose.inv() * transform;
this->_vertexList[vertexIdx].pose.convertTo(this->_vertexList[vertexIdx].pose, CV_32F);
}
}
}
double MultiCameraCalibration::optimizeExtrinsics()
{
// get om, t vector
int nVertex = (int)this->_vertexList.size();
Mat extrinParam(1, (nVertex-1)*6, CV_32F);
int offset = 0;
// the pose of the vertex[0] is eye
for (int i = 1; i < nVertex; ++i)
{
Mat rvec, tvec;
cv::Rodrigues(this->_vertexList[i].pose.rowRange(0,3).colRange(0, 3), rvec);
this->_vertexList[i].pose.rowRange(0,3).col(3).copyTo(tvec);
rvec.reshape(1, 1).copyTo(extrinParam.colRange(offset, offset + 3));
tvec.reshape(1, 1).copyTo(extrinParam.colRange(offset+3, offset +6));
offset += 6;
}
//double error_pre = computeProjectError(extrinParam);
// optimization
const double alpha_smooth = 0.01;
double change = 1;
for(int iter = 0; ; ++iter)
{
if ((_criteria.type == 1 && iter >= _criteria.maxCount) ||
(_criteria.type == 2 && change <= _criteria.epsilon) ||
(_criteria.type == 3 && (change <= _criteria.epsilon || iter >= _criteria.maxCount)))
break;
double alpha_smooth2 = 1 - std::pow(1 - alpha_smooth, (double)iter + 1.0);
Mat JTJ_inv, JTError;
this->computeJacobianExtrinsic(extrinParam, JTJ_inv, JTError);
Mat G = alpha_smooth2*JTJ_inv * JTError;
if (G.depth() == CV_64F)
{
G.convertTo(G, CV_32F);
}
extrinParam = extrinParam + G.reshape(1, 1);
change = norm(G) / norm(extrinParam);
//double error = computeProjectError(extrinParam);
}
double error = computeProjectError(extrinParam);
std::vector<Vec3f> rvecVertex, tvecVertex;
vector2parameters(extrinParam, rvecVertex, tvecVertex);
for (int verIdx = 1; verIdx < (int)_vertexList.size(); ++verIdx)
{
Mat R;
Mat pose = Mat::eye(4, 4, CV_32F);
Rodrigues(rvecVertex[verIdx-1], R);
R.copyTo(pose.colRange(0, 3).rowRange(0, 3));
Mat(tvecVertex[verIdx-1]).reshape(1, 3).copyTo(pose.rowRange(0, 3).col(3));
_vertexList[verIdx].pose = pose;
if (_verbose && verIdx < _nCamera)
{
std::cout << "final camera pose of camera " << verIdx << " is" << std::endl;
std::cout << pose << std::endl;
}
}
return error;
}
void MultiCameraCalibration::computeJacobianExtrinsic(const Mat& extrinsicParams, Mat& JTJ_inv, Mat& JTE)
{
int nParam = (int)extrinsicParams.total();
int nEdge = (int)_edgeList.size();
std::vector<int> pointsLocation(nEdge+1, 0);
for (int edgeIdx = 0; edgeIdx < nEdge; ++edgeIdx)
{
int nPoints = (int)_objectPointsForEachCamera[_edgeList[edgeIdx].cameraVertex][_edgeList[edgeIdx].photoIndex].total();
pointsLocation[edgeIdx+1] = pointsLocation[edgeIdx] + nPoints*2;
}
JTJ_inv = Mat(nParam, nParam, CV_64F);
JTE = Mat(nParam, 1, CV_64F);
Mat J = Mat::zeros(pointsLocation[nEdge], nParam, CV_64F);
Mat E = Mat::zeros(pointsLocation[nEdge], 1, CV_64F);
for (int edgeIdx = 0; edgeIdx < nEdge; ++edgeIdx)
{
int photoVertex = _edgeList[edgeIdx].photoVertex;
int photoIndex = _edgeList[edgeIdx].photoIndex;
int cameraVertex = _edgeList[edgeIdx].cameraVertex;
Mat objectPoints = _objectPointsForEachCamera[cameraVertex][photoIndex];
Mat imagePoints = _imagePointsForEachCamera[cameraVertex][photoIndex];
Mat rvecTran, tvecTran;
Mat R = _edgeList[edgeIdx].transform.rowRange(0, 3).colRange(0, 3);
tvecTran = _edgeList[edgeIdx].transform.rowRange(0, 3).col(3);
cv::Rodrigues(R, rvecTran);
Mat rvecPhoto = extrinsicParams.colRange((photoVertex-1)*6, (photoVertex-1)*6 + 3);
Mat tvecPhoto = extrinsicParams.colRange((photoVertex-1)*6 + 3, (photoVertex-1)*6 + 6);
Mat rvecCamera, tvecCamera;
if (cameraVertex > 0)
{
rvecCamera = extrinsicParams.colRange((cameraVertex-1)*6, (cameraVertex-1)*6 + 3);
tvecCamera = extrinsicParams.colRange((cameraVertex-1)*6 + 3, (cameraVertex-1)*6 + 6);
}
else
{
rvecCamera = Mat::zeros(3, 1, CV_32F);
tvecCamera = Mat::zeros(3, 1, CV_32F);
}
Mat jacobianPhoto, jacobianCamera, error;
computePhotoCameraJacobian(rvecPhoto, tvecPhoto, rvecCamera, tvecCamera, rvecTran, tvecTran,
objectPoints, imagePoints, this->_cameraMatrix[cameraVertex], this->_distortCoeffs[cameraVertex],
this->_xi[cameraVertex], jacobianPhoto, jacobianCamera, error);
if (cameraVertex > 0)
{
jacobianCamera.copyTo(J.rowRange(pointsLocation[edgeIdx], pointsLocation[edgeIdx+1]).
colRange((cameraVertex-1)*6, cameraVertex*6));
}
jacobianPhoto.copyTo(J.rowRange(pointsLocation[edgeIdx], pointsLocation[edgeIdx+1]).
colRange((photoVertex-1)*6, photoVertex*6));
error.copyTo(E.rowRange(pointsLocation[edgeIdx], pointsLocation[edgeIdx+1]));
}
//std::cout << J.t() * J << std::endl;
JTJ_inv = (J.t() * J + 1e-10).inv();
JTE = J.t() * E;
}
void MultiCameraCalibration::computePhotoCameraJacobian(const Mat& rvecPhoto, const Mat& tvecPhoto, const Mat& rvecCamera,
const Mat& tvecCamera, Mat& rvecTran, Mat& tvecTran, const Mat& objectPoints, const Mat& imagePoints, const Mat& K,
const Mat& distort, const Mat& xi, Mat& jacobianPhoto, Mat& jacobianCamera, Mat& E)
{
Mat drvecTran_drecvPhoto, drvecTran_dtvecPhoto,
drvecTran_drvecCamera, drvecTran_dtvecCamera,
dtvecTran_drvecPhoto, dtvecTran_dtvecPhoto,
dtvecTran_drvecCamera, dtvecTran_dtvecCamera;
MultiCameraCalibration::compose_motion(rvecPhoto, tvecPhoto, rvecCamera, tvecCamera, rvecTran, tvecTran,
drvecTran_drecvPhoto, drvecTran_dtvecPhoto, drvecTran_drvecCamera, drvecTran_dtvecCamera,
dtvecTran_drvecPhoto, dtvecTran_dtvecPhoto, dtvecTran_drvecCamera, dtvecTran_dtvecCamera);
if (rvecTran.depth() == CV_64F)
{
rvecTran.convertTo(rvecTran, CV_32F);
}
if (tvecTran.depth() == CV_64F)
{
tvecTran.convertTo(tvecTran, CV_32F);
}
float xif = 0.0f;
if (_camType == OMNIDIRECTIONAL)
{
xif= xi.at<float>(0);
}
Mat imagePoints2, jacobian, dx_drvecCamera, dx_dtvecCamera, dx_drvecPhoto, dx_dtvecPhoto;
if (_camType == PINHOLE)
{
cv::projectPoints(objectPoints, rvecTran, tvecTran, K, distort, imagePoints2, jacobian);
}
//else if (_camType == FISHEYE)
//{
// cv::fisheye::projectPoints(objectPoints, imagePoints2, rvecTran, tvecTran, K, distort, 0, jacobian);
//}
else if (_camType == OMNIDIRECTIONAL)
{
cv::omnidir::projectPoints(objectPoints, imagePoints2, rvecTran, tvecTran, K, xif, distort, jacobian);
}
if (objectPoints.depth() == CV_32F)
{
Mat(imagePoints - imagePoints2).convertTo(E, CV_64FC2);
}
else
{
E = imagePoints - imagePoints2;
}
E = E.reshape(1, (int)imagePoints.total()*2);
dx_drvecCamera = jacobian.colRange(0, 3) * drvecTran_drvecCamera + jacobian.colRange(3, 6) * dtvecTran_drvecCamera;
dx_dtvecCamera = jacobian.colRange(0, 3) * drvecTran_dtvecCamera + jacobian.colRange(3, 6) * dtvecTran_dtvecCamera;
dx_drvecPhoto = jacobian.colRange(0, 3) * drvecTran_drecvPhoto + jacobian.colRange(3, 6) * dtvecTran_drvecPhoto;
dx_dtvecPhoto = jacobian.colRange(0, 3) * drvecTran_dtvecPhoto + jacobian.colRange(3, 6) * dtvecTran_dtvecPhoto;
jacobianCamera = cv::Mat(dx_drvecCamera.rows, 6, CV_64F);
jacobianPhoto = cv::Mat(dx_drvecPhoto.rows, 6, CV_64F);
dx_drvecCamera.copyTo(jacobianCamera.colRange(0, 3));
dx_dtvecCamera.copyTo(jacobianCamera.colRange(3, 6));
dx_drvecPhoto.copyTo(jacobianPhoto.colRange(0, 3));
dx_dtvecPhoto.copyTo(jacobianPhoto.colRange(3, 6));
}
void MultiCameraCalibration::graphTraverse(const Mat& G, int begin, std::vector<int>& order, std::vector<int>& pre)
{
CV_Assert(!G.empty() && G.rows == G.cols);
int nVertex = G.rows;
order.resize(0);
pre.resize(nVertex, INVALID);
pre[begin] = -1;
std::vector<bool> visited(nVertex, false);
std::queue<int> q;
visited[begin] = true;
q.push(begin);
order.push_back(begin);
while(!q.empty())
{
int v = q.front();
q.pop();
Mat idx;
// use my findNonZero maybe
findRowNonZero(G.row(v), idx);
for(int i = 0; i < (int)idx.total(); ++i)
{
int neighbor = idx.at<int>(i);
if (!visited[neighbor])
{
visited[neighbor] = true;
q.push(neighbor);
order.push_back(neighbor);
pre[neighbor] = v;
}
}
}
}
void MultiCameraCalibration::findRowNonZero(const Mat& row, Mat& idx)
{
CV_Assert(!row.empty() && row.rows == 1 && row.channels() == 1);
Mat _row;
std::vector<int> _idx;
row.convertTo(_row, CV_32F);
for (int i = 0; i < (int)row.total(); ++i)
{
if (_row.at<float>(i) != 0)
{
_idx.push_back(i);
}
}
idx.release();
idx.create(1, (int)_idx.size(), CV_32S);
for (int i = 0; i < (int)_idx.size(); ++i)
{
idx.at<int>(i) = _idx[i];
}
}
double MultiCameraCalibration::computeProjectError(Mat& parameters)
{
int nVertex = (int)_vertexList.size();
CV_Assert((int)parameters.total() == (nVertex-1) * 6 && parameters.depth() == CV_32F);
int nEdge = (int)_edgeList.size();
// recompute the transform between photos and cameras
std::vector<edge> edgeList = this->_edgeList;
std::vector<Vec3f> rvecVertex, tvecVertex;
vector2parameters(parameters, rvecVertex, tvecVertex);
float totalError = 0;
int totalNPoints = 0;
for (int edgeIdx = 0; edgeIdx < nEdge; ++edgeIdx)
{
Mat RPhoto, RCamera, TPhoto, TCamera, transform;
int cameraVertex = edgeList[edgeIdx].cameraVertex;
int photoVertex = edgeList[edgeIdx].photoVertex;
int PhotoIndex = edgeList[edgeIdx].photoIndex;
TPhoto = Mat(tvecVertex[photoVertex - 1]).reshape(1, 3);
//edgeList[edgeIdx].transform = Mat::ones(4, 4, CV_32F);
transform = Mat::eye(4, 4, CV_32F);
cv::Rodrigues(rvecVertex[photoVertex-1], RPhoto);
if (cameraVertex == 0)
{
RPhoto.copyTo(transform.rowRange(0, 3).colRange(0, 3));
TPhoto.copyTo(transform.rowRange(0, 3).col(3));
}
else
{
TCamera = Mat(tvecVertex[cameraVertex - 1]).reshape(1, 3);
cv::Rodrigues(rvecVertex[cameraVertex - 1], RCamera);
Mat(RCamera*RPhoto).copyTo(transform.rowRange(0, 3).colRange(0, 3));
Mat(RCamera * TPhoto + TCamera).copyTo(transform.rowRange(0, 3).col(3));
}
transform.copyTo(edgeList[edgeIdx].transform);
Mat rvec, tvec;
cv::Rodrigues(transform.rowRange(0, 3).colRange(0, 3), rvec);
transform.rowRange(0, 3).col(3).copyTo(tvec);
Mat objectPoints, imagePoints, proImagePoints;
objectPoints = this->_objectPointsForEachCamera[cameraVertex][PhotoIndex];
imagePoints = this->_imagePointsForEachCamera[cameraVertex][PhotoIndex];
if (this->_camType == PINHOLE)
{
cv::projectPoints(objectPoints, rvec, tvec, _cameraMatrix[cameraVertex], _distortCoeffs[cameraVertex],
proImagePoints);
}
//else if (this->_camType == FISHEYE)
//{
// cv::fisheye::projectPoints(objectPoints, proImagePoints, rvec, tvec, _cameraMatrix[cameraVertex],
// _distortCoeffs[cameraVertex]);
//}
else if (this->_camType == OMNIDIRECTIONAL)
{
float xi = _xi[cameraVertex].at<float>(0);
cv::omnidir::projectPoints(objectPoints, proImagePoints, rvec, tvec, _cameraMatrix[cameraVertex],
xi, _distortCoeffs[cameraVertex]);
}
Mat error = imagePoints - proImagePoints;
Vec2f* ptr_err = error.ptr<Vec2f>();
for (int i = 0; i < (int)error.total(); ++i)
{
totalError += sqrt(ptr_err[i][0]*ptr_err[i][0] + ptr_err[i][1]*ptr_err[i][1]);
}
totalNPoints += (int)error.total();
}
double meanReProjError = totalError / totalNPoints;
_error = meanReProjError;
return meanReProjError;
}
void MultiCameraCalibration::compose_motion(InputArray _om1, InputArray _T1, InputArray _om2, InputArray _T2, Mat& om3, Mat& T3, Mat& dom3dom1,
Mat& dom3dT1, Mat& dom3dom2, Mat& dom3dT2, Mat& dT3dom1, Mat& dT3dT1, Mat& dT3dom2, Mat& dT3dT2)
{
Mat om1, om2, T1, T2;
_om1.getMat().convertTo(om1, CV_64F);
_om2.getMat().convertTo(om2, CV_64F);
_T1.getMat().reshape(1, 3).convertTo(T1, CV_64F);
_T2.getMat().reshape(1, 3).convertTo(T2, CV_64F);
/*Mat om2 = _om2.getMat();
Mat T1 = _T1.getMat().reshape(1, 3);
Mat T2 = _T2.getMat().reshape(1, 3);*/
//% Rotations:
Mat R1, R2, R3, dR1dom1(9, 3, CV_64FC1), dR2dom2;
cv::Rodrigues(om1, R1, dR1dom1);
cv::Rodrigues(om2, R2, dR2dom2);
/*JRodriguesMatlab(dR1dom1, dR1dom1);
JRodriguesMatlab(dR2dom2, dR2dom2);*/
dR1dom1 = dR1dom1.t();
dR2dom2 = dR2dom2.t();
R3 = R2 * R1;
Mat dR3dR2, dR3dR1;
//dAB(R2, R1, dR3dR2, dR3dR1);
matMulDeriv(R2, R1, dR3dR2, dR3dR1);
Mat dom3dR3;
cv::Rodrigues(R3, om3, dom3dR3);
//JRodriguesMatlab(dom3dR3, dom3dR3);
dom3dR3 = dom3dR3.t();
dom3dom1 = dom3dR3 * dR3dR1 * dR1dom1;
dom3dom2 = dom3dR3 * dR3dR2 * dR2dom2;
dom3dT1 = Mat::zeros(3, 3, CV_64FC1);
dom3dT2 = Mat::zeros(3, 3, CV_64FC1);
//% Translations:
Mat T3t = R2 * T1;
Mat dT3tdR2, dT3tdT1;
//dAB(R2, T1, dT3tdR2, dT3tdT1);
matMulDeriv(R2, T1, dT3tdR2, dT3tdT1);
Mat dT3tdom2 = dT3tdR2 * dR2dom2;
T3 = T3t + T2;
dT3dT1 = dT3tdT1;
dT3dT2 = Mat::eye(3, 3, CV_64FC1);
dT3dom2 = dT3tdom2;
dT3dom1 = Mat::zeros(3, 3, CV_64FC1);
}
void MultiCameraCalibration::vector2parameters(const Mat& parameters, std::vector<Vec3f>& rvecVertex, std::vector<Vec3f>& tvecVertexs)
{
int nVertex = (int)_vertexList.size();
CV_Assert((int)parameters.channels() == 1 && (int)parameters.total() == 6*(nVertex - 1));
CV_Assert(parameters.depth() == CV_32F);
parameters.reshape(1, 1);
rvecVertex.reserve(0);
tvecVertexs.resize(0);
for (int i = 0; i < nVertex - 1; ++i)
{
rvecVertex.push_back(Vec3f(parameters.colRange(i*6, i*6 + 3)));
tvecVertexs.push_back(Vec3f(parameters.colRange(i*6 + 3, i*6 + 6)));
}
}
void MultiCameraCalibration::parameters2vector(const std::vector<Vec3f>& rvecVertex, const std::vector<Vec3f>& tvecVertex, Mat& parameters)
{
CV_Assert(rvecVertex.size() == tvecVertex.size());
int nVertex = (int)rvecVertex.size();
// the pose of the first camera is known
parameters.create(1, 6*(nVertex-1), CV_32F);
for (int i = 0; i < nVertex-1; ++i)
{
Mat(rvecVertex[i]).reshape(1, 1).copyTo(parameters.colRange(i*6, i*6 + 3));
Mat(tvecVertex[i]).reshape(1, 1).copyTo(parameters.colRange(i*6 + 3, i*6 + 6));
}
}
void MultiCameraCalibration::writeParameters(const std::string& filename)
{
FileStorage fs( filename, FileStorage::WRITE );
fs << "nCameras" << _nCamera;
for (int camIdx = 0; camIdx < _nCamera; ++camIdx)
{
std::stringstream tmpStr;
tmpStr << camIdx;
std::string cameraMatrix = "camera_matrix_" + tmpStr.str();
std::string cameraPose = "camera_pose_" + tmpStr.str();
std::string cameraDistortion = "camera_distortion_" + tmpStr.str();
std::string cameraXi = "xi_" + tmpStr.str();
fs << cameraMatrix << _cameraMatrix[camIdx];
fs << cameraDistortion << _distortCoeffs[camIdx];
if (_camType == OMNIDIRECTIONAL)
{
fs << cameraXi << _xi[camIdx].at<float>(0);
}
fs << cameraPose << _vertexList[camIdx].pose;
}
fs << "meanReprojectError" <<_error;
for (int photoIdx = _nCamera; photoIdx < (int)_vertexList.size(); ++photoIdx)
{
std::stringstream tmpStr;
tmpStr << _vertexList[photoIdx].timestamp;
std::string photoTimestamp = "pose_timestamp_" + tmpStr.str();
fs << photoTimestamp << _vertexList[photoIdx].pose;
}
}
}} // namespace multicalib, cv