bool CameraProjectorCalibration::setDynamicProjectorImagePoints(cv::Mat img){
vector<cv::Point2f> chessImgPts;
bool bPrintedPatternFound = calibrationCamera.findBoard(img, chessImgPts, true);
if(bPrintedPatternFound) {
cv::Mat boardRot;
cv::Mat boardTrans;
calibrationCamera.computeCandidateBoardPose(chessImgPts, boardRot, boardTrans);
const auto & camCandObjPts = calibrationCamera.getCandidateObjectPoints();
Point3f axisX = camCandObjPts[1] - camCandObjPts[0];
Point3f axisY = camCandObjPts[calibrationCamera.getPatternSize().width] - camCandObjPts[0];
Point3f pos = camCandObjPts[0] - axisY * (calibrationCamera.getPatternSize().width-2);
vector<Point3f> auxObjectPoints;
for(int i = 0; i < calibrationProjector.getPatternSize().height; i++) {
for(int j = 0; j < calibrationProjector.getPatternSize().width; j++) {
auxObjectPoints.push_back(pos + axisX * float((2 * j) + (i % 2)) + axisY * i);
}
}
Mat Rc1, Tc1, Rc1inv, Tc1inv, Rc2, Tc2, Rp1, Tp1, Rp2, Tp2;
Rp1 = calibrationProjector.getBoardRotations().back();
Tp1 = calibrationProjector.getBoardTranslations().back();
Rc1 = calibrationCamera.getBoardRotations().back();
Tc1 = calibrationCamera.getBoardTranslations().back();
Rc2 = boardRot;
Tc2 = boardTrans;
Mat auxRinv = Mat::eye(3,3,CV_32F);
Rodrigues(Rc1,auxRinv);
auxRinv = auxRinv.inv();
Rodrigues(auxRinv, Rc1inv);
Tc1inv = -auxRinv*Tc1;
Mat Raux, Taux;
composeRT(Rc2, Tc2, Rc1inv, Tc1inv, Raux, Taux);
composeRT(Raux, Taux, Rp1, Tp1, Rp2, Tp2);
vector<Point2f> followingPatternImagePoints;
projectPoints(Mat(auxObjectPoints),
Rp2, Tp2,
calibrationProjector.getDistortedIntrinsics().getCameraMatrix(),
calibrationProjector.getDistCoeffs(),
followingPatternImagePoints);
calibrationProjector.setCandidateImagePoints(followingPatternImagePoints);
}
return bPrintedPatternFound;
}
void BoardDetector::rotateXAxis(Mat &rotation) {
cv::Mat R(3, 3, CV_32FC1);
Rodrigues(rotation, R);
// create a rotation matrix for x axis
cv::Mat RX = cv::Mat::eye(3, 3, CV_32FC1);
float angleRad = -M_PI / 2;
RX.at< float >(1, 1) = cos(angleRad);
RX.at< float >(1, 2) = -sin(angleRad);
RX.at< float >(2, 1) = sin(angleRad);
RX.at< float >(2, 2) = cos(angleRad);
// now multiply
R = R * RX;
// finally, the the rodrigues back
Rodrigues(R, rotation);
}
void transform3DPoints(const Mat &points,
const Mat &rvec,
const Mat &tvec,
Mat *modif_points)
{
Mat transformation;
if ((rvec.rows == 3 && rvec.cols == 1) || (rvec.rows == 1 && rvec.cols == 3))
{
Mat R;
Rodrigues(rvec, R);
cv::hconcat(R, tvec, transformation);
}
else
if (rvec.rows == 3 && rvec.cols == 3)
{
cv::hconcat(rvec, tvec, transformation);
}
else
{
cerr << "wrong size!" << endl;
return;
}
transform(points, *modif_points, transformation);
}
bool Camera::calculateExtrinsicParams(vector<Point3f> objectPoints, vector<Point2f> imagePoints)
{
OPENCV_ASSERT(isCalibrated,"Camera.calculateExtrinsicParams","Cannot calculate extrinsic parameters before camera calibration!");
Mat rvec, tvec;
Mat rotMtx;
bool solverResult = solvePnP(
Mat(objectPoints), Mat(imagePoints), // Input correspondences
cameraMatrix, distortionCoeffs, // Intrinsic camera parameters (have to be already available)
rvec, tvec);
if (solverResult)
{
// Create this->T from rvec and tvec
Rodrigues(rvec, rotMtx);
Matx44f T_inv = Matx44f(
(float)rotMtx.at<double>(0,0), (float)rotMtx.at<double>(0,1), (float)rotMtx.at<double>(0,2), (float)tvec.at<double>(0,0),
(float)rotMtx.at<double>(1,0), (float)rotMtx.at<double>(1,1), (float)rotMtx.at<double>(1,2), (float)tvec.at<double>(1,0),
(float)rotMtx.at<double>(2,0), (float)rotMtx.at<double>(2,1), (float)rotMtx.at<double>(2,2), (float)tvec.at<double>(2,0),
0.0F, 0.0F, 0.0F, 1.0F
);
T = T_inv.inv();
isTSet=true;
}
return solverResult;
}
void CameraCalibration::CvtCameraExtrins(const vector<Mat> *RVecs, const vector<Mat> *TVecs)
{
Mat rot3x3;
OpenGlMatrixSpec P = IdentityMatrix(GlModelViewStack);
if(stereo_mode)
{
CamExtrins = new vector<OpenGlMatrixSpec>[2];
for(int i=0; i<NumFrames; i++)
{
for(int c_idx=0; c_idx<2; c_idx++)
{
Rodrigues(RVecs[c_idx].at(i), rot3x3);
for(int col=0; col<3; col++)
for(int row=0; row<3; row++)
P.m[col*4+row] = rot3x3.at<double>(row, col);
copy(TVecs[c_idx].at(i).ptr<double>(), TVecs[c_idx].at(i).ptr<double>()+3, &P.m[12]);
CamExtrins[c_idx].push_back(P);
}
}
L2RExtrins = IdentityMatrix(GlModelViewStack);
for(int col=0; col<3; col++)
for(int row=0; row<3; row++)
L2RExtrins.m[col*4+row] = stereo_params->R.at<double>(row, col);
copy(stereo_params->T.ptr<double>(), stereo_params->T.ptr<double>()+3, &L2RExtrins.m[12]);
}
else
{
CamExtrins = new vector<OpenGlMatrixSpec>;
for(int i=0; i<NumFrames; i++)
{
Rodrigues(RVecs->at(i), rot3x3);
for(int col=0; col<3; col++)
for(int row=0; row<3; row++)
P.m[col*4+row] = rot3x3.at<double>(row, col);
copy(TVecs->at(i).ptr<double>(), TVecs->at(i).ptr<double>()+3, &P.m[12]);
CamExtrins->push_back(P);
}
}
}
double CalibCameraWrapperEx(
cv::Point3f const * pts3d, cv::Point2f const * pts2d, unsigned int ptCount,
unsigned int imgWidth, unsigned int imgHeight, float * intrinsicMatInOut,
float * rotationOut, float * translationOut )
{
cv::Mat distCoeffs_est;
std::vector<cv::Mat> rvecs(1), tvecs(1);
cv::Mat1d camMat_est(3,3, 0.);
if( intrinsicMatInOut )
{
cv::Mat1f(3,3,intrinsicMatInOut).convertTo( camMat_est, CV_64F );
}
std::vector<std::vector<cv::Point3f>> test3d_list( 1 );
test3d_list.front().assign( pts3d, pts3d + ptCount );
std::vector<std::vector<cv::Point2f>> test2d_list( 1 );
test2d_list.front().assign( pts2d, pts2d + ptCount );
cv::Size2f const imgSize(imgWidth,imgHeight);
int const flags =
(intrinsicMatInOut ? CV_CALIB_USE_INTRINSIC_GUESS : 0) |
CV_CALIB_ZERO_TANGENT_DIST |
CV_CALIB_FIX_K2 | CV_CALIB_FIX_K3 | CV_CALIB_FIX_K4 | CV_CALIB_FIX_K5 | CV_CALIB_FIX_K6;
double rep_err = cv::calibrateCamera(test3d_list, test2d_list, imgSize,
camMat_est, distCoeffs_est, rvecs, tvecs, flags );
if( translationOut )
{
cv::Mat1f tempOut(1,3,translationOut);
tvecs.front().convertTo( tempOut, CV_32F );
}
if( rotationOut )
{
cv::Mat1d temp(3,3);
Rodrigues( rvecs.front(), temp );
cv::Mat1f tempOut(3,3,rotationOut);
temp.convertTo( tempOut, CV_32F );
}
if( intrinsicMatInOut )
{
cv::Mat1f tempOut(3,3,intrinsicMatInOut);
camMat_est.convertTo( tempOut, CV_32F );
}
return rep_err;
}
void project3dPoints(const Mat& points, const Mat& rvec, const Mat& tvec, Mat& modif_points)
{
modif_points.create(1, points.cols, CV_32FC3);
Mat R(3, 3, CV_64FC1);
Rodrigues(rvec, R);
Mat transformation(3, 4, CV_64F);
Mat r = transformation.colRange(0, 3);
R.copyTo(r);
Mat t = transformation.colRange(3, 4);
tvec.copyTo(t);
transform(points, modif_points, transformation);
}
void
DynNewtonian::streamParticle(Particle &particle, const double &dt) const
{
particle.getPosition() += particle.getVelocity() * dt;
//The Vector copy is required to make sure that the cached
//orientation doesn't change during calculation
if (hasOrientationData())
orientationData[particle.getID()].orientation
= Rodrigues(orientationData[particle.getID()].angularVelocity * dt)
* Vector(orientationData[particle.getID()].orientation);
}
Eigen::Isometry3d transformEstimation(const Mat& rgb1,const Mat& rgb2,const Mat& depth1,const Mat& depth2,const CAMERA_INTRINSIC_PARAMETERS& C)
{
vector<KeyPoint> keyPts1,keyPts2;
Mat descriptors1,descriptors2;
extractKeypointsAndDescripotrs(rgb1,rgb2,keyPts1,keyPts2,descriptors1,descriptors2);
vector<DMatch> matches;
descriptorsMatch(rgb1,rgb2,keyPts1,keyPts2,descriptors1,descriptors2,matches);
vector<Point2f> points;
vector<Point3f> objectPoints;
vector<Eigen::Vector2d> imagePoints1,imagePoints2;
getObjectPointsAndImagePoints(depth1,depth2,keyPts1,keyPts2,matches,points,objectPoints,imagePoints1,imagePoints2,C);
Mat translation,rotation;
double camera_matrix_data[3][3] = {
{C.fx, 0, C.cx},
{0, C.fy, C.cy},
{0, 0, 1} };
Mat cameraMatrix(3,3,CV_64F,camera_matrix_data);
solvePnPRansac(objectPoints,points,cameraMatrix,Mat(),rotation,translation,false, 100, 1.0, 20);
Mat rot;
Rodrigues(rotation,rot);
Eigen::Matrix3d r;
Eigen::Vector3d t;
cout<<rot<<endl;
cout<<translation<<endl;
r<< ((double*)rot.data)[0],((double*)rot.data)[1],((double*)rot.data)[2],
((double*)rot.data)[3],((double*)rot.data)[4],((double*)rot.data)[5],
((double*)rot.data)[6],((double*)rot.data)[7],((double*)rot.data)[8];
t<<((double*)translation.data)[0],((double*)translation.data)[1],((double*)translation.data)[2];
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
T.rotate(r);
T.pretranslate(t);
cout<<T.matrix()<<endl;
BundleAdjustmentOptimization(objectPoints,imagePoints1,imagePoints2);
return T;
}
/*
void point3d2Mat(const Point3d& src, Mat& dest)
{
dest.create(3,1,CV_64F);
dest.at<double>(0,0)=src.x;
dest.at<double>(1,0)=src.y;
dest.at<double>(2,0)=src.z;
}
void setXYZ(Mat& in, double&x, double&y, double&z)
{
x=in.at<double>(0,0);
y=in.at<double>(1,0);
z=in.at<double>(2,0);
// cout<<format("set XYZ: %.04f %.04f %.04f\n",x,y,z);
}
void lookatBF(const Point3d& from, const Point3d& to, Mat& destR)
{
double x,y,z;
Mat fromMat;
Mat toMat;
point3d2Mat(from,fromMat);
point3d2Mat(to,toMat);
Mat fromtoMat;
add(toMat,fromMat,fromtoMat,Mat(),CV_64F);
double ndiv = 1.0/norm(fromtoMat);
fromtoMat*=ndiv;
setXYZ(fromtoMat,x,y,z);
destR = Mat::eye(3,3,CV_64F);
double yaw =-z/abs(z)*asin(y/sqrt(y*y+z*z))/CV_PI*180.0;
rotYaw(destR,destR,yaw);
Mat RfromtoMat = destR*fromtoMat;
setXYZ(RfromtoMat,x,y,z);
double pitch =z/abs(z)*asin(x/sqrt(x*x+z*z))/CV_PI*180.0;
rotPitch(destR,destR,pitch);
}
*/
void lookat(const Point3d& from, const Point3d& to, Mat& destR)
{
Mat destMat = Mat(Point3d(0.0, 0.0, 1.0));
Mat srcMat = Mat(from + to);
srcMat = srcMat / norm(srcMat);
Mat rotaxis = srcMat.cross(destMat);
double angle = acos(srcMat.dot(destMat));
//normalize cross product and multiply rotation angle
rotaxis = rotaxis / norm(rotaxis)*angle;
Rodrigues(rotaxis, destR);
}
int solveP3P( InputArray _opoints, InputArray _ipoints,
InputArray _cameraMatrix, InputArray _distCoeffs,
OutputArrayOfArrays _rvecs, OutputArrayOfArrays _tvecs, int flags) {
CV_INSTRUMENT_REGION();
Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat();
int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
CV_Assert( npoints == 3 && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) );
CV_Assert( flags == SOLVEPNP_P3P || flags == SOLVEPNP_AP3P );
Mat cameraMatrix0 = _cameraMatrix.getMat();
Mat distCoeffs0 = _distCoeffs.getMat();
Mat cameraMatrix = Mat_<double>(cameraMatrix0);
Mat distCoeffs = Mat_<double>(distCoeffs0);
Mat undistortedPoints;
undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
std::vector<Mat> Rs, ts;
int solutions = 0;
if (flags == SOLVEPNP_P3P)
{
p3p P3Psolver(cameraMatrix);
solutions = P3Psolver.solve(Rs, ts, opoints, undistortedPoints);
}
else if (flags == SOLVEPNP_AP3P)
{
ap3p P3Psolver(cameraMatrix);
solutions = P3Psolver.solve(Rs, ts, opoints, undistortedPoints);
}
if (solutions == 0) {
return 0;
}
if (_rvecs.needed()) {
_rvecs.create(solutions, 1, CV_64F);
}
if (_tvecs.needed()) {
_tvecs.create(solutions, 1, CV_64F);
}
for (int i = 0; i < solutions; i++) {
Mat rvec;
Rodrigues(Rs[i], rvec);
_tvecs.getMatRef(i) = ts[i];
_rvecs.getMatRef(i) = rvec;
}
return solutions;
}
void Epipolar::getEssMatrix(Mat& rot1, Mat& rot2, Mat& trans1, Mat& trans2, Mat& approxEss){
Mat rot1Mat, rot2Mat;
if (rot1.size() == Size(3, 3))
{
rot1Mat = rot1;
}
else
{
Rodrigues(rot1, rot1Mat);
}
if (rot2.size() == Size(3, 3))
{
rot2Mat = rot2;
}
else
{
Rodrigues(rot2, rot2Mat);
}
Mat relRot = rot1Mat.inv() * rot2Mat;
Mat relTrans = trans2 - trans1;
getEssMatrix(relRot, relTrans, approxEss);
}
void Marker::glGetModelViewMatrix( double modelview_matrix[16])throw(cv::Exception)
{
//check if paremeters are valid
bool invalid=false;
for (int i=0;i<3 && !invalid ;i++)
{
if (Tvec.at<float>(i,0)!=-999999) invalid|=false;
if (Rvec.at<float>(i,0)!=-999999) invalid|=false;
}
if (invalid) throw cv::Exception(9003,"extrinsic parameters are not set","Marker::getModelViewMatrix",__FILE__,__LINE__);
Mat Rot(3,3,CV_32FC1),Jacob;
Rodrigues(Rvec, Rot, Jacob);
double para[3][4];
for (int i=0;i<3;i++)
for (int j=0;j<3;j++) para[i][j]=Rot.at<float>(i,j);
//now, add the translation
para[0][3]=Tvec.at<float>(0,0);
para[1][3]=Tvec.at<float>(1,0);
para[2][3]=Tvec.at<float>(2,0);
double scale=1;
modelview_matrix[0 + 0*4] = para[0][0];
// R1C2
modelview_matrix[0 + 1*4] = para[0][1];
modelview_matrix[0 + 2*4] = para[0][2];
modelview_matrix[0 + 3*4] = para[0][3];
// R2
modelview_matrix[1 + 0*4] = para[1][0];
modelview_matrix[1 + 1*4] = para[1][1];
modelview_matrix[1 + 2*4] = para[1][2];
modelview_matrix[1 + 3*4] = para[1][3];
// R3
modelview_matrix[2 + 0*4] = -para[2][0];
modelview_matrix[2 + 1*4] = -para[2][1];
modelview_matrix[2 + 2*4] = -para[2][2];
modelview_matrix[2 + 3*4] = -para[2][3];
modelview_matrix[3 + 0*4] = 0.0;
modelview_matrix[3 + 1*4] = 0.0;
modelview_matrix[3 + 2*4] = 0.0;
modelview_matrix[3 + 3*4] = 1.0;
if (scale != 0.0)
{
modelview_matrix[12] *= scale;
modelview_matrix[13] *= scale;
modelview_matrix[14] *= scale;
}
}
ofMatrix4x4 makeMatrix(Mat rotation, Mat translation) {
Mat rot3x3;
if(rotation.rows == 3 && rotation.cols == 3) {
rot3x3 = rotation;
} else {
Rodrigues(rotation, rot3x3);
}
double* rm = rot3x3.ptr<double>(0);
double* tm = translation.ptr<double>(0);
return ofMatrix4x4(rm[0], rm[3], rm[6], 0.0f,
rm[1], rm[4], rm[7], 0.0f,
rm[2], rm[5], rm[8], 0.0f,
tm[0], tm[1], tm[2], 1.0f);
}
std::array<double, nderivs> eval(const double dt = 0) const
{
math::Vector u1new = Rodrigues(w1 * dt) * math::Vector(u1);
math::Vector u2new = Rodrigues(w2 * dt) * math::Vector(u2);
const double colldiam = 0.5 * (_diameter1 + _diameter2);
const double growthfactor = 1 + _invgamma * (_t + dt);
const math::Vector rij = r12 + dt * v12 + growthfactor * (u1new - u2new);
const math::Vector vij = v12 + growthfactor * ((w1 ^ u1new) - (w2 ^ u2new)) + _invgamma * (u1new - u2new);
const math::Vector aij = growthfactor * (-w1.nrm2() * u1new + w2.nrm2() * u2new) + 2 * _invgamma * ((w1 ^ u1new) - (w2 ^ u2new));
const math::Vector dotaij = growthfactor * (-w1.nrm2() * (w1 ^ u1new) + w2.nrm2() * (w2 ^ u2new)) + 3 * _invgamma * (-w1.nrm2() * u1new + w2.nrm2() * u2new);
std::array<double, nderivs> retval;
for (size_t i(0); i < nderivs; ++i)
switch (first_deriv + i) {
case 0: retval[i] = (rij | rij) - growthfactor * growthfactor * colldiam * colldiam; break;
case 1: retval[i] = 2 * (rij | vij) - 2 * _invgamma * growthfactor * colldiam * colldiam; break;
case 2: retval[i] = 2 * vij.nrm2() + 2 * (rij | aij) - 2 * _invgamma * _invgamma * colldiam * colldiam; break;
case 3: retval[i] = 6 * (vij | aij) + 2 * (rij | dotaij); break;
default:
M_throw() << "Invalid access";
}
return retval;
}
void Marker::rotateXAxis( cv::Mat& rotation)
{
cv::Mat R(3, 3, CV_32F);
cv::Rodrigues(rotation, R);
// create a rotation matrix for x axis
cv::Mat RX = cv::Mat::eye(3, 3, CV_32F);
const float angleRad = 3.14159265359f / 2.f;
RX.at<float>(1, 1) = cos(angleRad);
RX.at<float>(1, 2) = -sin(angleRad);
RX.at<float>(2, 1) = sin(angleRad);
RX.at<float>(2, 2) = cos(angleRad);
// now multiply
R = R * RX;
// finally, the the rodrigues back
Rodrigues(R, rotation);
}
void PositionCalculatorImpl::addMeasurementImpl( InputArray _tvec, InputArray _rvec, const Point2f _pt
, double /*time*/,
InputArray _cameraMatrix, InputArray _distortionMatrix )
{
Mat tvec = _tvec.getMat();
Mat rvec = _rvec.getMat();
Mat camera_matrix = _cameraMatrix.getMat();
const Mat distortion_matrix = _distortionMatrix.getMat();
std::vector< Point2f > pts_in, pts_out;
pts_in.push_back( _pt );
undistortPoints( pts_in, pts_out, camera_matrix, distortion_matrix, noArray(), camera_matrix );
Mat los( 3, 1,CV_64F );
los.at< double >( 0 ) = pts_out[0].x;
los.at< double >( 1 ) = pts_out[0].y;
los.at< double >( 2 ) = 1;
if ( camera_matrix.type() != CV_64F )
camera_matrix.convertTo( camera_matrix, CV_64F );
if ( rvec.type() != CV_64F )
rvec.convertTo( rvec, CV_64F );
if ( tvec.type() != CV_64F )
tvec.convertTo( tvec, CV_64F );
los = camera_matrix.inv() * los;
Mat camera_rotation;
if ( rvec.rows == 3 && rvec.cols == 3 )
camera_rotation = rvec;
else
Rodrigues( rvec, camera_rotation );
if(tvec.rows == 1)
tvec = tvec.t();
camera_rotation = camera_rotation.t();
const Mat camera_translation = ( -camera_rotation * tvec );
los = camera_rotation * los;
positions.push_back( camera_translation );
Mat zero_pos( 3, 1, CV_64F );
zero_pos.setTo( 0 );
const Point2d azel = getAzEl( zero_pos, los );
angles.push_back( azel );
}
// some crazy stuff, no idea what's happening there. So thanks Alvaro & Niklas!
bool CameraCalibration::backProject(const Mat& boardRot64,
const Mat& boardTrans64,
const vector<Point2f>& imgPt,
vector<Point3f>& worldPt) {
if( imgPt.size() == 0 ) {
return false;
}
else
{
Mat imgPt_h = Mat::zeros(3, imgPt.size(), CV_32F);
for( int h=0; h<imgPt.size(); ++h ) {
imgPt_h.at<float>(0,h) = imgPt[h].x;
imgPt_h.at<float>(1,h) = imgPt[h].y;
imgPt_h.at<float>(2,h) = 1.0f;
}
Mat Kinv64 = getUndistortedIntrinsics().getCameraMatrix().inv();
Mat Kinv,boardRot,boardTrans;
Kinv64.convertTo(Kinv, CV_32F);
boardRot64.convertTo(boardRot, CV_32F);
boardTrans64.convertTo(boardTrans, CV_32F);
// Transform all image points to world points in camera reference frame
// and then into the plane reference frame
Mat worldImgPt = Mat::zeros( 3, imgPt.size(), CV_32F );
Mat rot3x3;
Rodrigues(boardRot, rot3x3);
Mat transPlaneToCam = rot3x3.inv()*boardTrans;
for( int i=0; i<imgPt.size(); ++i ) {
Mat col = imgPt_h.col(i);
Mat worldPtcam = Kinv*col;
Mat worldPtPlane = rot3x3.inv()*(worldPtcam);
float scale = transPlaneToCam.at<float>(2)/worldPtPlane.at<float>(2);
Mat worldPtPlaneReproject = scale*worldPtPlane-transPlaneToCam;
Point3f pt;
pt.x = worldPtPlaneReproject.at<float>(0);
pt.y = worldPtPlaneReproject.at<float>(1);
pt.z = 0;
worldPt.push_back(pt);
}
}
return true;
}
void Pose::UpdateMat() {
Matx33d rmat;
Rodrigues(rvec_, rmat);
mat_(0, 0) = rmat(0, 0);
mat_(0, 1) = rmat(0, 1);
mat_(0, 2) = rmat(0, 2);
mat_(1, 0) = rmat(1, 0);
mat_(1, 1) = rmat(1, 1);
mat_(1, 2) = rmat(1, 2);
mat_(2, 0) = rmat(2, 0);
mat_(2, 1) = rmat(2, 1);
mat_(2, 2) = rmat(2, 2);
mat_(0, 3) = tvec_(0);
mat_(1, 3) = tvec_(1);
mat_(2, 3) = tvec_(2);
}
PinholeCamera::PinholeCamera(Vec3f rvector, Vec3f translation)
{
Rodrigues(rvector,this->R);
this->T = Mat(translation);
this->r = -1;
this->l = 1;
this->b = -1;
this->t = 1;
this->n = 1;
this->f = 2;
this->setFrustum(l,r,b,t,n,f);
cout << "olha1\n " << this->R << endl;
cout << this->T << endl;
}
Isometry3d cvmat2eigen(Mat &r, Mat &t)
{
//rotation vector to matrix
Mat R;
Rodrigues(r, R);
//opencv matrix to eigen matrix format
Matrix3d re;
cv2eigen(R, re);
AngleAxisd angle(re);
//3x3 identity matrix
Isometry3d T = Isometry3d::Identity();
T = angle;
T(0,3) = t.at<double>(0,0);
T(1,3) = t.at<double>(0,1);
T(2,3) = t.at<double>(0,2);
return T;
}
void Warp::set(Matx13f rotate)
{
r = rotate;
Matx<float, 3, 9> J;
Rodrigues(r, R, J);
Dx = Matx33f(
J(0, 0), J(1, 0), J(2, 0),
J(0, 3), J(1, 3), J(2, 3),
J(0, 6), J(1, 6), J(2, 6)
);
Dy = Matx33f(
J(0, 1), J(1, 1), J(2, 1),
J(0, 4), J(1, 4), J(2, 4),
J(0, 7), J(1, 7), J(2, 7)
);
Dz = Matx33f(
J(0, 2), J(1, 2), J(2, 2),
J(0, 5), J(1, 5), J(2, 5),
J(0, 8), J(1, 8), J(2, 8)
);
}
void PnPProblem::estimatePoseRANSAC( const std::vector<cv::Point3f> &list_points3d, // list with model 3D coordinates
const std::vector<cv::Point2f> &list_points2d, // list with scene 2D coordinates
int flags, cv::Mat &inliers, int iterationsCount, // PnP method; inliers container
float reprojectionError, double confidence ) // Ransac parameters
{
cv::Mat distCoeffs = cv::Mat::zeros(4, 1, CV_64FC1); // vector of distortion coefficients
cv::Mat rvec = cv::Mat::zeros(3, 1, CV_64FC1); // output rotation vector
cv::Mat tvec = cv::Mat::zeros(3, 1, CV_64FC1); // output translation vector
bool useExtrinsicGuess = false; // if true the function uses the provided rvec and tvec values as
// initial approximations of the rotation and translation vectors
cv::solvePnPRansac( list_points3d, list_points2d, _A_matrix, distCoeffs, rvec, tvec,
useExtrinsicGuess, iterationsCount, reprojectionError, confidence,
inliers, flags );
Rodrigues(rvec,_R_matrix); // converts Rotation Vector to Matrix
_t_matrix = tvec; // set translation matrix
this->set_P_matrix(_R_matrix, _t_matrix); // set rotation-translation matrix
}
// Estimate the pose given a list of 2D/3D correspondences and the method to use
bool PnPProblem::estimatePose( const std::vector<cv::Point3f> &list_points3d,
const std::vector<cv::Point2f> &list_points2d,
int flags)
{
cv::Mat distCoeffs = cv::Mat::zeros(4, 1, CV_64FC1);
cv::Mat rvec = cv::Mat::zeros(3, 1, CV_64FC1);
cv::Mat tvec = cv::Mat::zeros(3, 1, CV_64FC1);
bool useExtrinsicGuess = false;
// Pose estimation
bool correspondence = cv::solvePnP( list_points3d, list_points2d, _A_matrix, distCoeffs, rvec, tvec,
useExtrinsicGuess, flags);
// Transforms Rotation Vector to Matrix
Rodrigues(rvec,_R_matrix);
_t_matrix = tvec;
// Set projection matrix
this->set_P_matrix(_R_matrix, _t_matrix);
return correspondence;
}
bool solvePnP( InputArray _opoints, InputArray _ipoints,
InputArray _cameraMatrix, InputArray _distCoeffs,
OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess, int flags )
{
CV_INSTRUMENT_REGION()
Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat();
int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
CV_Assert( npoints >= 0 && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) );
Mat rvec, tvec;
if( flags != SOLVEPNP_ITERATIVE )
useExtrinsicGuess = false;
if( useExtrinsicGuess )
{
int rtype = _rvec.type(), ttype = _tvec.type();
Size rsize = _rvec.size(), tsize = _tvec.size();
CV_Assert( (rtype == CV_32F || rtype == CV_64F) &&
(ttype == CV_32F || ttype == CV_64F) );
CV_Assert( (rsize == Size(1, 3) || rsize == Size(3, 1)) &&
(tsize == Size(1, 3) || tsize == Size(3, 1)) );
}
else
{
int mtype = CV_64F;
// use CV_32F if all PnP inputs are CV_32F and outputs are empty
if (_ipoints.depth() == _cameraMatrix.depth() && _ipoints.depth() == _opoints.depth() &&
_rvec.empty() && _tvec.empty())
mtype = _opoints.depth();
_rvec.create(3, 1, mtype);
_tvec.create(3, 1, mtype);
}
rvec = _rvec.getMat();
tvec = _tvec.getMat();
Mat cameraMatrix0 = _cameraMatrix.getMat();
Mat distCoeffs0 = _distCoeffs.getMat();
Mat cameraMatrix = Mat_<double>(cameraMatrix0);
Mat distCoeffs = Mat_<double>(distCoeffs0);
bool result = false;
if (flags == SOLVEPNP_EPNP || flags == SOLVEPNP_DLS || flags == SOLVEPNP_UPNP)
{
Mat undistortedPoints;
undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
epnp PnP(cameraMatrix, opoints, undistortedPoints);
Mat R;
PnP.compute_pose(R, tvec);
Rodrigues(R, rvec);
result = true;
}
else if (flags == SOLVEPNP_P3P)
{
CV_Assert( npoints == 4);
Mat undistortedPoints;
undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
p3p P3Psolver(cameraMatrix);
Mat R;
result = P3Psolver.solve(R, tvec, opoints, undistortedPoints);
if (result)
Rodrigues(R, rvec);
}
else if (flags == SOLVEPNP_AP3P)
{
CV_Assert( npoints == 4);
Mat undistortedPoints;
undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
ap3p P3Psolver(cameraMatrix);
Mat R;
result = P3Psolver.solve(R, tvec, opoints, undistortedPoints);
if (result)
Rodrigues(R, rvec);
}
else if (flags == SOLVEPNP_ITERATIVE)
{
CvMat c_objectPoints = opoints, c_imagePoints = ipoints;
CvMat c_cameraMatrix = cameraMatrix, c_distCoeffs = distCoeffs;
CvMat c_rvec = rvec, c_tvec = tvec;
cvFindExtrinsicCameraParams2(&c_objectPoints, &c_imagePoints, &c_cameraMatrix,
c_distCoeffs.rows*c_distCoeffs.cols ? &c_distCoeffs : 0,
&c_rvec, &c_tvec, useExtrinsicGuess );
result = true;
}
/*else if (flags == SOLVEPNP_DLS)
{
Mat undistortedPoints;
undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
dls PnP(opoints, undistortedPoints);
Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat();
bool result = PnP.compute_pose(R, tvec);
if (result)
Rodrigues(R, rvec);
return result;
}
else if (flags == SOLVEPNP_UPNP)
{
upnp PnP(cameraMatrix, opoints, ipoints);
Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat();
PnP.compute_pose(R, tvec);
Rodrigues(R, rvec);
return true;
}*/
else
CV_Error(CV_StsBadArg, "The flags argument must be one of SOLVEPNP_ITERATIVE, SOLVEPNP_P3P, SOLVEPNP_EPNP or SOLVEPNP_DLS");
return result;
}
void CalcObjectLocation::calculate() {
CLOG(LDEBUG)<<"in calculate()";
Types::HomogMatrix homogMatrix;
vector<cv::Mat_<double> > rvec;
vector<cv::Mat_<double> > tvec;
vector<cv::Mat_<double> > axis;
vector<double> fi;
cv::Mat_<double> tvectemp;
cv::Mat_<double> rotMatrix;
rotMatrix = cv::Mat_<double>::zeros(3,3);
tvectemp = cv::Mat_<double>::zeros(3,1);
while (!in_homogMatrix.empty()) {
cv::Mat_<double> rvectemp;
homogMatrix=in_homogMatrix.read();
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
rotMatrix(i,j)=homogMatrix.elements[i][j];
}
tvectemp(i, 0) = homogMatrix.elements[i][3];
}
Rodrigues(rotMatrix, rvectemp);
CLOG(LINFO) << rvectemp << "\n";
rvec.push_back(rvectemp);
tvec.push_back(tvectemp);
}
if (rvec.size() == 1) {
out_homogMatrix.write(homogMatrix);
return;
}
float fi_sum=0, fi_avg;
cv::Mat_<double> axis_sum, axis_avg;
cv::Mat_<double> rvec_avg;
cv::Mat_<double> tvec_avg, tvec_sum;
axis_sum = cv::Mat_<double>::zeros(3,1);
tvec_sum = cv::Mat_<double>::zeros(3,1);
for(int i = 0; i < rvec.size(); i++) {
float fitmp = sqrt((pow(rvec.at(i)(0,0), 2) + pow(rvec.at(i)(1,0), 2)+pow(rvec.at(i)(2,0),2)));
fi.push_back(fitmp);
fi_sum+=fitmp;
cv::Mat_<double> axistemp;
axistemp.create(3,1);
for(int k=0;k<3;k++) {
axistemp(k,0)=rvec.at(i)(k,0)/fitmp;
}
axis.push_back(axistemp);
axis_sum+=axistemp;
tvec_sum+=tvec.at(i);
}
fi_avg = fi_sum/fi.size();
axis_avg = axis_sum/axis.size();
rvec_avg = axis_avg * fi_avg;
tvec_avg = tvec_sum/tvec.size();
Types::HomogMatrix hm;
cv::Mat_<double> rottMatrix;
Rodrigues(rvec_avg, rottMatrix);
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
hm.elements[i][j] = rottMatrix(i, j);
CLOG(LINFO) << hm.elements[i][j] << " ";
}
hm.elements[i][3] = tvec_avg(i, 0);
CLOG(LINFO) << hm.elements[i][3] << "\n";
}
out_homogMatrix.write(hm);
}
bool ReconstructionHandler::initializeStereoModel(std::vector<cv::Point3d> &points, std::vector<cv::Vec3b> &pointsRGB, int imageInitializedCnt){
// Camera parameters - necessary for reconstruction
cv::Matx34d P0,P1;
cv::Mat_<double> t;
cv::Mat_<double> R;
cv::Mat_<double> rvec(1,3);
// Set arbitrary parameters - will be refined
P0 = cv::Matx34d(1,0,0,0,
0,1,0,0,
0,0,1,0);
P1 = cv::Matx34d(1,0,0,0,
0,1,0,0,
0,0,1,0);
// Attempt to find baseline traingulation
LOG(Debug, "Trying to find baseline triangulation...");
// Find best two images to start with
std::vector<CloudPoint> tmpPcloud;
// Sort pairwise matches to find the lowest Homography inliers [Snavely07 4.2]
// Take less matches tan Snavely suggests...
std::list<std::pair<int,std::pair<int,int> > > matchesSizes;
// For debugging
std::map<std::pair<int,int> ,std::vector<cv::DMatch> > tempMatches = sceneModel->getMatches();
for(std::map<std::pair<int,int> ,std::vector<cv::DMatch> >::iterator i = tempMatches.begin(); i != tempMatches.end(); ++i) {
if((*i).second.size() < 60)
matchesSizes.push_back(make_pair(100,(*i).first));
else {
int Hinliers = findHomographyInliers2Views((*i).first.first,(*i).first.second);
int percent = (int)(((double)Hinliers) / ((double)(*i).second.size()) * 100.0);
LOG(Info, "The percentage of inliers for images ", (*i).first.first, "," , (*i).first.second, " = ", percent);
matchesSizes.push_back(make_pair((int)percent,(*i).first));
}
}
matchesSizes.sort(sortByFirst);
// Reconstruct from two views
bool goodF = false;
int highest_pair = 0;
m_first_view = m_second_view = 0;
// Reverse iterate by number of matches
for(std::list<std::pair<int,std::pair<int,int> > >::iterator highest_pair = matchesSizes.begin();
highest_pair != matchesSizes.end() && !goodF;
++highest_pair)
{
m_second_view = (*highest_pair).second.second;
m_first_view = (*highest_pair).second.first;
LOG(Debug, "Attempting reconstruction from view ", m_first_view, " and ", m_second_view);
// If reconstrcution of first two views is bad, fallback to another pair
// See if the Fundamental Matrix between these two views is good
std::vector<cv::KeyPoint> keypoints1Refined;
std::vector<cv::KeyPoint> keypoints2Refined;
goodF = findCameraMatrices(sceneModel->K, sceneModel->Kinv, sceneModel->distortionCoefficients,
sceneModel->getKeypoints(m_first_view), sceneModel->getKeypoints(m_second_view),
keypoints1Refined, keypoints2Refined, P0, P1, sceneModel->getMatches(m_first_view,m_second_view),
tmpPcloud);
if (goodF) {
std::vector<CloudPoint> new_triangulated;
std::vector<int> add_to_cloud;
sceneModel->poseMats[m_first_view] = P0;
sceneModel->poseMats[m_second_view] = P1;
// TODO imageInitialzedCnt used to initalize correspondence matching vector
// Will fail if more images are added later...
bool good_triangulation = triangulatePointsBetweenViews(m_second_view,m_first_view,new_triangulated,add_to_cloud, imageInitializedCnt);
if(!good_triangulation || cv::countNonZero(add_to_cloud) < 10) {
LOG(Debug, " Triangulation failed");
goodF = false;
sceneModel->poseMats[m_first_view] = 0;
sceneModel->poseMats[m_second_view] = 0;
m_second_view++;
} else {
assert(new_triangulated[0].imgpt_for_img.size() > 0);
LOG(Debug, " Points before triangulation: ", (int)sceneModel->reconstructedPts.size());
for (unsigned int j=0; j<add_to_cloud.size(); j++) {
if(add_to_cloud[j] == 1) {
sceneModel->reconstructedPts.push_back(new_triangulated[j]);
}
}
LOG(Debug, " Points after triangulation: ", (int)sceneModel->reconstructedPts.size());
}
}
}
if (!goodF) {
LOG(Error, "Cannot find a good pair of images to obtain a baseline triangulation");
return false;
}
LOG(Debug, "===================================================================");
LOG(Debug, "Taking baseline from " , m_first_view , " and " , m_second_view);
LOG(Debug, "===================================================================");
// Adjust bundle for everything so far before display
cv::Mat temporaryCameraMatrix = sceneModel->K;
BundleAdjustment BA;
BA.adjustBundle(sceneModel->reconstructedPts,temporaryCameraMatrix,sceneModel->getKeypoints(),sceneModel->poseMats);
updateReprojectionErrors();
#ifdef __DEBUG__DISPLAY__
// DEBUG - Drawing matches that survived the fundamental matrix
cv::drawMatches(sceneModel->frames[0], sceneModel->getKeypoints(0),
sceneModel->frames[1], sceneModel->getKeypoints(1),
sceneModel->getMatches(0,1), img_matches, cv::Scalar(0,0,255));
imshow( "Good Matches After F Refinement", img_matches );
cv::waitKey(0);
#endif
// Pass reconstructed points to 3D display
// TODO unnecessary copying
for (int i = 0; i < sceneModel->reconstructedPts.size(); ++i) {
points.push_back(sceneModel->reconstructedPts[i].pt);
}
getRGBForPointCloud(sceneModel->reconstructedPts, pointsRGB);
// End of stereo initialization
LOG(Debug, "Initialized stereo model");
P1 = sceneModel->poseMats[m_second_view];
t = (cv::Mat_<double>(1,3) << P1(0,3), P1(1,3), P1(2,3));
R = (cv::Mat_<double>(3,3) << P1(0,0), P1(0,1), P1(0,2),
P1(1,0), P1(1,1), P1(1,2),
P1(2,0), P1(2,1), P1(2,2));
Rodrigues(R, rvec);
// Update sets which track what was processed
sceneModel->doneViews.clear();
sceneModel->goodViews.clear();
sceneModel->doneViews.insert(m_first_view);
sceneModel->doneViews.insert(m_second_view);
sceneModel->goodViews.insert(m_first_view);
sceneModel->goodViews.insert(m_second_view);
return true;
}
void estimatePose(vector<Point3d> modelPoints, vector<Point2f> imagePoints, Point2f centerPoint, double (cameraMatrix)[16], double (&resultMatrix)[16] ){
Mat rvec, tvec, rMat;
double rot[9] = { 0 };
vector<double> rv(3), tv(3);
rvec = Mat(rv);
double _d[9] = { 1, 0, 0, 0, -1, 0, 0, 0, -1 };
Rodrigues(Mat(3, 3, CV_64FC1, _d), rvec);
tv[0] = 0;
tv[1] = 0;
tv[2] = 1;
tvec = Mat(tv);
// Camera matrix
double _cm[9] = { 3.1810194786433851e+02, 0., 2.0127743042733985e+02, 0.,
3.1880182087777166e+02, 1.2606640793496385e+02, 0., 0., 1 };
// Distortion coefficients
double _dc[] = { 0, 0, 0, 0 };
Mat camMatrix = Mat(3, 3, CV_64FC1, _cm);
solvePnP(Mat(modelPoints), Mat(imagePoints), camMatrix, Mat(1, 4,
CV_64FC1, _dc), rvec, tvec, true);
rMat = Mat(3, 3, CV_64FC1, rot);
Rodrigues(rvec, rMat);
tvec.at<double> (0, 0) += 2.0127743042733985e+02 + 4.69099e-02;
tvec.at<double> (1, 0) += 1.2606640793496385e+02 - 2.5968e-02;
tvec.at<double> (2, 0) -= 3.1810194786433851e+02 + 2.31789e-01;
resultMatrix[0] = rMat.at<double> (0, 0);//*scale;
resultMatrix[1] = -rMat.at<double> (1, 0);
resultMatrix[2] = -rMat.at<double> (2, 0);
resultMatrix[3] = 0;
resultMatrix[4] = rMat.at<double> (0, 1);
resultMatrix[5] = -rMat.at<double> (1, 1);//*scale;
resultMatrix[6] = -rMat.at<double> (2, 1);
resultMatrix[7] = 0;
resultMatrix[8] = rMat.at<double> (0, 2);
resultMatrix[9] = -rMat.at<double> (1, 2);
resultMatrix[10] = -rMat.at<double> (2, 2);//*scale;
resultMatrix[11] = 0;
resultMatrix[12] = 4.0f*(4.0f/3.0f)*(5.0f/3.0f)*(centerPoint.x - 200)/400;
resultMatrix[13] = -4.0f*(4.0f/3.0f)*1.0f*(centerPoint.y - 120)/240;
resultMatrix[14] = -4.0f;
resultMatrix[15] = 1.0f;
}
void solvePlanarPnP(const Mat& objectPoints, const Mat& imagePoints, const Mat& cameraMatrix, const Mat& distCoeffs,
Mat& _rvec, Mat& _tvec, bool useExtrinsicGuess)
{
CV_Assert(objectPoints.depth() == CV_32F && imagePoints.depth() == CV_32F);
if(useExtrinsicGuess == false)
{
solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, _rvec, _tvec, false);
return;
}
Mat rvec, tvec;
_rvec.convertTo(rvec, CV_32FC1);
_tvec.convertTo(tvec, CV_32FC1);
// calculate rotation matrix
Mat R(3, 3, CV_32FC1);
Rodrigues(rvec, R);
CV_Assert(R.type() == CV_32FC1);
// calculate object normal
Point3f normal0 = getPlanarObjectNormal(objectPoints);
// printf("Normal0: %f %f %f\n", normal0.x, normal0.y, normal0.z);
Mat Normal0(3, 1, CV_32F);
Normal0.at<Point3f>(0, 0) = normal0;
Mat Normal = R*Normal0;
Point3f normal = Normal.at<Point3f>(0, 0);
normal = normal*(1.0/norm(normal));
if(normal.z < 0) normal = -normal; // z points from the camera
// printf("Normal: %f %f %f\n", normal.x, normal.y, normal.z);
vector<Point3f> rotated_object_points;
rotated_object_points.resize(objectPoints.rows);
for(size_t i = 0; i < rotated_object_points.size(); i++)
{
Mat p = objectPoints.rowRange(i, i + 1);
p = p.reshape(1, 3);
Mat res = R*p;
rotated_object_points[i] = res.at<Point3f>(0, 0);
}
double alpha, C;
vector<Point3f> object_points_crf;
findPlanarObjectPose(rotated_object_points, imagePoints, normal, cameraMatrix, distCoeffs, alpha, C, object_points_crf);
Mat rp(3, 1, CV_32FC1);
rp.at<Point3f>(0, 0) = normal*alpha;
Mat Rp;
Rodrigues(rp, Rp);
R = Rp*R;
Rodrigues(R, rvec);
Point3f center1 = massCenter(rotated_object_points);
Mat mcenter1(3, 1, CV_32FC1, ¢er1);
Mat mcenter1_alpha = Rp*mcenter1;
Point3f center1_alpha = mcenter1_alpha.at<Point3f>(0, 0);
Point3f center2 = massCenter(object_points_crf);
tvec.at<Point3f>(0, 0) = center2 - center1_alpha;
Mat mobj;
objectPoints.copyTo(mobj);
mobj = mobj.reshape(1);
CV_Assert(R.type() == CV_32FC1 && mobj.type() == CV_32FC1);
Mat mrobj = R*mobj.t();
mrobj = mrobj.t();
Point3f p1 = mrobj.at<Point3f>(0, 0) + center2 - center1;
Point3f p2 = object_points_crf[0];
// printf("point1: %f %f %f\n", p1.x, p1.y, p1.z);
// printf("point2: %f %f %f\n", p2.x, p2.y, p2.z);
rvec.convertTo(_rvec, _rvec.depth());
tvec.convertTo(_tvec, _tvec.depth());
}
void stream(const double& dt)
{
u1 = Rodrigues(w1 * dt) * Vector(u1);
u2 = Rodrigues(w2 * dt) * Vector(u2);
r12 += v12 * dt;
}
