double Distance( const Conic& c1, const Conic& c2, double circle_radius )
{
const Matrix3d Q = c1.Dual * c2.C;
// const double dsq = 3 - trace(Q)* pow(determinant(Q),-1.0/3.0);
const double dsq = 3 - Q.trace()* 1.0 / cbrt(Q.determinant());
return sqrt(dsq) * circle_radius;
}
TEST(Rotation, TestEulerAnglesAndMatrices) {
// Randomly generate euler angles, convert to a matrix, then convert back.
// Check that (1) the rotation matrix has det(R)==1, and (2), that we get the
// same angles back.
math::RandomGenerator rng(0);
for (int ii = 0; ii < 1000; ++ii) {
Vector3d e1;
e1.setRandom();
// Converting from rotation matrices to euler angles is only valid when phi,
// theta, and psi are all < 0.5*PI. Otherwise the problem has multiple
// solutions, and we can only return one of them with our function.
e1 *= 0.5 * M_PI;
Matrix3d R = EulerAnglesToMatrix(e1);
EXPECT_NEAR(1.0, R.determinant(), 1e-8);
Vector3d e2 = MatrixToEulerAngles(R);
EXPECT_NEAR(0.0, S1Distance(e1(0), e2(0)), 1e-8);
EXPECT_NEAR(0.0, S1Distance(e1(1), e2(1)), 1e-8);
EXPECT_NEAR(0.0, S1Distance(e1(2), e2(2)), 1e-8);
EXPECT_TRUE(R.isApprox(EulerAnglesToMatrix(e2), 1e-4));
}
}
double JacobianEva::evaluate(HexVertex *hv, HexVertex* nhv[3]) {
Matrix3d J = Matrix3d::Zero();
for (int i = 0; i < 3; ++i) {
Point pt = nhv[i]->point() - hv->point();
pt /= pt.norm();
for (int j = 0; j < 3; ++j) {
J(j, i) = pt[j];
}
}
return J.determinant();
}
Vector4d rigidBodyDynamics::quaternionFromRot(Matrix3d& R) const{
Vector4d q;
double div1, div2, div3, div4;
double numerical_limit = 1.0e-4;
if (abs(R.determinant()-1) > numerical_limit ) {
std::cerr << "R does not have a determinant of +1" << std::endl;
} else {
div1 = 0.5*sqrt(1+R(0,0)+R(1,1)+R(2,2));
div2 = 0.5*sqrt(1+R(0,0)-R(1,1)-R(2,2));
div3 = 0.5*sqrt(1-R(0,0)-R(1,1)+R(2,2));
div4 = 0.5*sqrt(1-R(0,0)+R(1,1)-R(2,2));
//if (div1 > div2 && div1 > div3 && div1 > div4) {
if (fabs(div1) > numerical_limit) {
q(3) = div1;
q(0) = 0.25*(R(1,2)-R(2,1))/q(3);
q(1) = 0.25*(R(2,0)-R(0,2))/q(3);
q(2) = 0.25*(R(0,1)-R(1,0))/q(3);
} else if (fabs(div2) > numerical_limit) {
//} else if (div2 > div1 && div2 > div3 && div2 > div4) {
q(0) = div2;
q(1) = 0.25*(R(0,1)+R(1,0))/q(0);
q(2) = 0.25*(R(0,2)+R(2,0))/q(0);
q(3) = 0.25*(R(1,2)+R(2,1))/q(0);
} else if (fabs(div3) > numerical_limit) {
//} else if (div3 > div1 && div3 > div2 && div3 > div4) {
q(2) = div3;
q(0) = 0.25*(R(0,2)+R(2,0))/q(2);
q(1) = 0.25*(R(1,2)+R(2,1))/q(2);
q(3) = 0.25*(R(0,1)-R(1,0))/q(2);
//} else {
} else if (fabs(div4) > numerical_limit) {
q(1) = div4;
q(0) = 0.25*(R(0,1)+R(1,0))/q(1);
q(2) = 0.25*(R(1,2)+R(2,1))/q(1);
q(3) = 0.25*(R(2,0)-R(0,2))/q(1);
} else {
std::cerr << "quaternionFromRot didn't convert: [" << div1 << ", " << div2 << ", " << div3 << ", " << div4 << std::endl;
std::cerr << "Rotation Matrix: " << R << std::endl;
}
}
/*
if (q(3) < 0) {
q *= -1;
}
*/
q /=q.norm();
return q;
}
// Original code from the ROS vslam package pe3d.cpp
// uses the SVD procedure for aligning point clouds
// SEE: Arun, Huang, Blostein: Least-Squares Fitting of Two 3D Point Sets
Eigen::Matrix4d threePointSvd(Eigen::MatrixXd const & p0, Eigen::MatrixXd const & p1)
{
using namespace Eigen;
SM_ASSERT_EQ_DBG(std::runtime_error, p0.rows(), 3, "p0 must be a 3xK matrix");
SM_ASSERT_EQ_DBG(std::runtime_error, p1.rows(), 3, "p1 must be a 3xK matrix");
SM_ASSERT_EQ_DBG(std::runtime_error, p0.cols(), p1.cols(), "p0 and p1 must have the same number of columns");
Vector3d c0 = p0.rowwise().mean();
Vector3d c1 = p1.rowwise().mean();
Matrix3d H(Matrix3d::Zero());
// subtract out
// p0a -= c0;
// p0b -= c0;
// p0c -= c0;
// p1a -= c1;
// p1b -= c1;
// p1c -= c1;
// Matrix3d H = p1a*p0a.transpose() + p1b*p0b.transpose() +
// p1c*p0c.transpose();
for(int i = 0; i < p0.cols(); ++i)
{
H += (p0.col(i) - c0) * (p1.col(i) - c1).transpose();
}
// do the SVD thang
JacobiSVD<Matrix3d> svd(H,ComputeFullU | ComputeFullV);
Matrix3d V = svd.matrixV();
Matrix3d R = V * svd.matrixU().transpose();
double det = R.determinant();
if (det < 0.0)
{
V.col(2) = V.col(2) * -1.0;
R = V * svd.matrixU().transpose();
}
Vector3d tr = c0-R.transpose()*c1; // translation
// transformation matrix, 3x4
Matrix4d tfm(Matrix4d::Identity());
// tfm.block<3,3>(0,0) = R.transpose();
// tfm.col(3) = -R.transpose()*tr;
tfm.topLeftCorner<3,3>() = R.transpose();
tfm.topRightCorner<3,1>() = tr;
return tfm;
}
void CameraDirectLinearTransformation::decomposePMatrix(const Eigen::Matrix<double,3,4> &P)
{
Matrix3d M = P.topLeftCorner<3,3>();
Vector3d m3 = M.row(2).transpose();
// Follow the HartleyZisserman - "Multiple view geometry in computer vision" implementation chapter 3
Matrix3d P123,P023,P013,P012;
P123 << P.col(1),P.col(2),P.col(3);
P023 << P.col(0),P.col(2),P.col(3);
P013 << P.col(0),P.col(1),P.col(3);
P012 << P.col(0),P.col(1),P.col(2);
double X = P123.determinant();
double Y = -P023.determinant();
double Z = P013.determinant();
double T = -P012.determinant();
C << X/T,Y/T,Z/T;
// Image Principal points computed with homogeneous normalization
this->principalPoint = (M*m3).eval().hnormalized().head<2>();
// Principal vector from the camera centre C through pp pointing out from the camera. This may not be the same as R(:,3)
// if the principal point is not at the centre of the image, but it should be similar.
this->principalVector = (M.determinant()*m3).normalized();
this->R = this->K = Matrix3d::Identity();
this->rq3(M,this->K,this->R);
// To understand how K is formed http://ksimek.github.io/2013/08/13/intrinsic/
// and also read http://ksimek.github.io/2012/08/14/decompose/
K/=K(2,2); // EXTREMELY IMPORTANT TO MAKE THE K(2,2)==1 !!!
// K = [ fx, s, x0;
// 0, fy, y0;
// 0, 0, 1];
// Where fx is the focal length on x measured in pixels, fy is the focal length ony measured in pixels
// Negate the second column of K and R because the y window coordinates and camera y direction are opposite is positive
// This is the solution I've found personally to correct the behaviour using OpenGL gluPerspective convention
this->R.row(2)=-R.row(2);
// Our 3x3 intrinsic camera matrix K needs two modifications before it's ready to use in OpenGL. First, for proper clipping, the (3,3) element of K must be -1. OpenGL's camera looks down the negative z-axis, so if K33 is positive, vertices in front of the camera will have a negative w coordinate after projection. In principle, this is okay, but because of how OpenGL performs clipping, all of these points will be clipped.
//this->K.col(2) = -K.col(2);
// t is the location of the world origin in camera coordinates.
t = -R*C;
}
void ConsState::prepareOutputs()
{
Matrix3d rhoF;
ConsState temp = *this;
rhoF << v[3], v[4], v[5],
v[6], v[7], v[8],
v[9], v[10], v[11];
/* rho = pow(rhoF.determinant()/2., 2.); */
rho = sqrt(rhoF.determinant()/rho_0);
//deformation gradient
Matrix3d F = rhoF/rho;
// Calculate finger tensor
Matrix3d G = strainTensor(F);
//calculate invariants
/* cout << B_0 << endl; */
invariants(G);
dInvariants_G();
//Stress
stress(G);
}
Matrix3d
getOrientationAndCentriods(Vector3d & p0a,
Vector3d & p0b,
Vector3d & p0c,
Vector3d & p1a,
Vector3d & p1b,
Vector3d & p1c,
Vector3d & c0,
Vector3d & c1)
{
c0 = (p0a+p0b+p0c)*(1.0/3.0);
c1 = (p1a+p1b+p1c)*(1.0/3.0);
// subtract out
p0a -= c0;
p0b -= c0;
p0c -= c0;
p1a -= c1;
p1b -= c1;
p1c -= c1;
Matrix3d H = p1a*p0a.transpose() + p1b*p0b.transpose() +
p1c*p0c.transpose();
JacobiSVD<Matrix3d> svd(H, ComputeFullU | ComputeFullV);
Matrix3d V = svd.matrixV();
Matrix3d R;
R = V * svd.matrixU().transpose();
double det = R.determinant();
if (det < 0.0)
{
V.col(2) = V.col(2)*-1.0;
R = V * svd.matrixU().transpose();
}
return R;
}
bool Homography::
decompose()
{
decompositions.clear();
JacobiSVD<MatrixXd> svd(H_c2_from_c1, ComputeThinU | ComputeThinV);
Vector3d singular_values = svd.singularValues();
double d1 = fabs(singular_values[0]); // The paper suggests the square of these (e.g. the evalues of AAT)
double d2 = fabs(singular_values[1]); // should be used, but this is wrong. c.f. Faugeras' book.
double d3 = fabs(singular_values[2]);
Matrix3d U = svd.matrixU();
Matrix3d V = svd.matrixV(); // VT^T
double s = U.determinant() * V.determinant();
double dPrime_PM = d2;
int nCase;
if(d1 != d2 && d2 != d3)
nCase = 1;
else if( d1 == d2 && d2 == d3)
nCase = 3;
else
nCase = 2;
if(nCase != 1)
{
printf("FATAL Homography Initialization: This motion case is not implemented or is degenerate. Try again. ");
return false;
}
double x1_PM;
double x2;
double x3_PM;
// All below deals with the case = 1 case.
// Case 1 implies (d1 != d3)
{ // Eq. 12
x1_PM = sqrt((d1*d1 - d2*d2) / (d1*d1 - d3*d3));
x2 = 0;
x3_PM = sqrt((d2*d2 - d3*d3) / (d1*d1 - d3*d3));
};
double e1[4] = {1.0,-1.0, 1.0,-1.0};
double e3[4] = {1.0, 1.0,-1.0,-1.0};
Vector3d np;
HomographyDecomposition decomp;
// Case 1, d' > 0:
decomp.d = s * dPrime_PM;
for(size_t signs=0; signs<4; signs++)
{
// Eq 13
decomp.R = Matrix3d::Identity();
double dSinTheta = (d1 - d3) * x1_PM * x3_PM * e1[signs] * e3[signs] / d2;
double dCosTheta = (d1 * x3_PM * x3_PM + d3 * x1_PM * x1_PM) / d2;
decomp.R(0,0) = dCosTheta;
decomp.R(0,2) = -dSinTheta;
decomp.R(2,0) = dSinTheta;
decomp.R(2,2) = dCosTheta;
// Eq 14
decomp.t[0] = (d1 - d3) * x1_PM * e1[signs];
decomp.t[1] = 0.0;
decomp.t[2] = (d1 - d3) * -x3_PM * e3[signs];
np[0] = x1_PM * e1[signs];
np[1] = x2;
np[2] = x3_PM * e3[signs];
decomp.n = V * np;
decompositions.push_back(decomp);
}
// Case 1, d' < 0:
decomp.d = s * -dPrime_PM;
for(size_t signs=0; signs<4; signs++)
{
// Eq 15
decomp.R = -1 * Matrix3d::Identity();
double dSinPhi = (d1 + d3) * x1_PM * x3_PM * e1[signs] * e3[signs] / d2;
double dCosPhi = (d3 * x1_PM * x1_PM - d1 * x3_PM * x3_PM) / d2;
decomp.R(0,0) = dCosPhi;
decomp.R(0,2) = dSinPhi;
decomp.R(2,0) = dSinPhi;
decomp.R(2,2) = -dCosPhi;
// Eq 16
decomp.t[0] = (d1 + d3) * x1_PM * e1[signs];
decomp.t[1] = 0.0;
decomp.t[2] = (d1 + d3) * x3_PM * e3[signs];
np[0] = x1_PM * e1[signs];
np[1] = x2;
np[2] = x3_PM * e3[signs];
decomp.n = V * np;
decompositions.push_back(decomp);
}
// Save rotation and translation of the decomposition
for(unsigned int i=0; i<decompositions.size(); i++)
{
Matrix3d R = s * U * decompositions[i].R * V.transpose();
Vector3d t = U * decompositions[i].t;
decompositions[i].T = Sophus::SE3(R, t);
}
return true;
}
// assumes cv::Mats are of type <float>
int PoseEstimator::estimate(const matches_t &matches,
const cv::Mat &train_kpts, const cv::Mat &query_kpts,
const cv::Mat &train_pts, const cv::Mat &query_pts,
const cv::Mat &K, const double baseline)
{
// make sure we're using floats
if (train_kpts.depth() != CV_32F ||
query_kpts.depth() != CV_32F ||
train_pts.depth() != CV_32F ||
query_pts.depth() != CV_32F)
throw std::runtime_error("Expected input to be of floating point (CV_32F)");
int nmatch = matches.size();
cout << endl << "[pe3d] Matches: " << nmatch << endl;
// set up data structures for fast processing
// indices to good matches
std::vector<Eigen::Vector3f> t3d, q3d;
std::vector<Eigen::Vector2f> t2d, q2d;
std::vector<int> m; // keep track of matches
for (int i=0; i<nmatch; i++)
{
const float* ti = train_pts.ptr<float>(matches[i].trainIdx);
const float* qi = query_pts.ptr<float>(matches[i].queryIdx);
const float* ti2 = train_kpts.ptr<float>(matches[i].trainIdx);
const float* qi2 = query_kpts.ptr<float>(matches[i].queryIdx);
// make sure we have points within range; eliminates NaNs as well
if (matches[i].distance < 60 && // TODO: get this out of the estimator
ti[2] < maxMatchRange &&
qi[2] < maxMatchRange)
{
m.push_back(i);
t2d.push_back(Eigen::Vector2f(ti2[0],ti2[1]));
q2d.push_back(Eigen::Vector2f(qi2[0],qi2[1]));
t3d.push_back(Eigen::Vector3f(ti[0],ti[1],ti[2]));
q3d.push_back(Eigen::Vector3f(qi[0],qi[1],qi[2]));
}
}
nmatch = m.size();
cout << "[pe3d] Filtered matches: " << nmatch << endl;
if (nmatch < 3) return 0; // can't do it...
int bestinl = 0;
Matrix3f Kf;
cv::cv2eigen(K,Kf); // camera matrix
float fb = Kf(0,0)*baseline; // focal length times baseline
float maxInlierXDist2 = maxInlierXDist*maxInlierXDist;
float maxInlierDDist2 = maxInlierDDist*maxInlierDDist;
// set up minimum hyp pixel distance
float minpdist = minPDist * Kf(0,2) * 2.0;
// RANSAC loop
int numchoices = 1 + numRansac / 10;
for (int ii=0; ii<numRansac; ii++)
{
// find a candidate
int k;
int a=rand()%nmatch;
int b;
for (k=0; k<numchoices; k++)
{
b=rand()%nmatch;
if (a!=b && (t2d[a]-t2d[b]).norm() > minpdist
&& (q2d[a]-q2d[b]).norm() > minpdist)
// TODO: add distance check
break;
}
if (k >= numchoices) continue;
int c;
for (k=0; k<numchoices+numchoices; k++)
{
c=rand()%nmatch;
if (c!=b && c!=a && (t2d[a]-t2d[c]).norm() > minpdist
&& (t2d[b]-t2d[c]).norm() > minpdist
&& (q2d[a]-q2d[c]).norm() > minpdist
&& (q2d[b]-q2d[c]).norm() > minpdist)
// TODO: add distance check
break;
}
if (k >= numchoices+numchoices) continue;
// get centroids
Vector3f p0a = t3d[a];
Vector3f p0b = t3d[b];
Vector3f p0c = t3d[c];
Vector3f p1a = q3d[a];
Vector3f p1b = q3d[b];
Vector3f p1c = q3d[c];
Vector3f c0 = (p0a+p0b+p0c)*(1.0/3.0);
Vector3f c1 = (p1a+p1b+p1c)*(1.0/3.0);
// subtract out
p0a -= c0;
p0b -= c0;
p0c -= c0;
p1a -= c1;
p1b -= c1;
p1c -= c1;
Matrix3f Hf = p1a*p0a.transpose() + p1b*p0b.transpose() +
p1c*p0c.transpose();
Matrix3d H = Hf.cast<double>();
#if 0
cout << p0a.transpose() << endl;
cout << p0b.transpose() << endl;
cout << p0c.transpose() << endl;
#endif
// do the SVD thang
JacobiSVD<Matrix3d> svd(H, ComputeFullU | ComputeFullV);
Matrix3d V = svd.matrixV();
Matrix3d R = V * svd.matrixU().transpose();
double det = R.determinant();
//ntot++;
if (det < 0.0)
{
//nneg++;
V.col(2) = V.col(2)*-1.0;
R = V * svd.matrixU().transpose();
}
Vector3d cd0 = c0.cast<double>();
Vector3d cd1 = c1.cast<double>();
Vector3d tr = cd0-R*cd1; // translation
// cout << "[PE test] R: " << endl << R << endl;
// cout << "[PE test] t: " << tr.transpose() << endl;
Vector3f trf = tr.cast<float>(); // convert to floats
Matrix3f Rf = R.cast<float>();
Rf = Kf*Rf;
trf = Kf*trf;
// find inliers, based on image reprojection
int inl = 0;
for (int i=0; i<nmatch; i++)
{
const Vector3f &pt1 = q3d[i];
Vector3f ipt = Rf*pt1+trf; // transform query point
float iz = 1.0/ipt.z();
Vector2f &kp = t2d[i];
// cout << kp.transpose() << " " << pt1.transpose() << " " << ipt.transpose() << endl;
float dx = kp.x() - ipt.x()*iz;
float dy = kp.y() - ipt.y()*iz;
float dd = fb/t3d[i].z() - fb/ipt.z(); // diff of disparities, could pre-compute t3d
if (dx*dx < maxInlierXDist2 && dy*dy < maxInlierXDist2
&& dd*dd < maxInlierDDist2)
// inl+=(int)fsqrt(ipt.z()); // clever way to weight closer points
inl++;
}
if (inl > bestinl)
{
bestinl = inl;
trans = tr.cast<float>(); // convert to floats
rot = R.cast<float>();
}
}
cout << "[pe3d] Best inliers: " << bestinl << endl;
// printf("Total ransac: %d Neg det: %d\n", ntot, nneg);
// reduce matches to inliers
matches_t inls; // temporary for current inliers
inliers.clear();
Matrix3f Rf = Kf*rot;
Vector3f trf = Kf*trans;
// cout << "[pe3e] R: " << endl << rot << endl;
cout << "[pe3d] t: " << trans.transpose() << endl;
AngleAxisf aa;
aa.fromRotationMatrix(rot);
cout << "[pe3d] AA: " << aa.angle()*180.0/M_PI << " " << aa.axis().transpose() << endl;
for (int i=0; i<nmatch; i++)
{
Vector3f &pt1 = q3d[i];
Vector3f ipt = Rf*pt1+trf; // transform query point
float iz = 1.0/ipt.z();
Vector2f &kp = t2d[i];
// cout << kp.transpose() << " " << pt1.transpose() << " " << ipt.transpose() << endl;
float dx = kp.x() - ipt.x()*iz;
float dy = kp.y() - ipt.y()*iz;
float dd = fb/t3d[i].z() - fb/ipt.z(); // diff of disparities, could pre-compute t3d
if (dx*dx < maxInlierXDist2 && dy*dy < maxInlierXDist2 &&
dd*dd < maxInlierDDist2)
inls.push_back(matches[m[i]]);
}
cout << "[pe3d] Final inliers: " << inls.size() << endl;
// polish with SVD
if (polish)
{
Matrix3d Rd = rot.cast<double>();
Vector3d trd = trans.cast<double>();
StereoPolish pol(5,false);
pol.polish(inls,train_kpts,query_kpts,train_pts,query_pts,K,baseline,
Rd,trd);
AngleAxisd aa;
aa.fromRotationMatrix(Rd);
cout << "[pe3d] Polished t: " << trd.transpose() << endl;
cout << "[pe3d] Polished AA: " << aa.angle()*180.0/M_PI << " " << aa.axis().transpose() << endl;
int num = inls.size();
// get centroids
Vector3f c0(0,0,0);
Vector3f c1(0,0,0);
for (int i=0; i<num; i++)
{
c0 += t3d[i];
c1 += q3d[i];
}
c0 = c0 / (float)num;
c1 = c1 / (float)num;
// subtract out and create H matrix
Matrix3f Hf;
Hf.setZero();
for (int i=0; i<num; i++)
{
Vector3f p0 = t3d[i]-c0;
Vector3f p1 = q3d[i]-c1;
Hf += p1*p0.transpose();
}
Matrix3d H = Hf.cast<double>();
// do the SVD thang
JacobiSVD<Matrix3d> svd(H, ComputeFullU | ComputeFullV);
Matrix3d V = svd.matrixV();
Matrix3d R = V * svd.matrixU().transpose();
double det = R.determinant();
//ntot++;
if (det < 0.0)
{
//nneg++;
V.col(2) = V.col(2)*-1.0;
R = V * svd.matrixU().transpose();
}
Vector3d cd0 = c0.cast<double>();
Vector3d cd1 = c1.cast<double>();
Vector3d tr = cd0-R*cd1; // translation
aa.fromRotationMatrix(R);
cout << "[pe3d] t: " << tr.transpose() << endl;
cout << "[pe3d] AA: " << aa.angle()*180.0/M_PI << " " << aa.axis().transpose() << endl;
#if 0
// system
SysSBA sba;
sba.verbose = 0;
// set up nodes
// should have a frame => node function
Vector4d v0 = Vector4d(0,0,0,1);
Quaterniond q0 = Quaternion<double>(Vector4d(0,0,0,1));
sba.addNode(v0, q0, f0.cam, true);
Quaterniond qr1(rot); // from rotation matrix
Vector4d temptrans = Vector4d(trans(0), trans(1), trans(2), 1.0);
// sba.addNode(temptrans, qr1.normalized(), f1.cam, false);
qr1.normalize();
sba.addNode(temptrans, qr1, f1.cam, false);
int in = 3;
if (in > (int)inls.size())
in = inls.size();
// set up projections
for (int i=0; i<(int)inls.size(); i++)
{
// add point
int i0 = inls[i].queryIdx;
int i1 = inls[i].trainIdx;
Vector4d pt = query_pts[i0];
sba.addPoint(pt);
// projected point, ul,vl,ur
Vector3d ipt;
ipt(0) = f0.kpts[i0].pt.x;
ipt(1) = f0.kpts[i0].pt.y;
ipt(2) = ipt(0)-f0.disps[i0];
sba.addStereoProj(0, i, ipt);
// projected point, ul,vl,ur
ipt(0) = f1.kpts[i1].pt.x;
ipt(1) = f1.kpts[i1].pt.y;
ipt(2) = ipt(0)-f1.disps[i1];
sba.addStereoProj(1, i, ipt);
}
sba.huber = 2.0;
sba.doSBA(5,10e-4,SBA_DENSE_CHOLESKY);
int nbad = sba.removeBad(2.0);
// cout << endl << "Removed " << nbad << " projections > 2 pixels error" << endl;
sba.doSBA(5,10e-5,SBA_DENSE_CHOLESKY);
// cout << endl << sba.nodes[1].trans.transpose().head(3) << endl;
// get the updated transform
trans = sba.nodes[1].trans.head(3);
Quaterniond q1;
q1 = sba.nodes[1].qrot;
rot = q1.toRotationMatrix();
// set up inliers
inliers.clear();
for (int i=0; i<(int)inls.size(); i++)
{
ProjMap &prjs = sba.tracks[i].projections;
if (prjs[0].isValid && prjs[1].isValid) // valid track
inliers.push_back(inls[i]);
}
#if 0
printf("Inliers: %d After polish: %d\n", (int)inls.size(), (int)inliers.size());
#endif
#endif
}
inliers = inls;
return (int)inls.size();
}
int PoseEstimator3d::estimate(const Frame& f0, const Frame& f1,
const std::vector<cv::DMatch> &matches)
{
// convert keypoints in match to 3d points
std::vector<Vector4d, aligned_allocator<Vector4d> > p0; // homogeneous coordinates
std::vector<Vector4d, aligned_allocator<Vector4d> > p1;
int nmatch = matches.size();
// set up data structures for fast processing
// indices to good matches
vector<int> m0, m1;
for (int i=0; i<nmatch; i++)
{
if (f0.disps[matches[i].queryIdx] > minMatchDisp &&
f1.disps[matches[i].trainIdx] > minMatchDisp)
{
m0.push_back(matches[i].queryIdx);
m1.push_back(matches[i].trainIdx);
}
}
nmatch = m0.size();
if (nmatch < 3) return 0; // can't do it...
int bestinl = 0;
// RANSAC loop
#pragma omp parallel for shared( bestinl )
for (int i=0; i<numRansac; i++)
{
// find a candidate
int a=rand()%nmatch;
int b = a;
while (a==b)
b=rand()%nmatch;
int c = a;
while (a==c || b==c)
c=rand()%nmatch;
int i0a = m0[a];
int i0b = m0[b];
int i0c = m0[c];
int i1a = m1[a];
int i1b = m1[b];
int i1c = m1[c];
if (i0a == i0b || i0a == i0c || i0b == i0c ||
i1a == i1b || i1a == i1c || i1b == i1c)
continue;
// get centroids
Vector3d p0a = f0.pts[i0a].head<3>();
Vector3d p0b = f0.pts[i0b].head<3>();
Vector3d p0c = f0.pts[i0c].head<3>();
Vector3d p1a = f1.pts[i1a].head<3>();
Vector3d p1b = f1.pts[i1b].head<3>();
Vector3d p1c = f1.pts[i1c].head<3>();
Vector3d c0 = (p0a+p0b+p0c)*(1.0/3.0);
Vector3d c1 = (p1a+p1b+p1c)*(1.0/3.0);
// subtract out
p0a -= c0;
p0b -= c0;
p0c -= c0;
p1a -= c1;
p1b -= c1;
p1c -= c1;
Matrix3d H = p1a*p0a.transpose() + p1b*p0b.transpose() +
p1c*p0c.transpose();
#if 0
cout << p0a.transpose() << endl;
cout << p0b.transpose() << endl;
cout << p0c.transpose() << endl;
#endif
// do the SVD thang
JacobiSVD<Matrix3d> svd(H, ComputeFullU | ComputeFullV);
Matrix3d V = svd.matrixV();
Matrix3d R = V * svd.matrixU().transpose();
double det = R.determinant();
//ntot++;
if (det < 0.0)
{
//nneg++;
V.col(2) = V.col(2)*-1.0;
R = V * svd.matrixU().transpose();
}
Vector3d tr = c0-R*c1; // translation
// cout << "[PE test] R: " << endl << R << endl;
// cout << "[PE test] t: " << tr.transpose() << endl;
#if 0
R << 0.9997, 0.002515, 0.02418,
-0.002474, .9999, -0.001698,
-0.02418, 0.001638, 0.9997;
tr << -0.005697, -0.01753, 0.05666;
R = R.transpose();
tr = -R*tr;
#endif
// transformation matrix, 3x4
Matrix<double,3,4> tfm;
// tfm.block<3,3>(0,0) = R.transpose();
// tfm.col(3) = -R.transpose()*tr;
tfm.block<3,3>(0,0) = R;
tfm.col(3) = tr;
// cout << "[PE test] T: " << endl << tfm << endl;
// find inliers, based on image reprojection
int inl = 0;
for (int i=0; i<nmatch; i++)
{
Vector3d pt = tfm*f1.pts[m1[i]];
Vector3d ipt = f0.cam2pix(pt);
const cv::KeyPoint &kp = f0.kpts[m0[i]];
double dx = kp.pt.x - ipt.x();
double dy = kp.pt.y - ipt.y();
double dd = f0.disps[m0[i]] - ipt.z();
if (dx*dx < maxInlierXDist2 && dy*dy < maxInlierXDist2 &&
dd*dd < maxInlierDDist2)
inl+=(int)sqrt(ipt.z()); // clever way to weight closer points
// inl++;
}
#pragma omp critical
if (inl > bestinl)
{
bestinl = inl;
rot = R;
trans = tr;
}
}
// printf("Best inliers: %d\n", bestinl);
// printf("Total ransac: %d Neg det: %d\n", ntot, nneg);
// reduce matches to inliers
vector<cv::DMatch> inls; // temporary for current inliers
inliers.clear();
Matrix<double,3,4> tfm;
tfm.block<3,3>(0,0) = rot;
tfm.col(3) = trans;
nmatch = matches.size();
for (int i=0; i<nmatch; i++)
{
if (!f0.goodPts[matches[i].queryIdx] || !f1.goodPts[matches[i].trainIdx])
continue;
Vector3d pt = tfm*f1.pts[matches[i].trainIdx];
Vector3d ipt = f0.cam2pix(pt);
const cv::KeyPoint &kp = f0.kpts[matches[i].queryIdx];
double dx = kp.pt.x - ipt.x();
double dy = kp.pt.y - ipt.y();
double dd = f0.disps[matches[i].queryIdx] - ipt.z();
if (dx*dx < maxInlierXDist2 && dy*dy < maxInlierXDist2 &&
dd*dd < maxInlierDDist2)
inls.push_back(matches[i]);
}
#if 0
cout << endl << trans.transpose().head(3) << endl << endl;
cout << rot << endl;
#endif
// polish with stereo sba
if (polish)
{
// system
SysSBA sba;
sba.verbose = 0;
// set up nodes
// should have a frame => node function
Vector4d v0 = Vector4d(0,0,0,1);
Quaterniond q0 = Quaternion<double>(Vector4d(0,0,0,1));
sba.addNode(v0, q0, f0.cam, true);
Quaterniond qr1(rot); // from rotation matrix
Vector4d temptrans = Vector4d(trans(0), trans(1), trans(2), 1.0);
// sba.addNode(temptrans, qr1.normalized(), f1.cam, false);
qr1.normalize();
sba.addNode(temptrans, qr1, f1.cam, false);
int in = 3;
if (in > (int)inls.size())
in = inls.size();
// set up projections
for (int i=0; i<(int)inls.size(); i++)
{
// add point
int i0 = inls[i].queryIdx;
int i1 = inls[i].trainIdx;
Vector4d pt = f0.pts[i0];
sba.addPoint(pt);
// projected point, ul,vl,ur
Vector3d ipt;
ipt(0) = f0.kpts[i0].pt.x;
ipt(1) = f0.kpts[i0].pt.y;
ipt(2) = ipt(0)-f0.disps[i0];
sba.addStereoProj(0, i, ipt);
// projected point, ul,vl,ur
ipt(0) = f1.kpts[i1].pt.x;
ipt(1) = f1.kpts[i1].pt.y;
ipt(2) = ipt(0)-f1.disps[i1];
sba.addStereoProj(1, i, ipt);
}
sba.huber = 2.0;
sba.doSBA(5,10e-4,SBA_DENSE_CHOLESKY);
int nbad = sba.removeBad(2.0);
// cout << endl << "Removed " << nbad << " projections > 2 pixels error" << endl;
sba.doSBA(5,10e-5,SBA_DENSE_CHOLESKY);
// cout << endl << sba.nodes[1].trans.transpose().head(3) << endl;
// get the updated transform
trans = sba.nodes[1].trans.head(3);
Quaterniond q1;
q1 = sba.nodes[1].qrot;
rot = q1.toRotationMatrix();
// set up inliers
inliers.clear();
for (int i=0; i<(int)inls.size(); i++)
{
ProjMap &prjs = sba.tracks[i].projections;
if (prjs[0].isValid && prjs[1].isValid) // valid track
inliers.push_back(inls[i]);
}
#if 0
printf("Inliers: %d After polish: %d\n", (int)inls.size(), (int)inliers.size());
#endif
}
return (int)inliers.size();
}