int contactConstraints(RigidBodyManipulator *r, int nc, std::vector<SupportStateElement,Eigen::aligned_allocator<SupportStateElement>>& supp, void *map_ptr, MatrixXd &n, MatrixXd &D, MatrixXd &Jp, MatrixXd &Jpdot,double terrain_height)
{
int j, k=0, nq = r->num_positions;
n.resize(nc,nq);
D.resize(nq,nc*2*m_surface_tangents);
Jp.resize(3*nc,nq);
Jpdot.resize(3*nc,nq);
Vector3d contact_pos,pos,normal;
MatrixXd J(3,nq);
Matrix<double,3,m_surface_tangents> d;
for (std::vector<SupportStateElement,Eigen::aligned_allocator<SupportStateElement>>::iterator iter = supp.begin(); iter!=supp.end(); iter++) {
if (nc>0) {
for (std::vector<Vector4d,aligned_allocator<Vector4d>>::iterator pt_iter=iter->contact_pts.begin(); pt_iter!=iter->contact_pts.end(); pt_iter++) {
r->forwardKin(iter->body_idx,*pt_iter,0,contact_pos);
r->forwardJac(iter->body_idx,*pt_iter,0,J);
collisionDetect(map_ptr,contact_pos,pos,&normal,terrain_height);
surfaceTangents(normal,d);
n.row(k) = normal.transpose()*J;
for (j=0; j<m_surface_tangents; j++) {
D.col(2*k*m_surface_tangents+j) = J.transpose()*d.col(j);
D.col((2*k+1)*m_surface_tangents+j) = -D.col(2*k*m_surface_tangents+j);
}
// store away kin sols into Jp and Jpdot
// NOTE: I'm cheating and using a slightly different ordering of J and Jdot here
Jp.block(3*k,0,3,nq) = J;
r->forwardJacDot(iter->body_idx,*pt_iter,0,J);
Jpdot.block(3*k,0,3,nq) = J;
k++;
}
}
}
return k;
}
Matrix TaskSpace::Task::dynamicallyConsistentJacobianInverse(const State& s)
const
{
// J^T \Lambda
// -----------
Matrix jacobianTransposeTimesLambda =
jacobian(s).transpose() * taskSpaceMassMatrix(s);
// A^{-1} J^T \Lambda
// ------------------
Matrix dynConsistentJacobianInverse(s.getNU(), getNumScalarTasks());
for (unsigned int iST = 0; iST < getNumScalarTasks(); ++iST)
{
m_model->getMatterSubsystem().multiplyByMInv(s,
jacobianTransposeTimesLambda.col(iST),
dynConsistentJacobianInverse.updCol(iST));
}
return dynConsistentJacobianInverse;
}
void surfaceTangents(const Vector3d & normal, Matrix<double,3,m_surface_tangents> & d)
{
Vector3d t1,t2;
double theta;
if (1 - normal(2) < EPSILON) { // handle the unit-normal case (since it's unit length, just check z)
t1 << 1,0,0;
} else if(1 + normal(2) < EPSILON) {
t1 << -1,0,0; //same for the reflected case
} else {// now the general case
t1 << normal(1), -normal(0) , 0;
t1 /= sqrt(normal(1)*normal(1) + normal(0)*normal(0));
}
t2 = t1.cross(normal);
for (int k=0; k<m_surface_tangents; k++) {
theta = k*M_PI/m_surface_tangents;
d.col(k)=cos(theta)*t1 + sin(theta)*t2;
}
}
/*-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-*/
Data::Data(ifstream &inputFile, ofstream &stream, char* inFile, std::ofstream ×Log, int method){
//Instancia la clase que sabe hacer todo con las imagenes
setearParamsSimples(inputFile, inFile);
importarImgs(inputFile);
leerImagenesDeTest(inputFile);
init_time();
restarMuYHacerSqrt();
kCommonTime = get_time();
calcularAutovectores(stream, method); //este metodo caculas los tiempos de forma independiente
//guardo los autovectores originales
Matrix todosLosAutovectores = autovectores;
for (kActual = 1; kActual <= k; kActual++){
Matrix autovectoresK(todosLosAutovectores.n,kActual);
for(int j = 0; j < kActual; j++){
Matrix unAutovector = todosLosAutovectores.col(j);
autovectoresK.setColumn(j,unAutovector);
}
//defino autovectores, con los primeros i de la matriz orignal
autovectores = autovectoresK;
init_time();
calcularNuevasCoordenadas();
tTodos = get_time(); //solo necesito cambiar de coordenadas para comparar contra todos
calcularKCentrosDeMasa();
tCentro = get_time(); //para comparar con los centors tambien los debo calcular
identificarSujetos();//este metodo guarda los tiempos de manera independiente en tTods y tCentro
timesLog << kActual << "\t" << samples << "\t" << subjects << "\t";
timesLog << (kCommonTime + tK[kActual-1] + kOnlyTransposedTime*kActual/k); //kOnlytransposedTime es cero en caso de utilizar el metodo de la matriz transpuesta, por lo que no afecta los calculos
timesLog << "\t" << tTodos << "\t" << tCentro;
timesLog << "\t" << hitsTodos << "\t" << hitsCentro << "\t" << endl;
}
}
void DefaultLocalAssemblerForOperatorsOnSurfacesUtilities<BasisFunctionType>::
precalculateElementSizesAndCentersForSingleGrid(
const RawGridGeometry<CoordinateType> &rawGeometry,
std::vector<CoordinateType> &elementSizesSquared,
Matrix<CoordinateType> &elementCenters,
CoordinateType &averageElementSize) {
const size_t elementCount = rawGeometry.elementCount();
const int worldDim = rawGeometry.worldDimension();
averageElementSize = 0.; // We will store here temporarily
// the sum of element sizes
elementSizesSquared.resize(elementCount);
for (int e = 0; e < elementCount; ++e) {
elementSizesSquared[e] = elementSizeSquared(e, rawGeometry);
averageElementSize += sqrt(elementSizesSquared[e]);
}
averageElementSize /= elementCount;
elementCenters.resize(worldDim, elementCount);
for (int e = 0; e < elementCount; ++e)
elementCenters.col(e) = elementCenter(e, rawGeometry);
}
T ErrorMinimizersImpl<T>::PointToPlaneErrorMinimizer::getOverlap() const
{
const int nbPoints = this->lastErrorElements.reading.features.cols();
const int dim = this->lastErrorElements.reading.features.rows();
if(nbPoints == 0)
{
throw std::runtime_error("Error, last error element empty. Error minimizer needs to be called at least once before using this method.");
}
if (!this->lastErrorElements.reading.descriptorExists("simpleSensorNoise") ||
!this->lastErrorElements.reading.descriptorExists("normals"))
{
LOG_INFO_STREAM("PointToPlaneErrorMinimizer - warning, no sensor noise or normals found. Using best estimate given outlier rejection instead.");
return this->weightedPointUsedRatio;
}
const BOOST_AUTO(noises, this->lastErrorElements.reading.getDescriptorViewByName("simpleSensorNoise"));
const BOOST_AUTO(normals, this->lastErrorElements.reading.getDescriptorViewByName("normals"));
const Matrix delta = (this->lastErrorElements.reading.features.topRows(dim-1) - this->lastErrorElements.reference.features.topRows(dim-1));
const T mean = delta.colwise().norm().sum()/nbPoints;
cerr << "mean:" << mean << endl;
int count = 0;
for(int i=0; i < nbPoints; i++)
{
const Vector n = normals.col(i);
const T projectionDist = delta.col(i).dot(n.normalized());
if(anyabs(projectionDist) < (mean + noises(0,i)))
{
count++;
}
}
return (T)count/(T)nbPoints;
}
// Rot-trans-rot odometry based motion model
Vector3d motion_model(Vector3d x, Matrix<double, 3, 2> u) {
Matrix<double, 4, 1> noise;
noise << 0.05, 0.26, 0.01, 0.0057; // rad/rad, rad/m, m/m, m/rad
// noise << 0.1, 0.1, 0.1, 0.1;
auto u_old = u.col(0);
auto u_new = u.col(1);
// Start by defining del
Vector3d del;
auto drot1 = atan2(u_new[1] - u_old[1], u_new[0] - u_old[0]) - u_old[2];
del <<
sqrt(pow(u_new[0] - u_old[0], 2) + pow(u_new[1] - u_old[1], 2)),
drot1,
u_new[2] - u_old[2] - drot1;
// Introduce noise
Vector3d del_hat;
auto d0 = sample_normal(noise[2] * del[0] + noise[3] * (abs(del[1]) + abs(del[2])));
auto d1 = sample_normal(noise[0] * abs(del[1]) + noise[1] * del[0]);
auto d2 = sample_normal(noise[0] * abs(del[2]) + noise[1] * del[0]);
del_hat <<
del[0] - d0,
del[1] - d1,
del[2] - d2;
// Evaluate output
Vector3d x_new;
x_new <<
x[0] + del_hat[0] * cos(x[2] + del_hat[1]),
x[1] + del_hat[0] * sin(x[2] + del_hat[1]),
x[2] + del_hat[1] + del_hat[2];
return x_new;
}
void
interpolate(
const_Matrix<IT, Block1> indices, // n x m
Tensor<T, Block2> window, // n x m x I
const_Matrix<complex<T>, Block3> in, // n x m
Matrix<complex<T>, Block4> out, // nx x m
length_type depth)
{
length_type n = indices.size(0);
length_type m = indices.size(1);
length_type nx = out.size(0);
length_type I = depth; // window.size(2) may include padding
assert(n == in.size(0));
assert(m == in.size(1));
assert(m == out.size(1));
assert(window.size(0) == n);
assert(window.size(1) == m);
out = complex<T>(0);
for (index_type j = 0; j < m; ++j)
{
for (index_type i = 0; i < n; ++i)
{
index_type ikxrows = indices.get(i, j);
index_type i_shift = (i + n/2) % n;
for (index_type h = 0; h < I; ++h)
{
out.put(ikxrows + h, j, out.get(ikxrows + h, j) +
(in.get(i_shift, j) * window.get(i, j, h)));
}
}
out.col(j)(Domain<1>(j%2, 2, nx/2)) *= T(-1);
}
}
SimpleCamera simpleCamera(const Matrix& P) {
// P = [A|a] = s K cRw [I|-T], with s the unknown scale
Matrix A = P.topLeftCorner(3, 3);
Vector a = P.col(3);
// do RQ decomposition to get s*K and cRw angles
Matrix sK;
Vector xyz;
boost::tie(sK, xyz) = RQ(A);
// Recover scale factor s and K
double s = sK(2, 2);
Matrix K = sK / s;
// Recover cRw itself, and its inverse
Rot3 cRw = Rot3::RzRyRx(xyz);
Rot3 wRc = cRw.inverse();
// Now, recover T from a = - s K cRw T = - A T
Vector T = -(A.inverse() * a);
return SimpleCamera(Pose3(wRc, T),
Cal3_S2(K(0, 0), K(1, 1), K(0, 1), K(0, 2), K(1, 2)));
}
Matrix gramschmidt( const Matrix & A ) {
Matrix Q = A;
// First vector just gets normalized
Q.col(0).normalize();
for(unsigned int j = 1; j < A.cols(); ++j) {
// Replace inner loop over each previous vector in Q with fast matrix-vector multiplication
Q.col(j) -= Q.leftCols(j) * (Q.leftCols(j).transpose() * A.col(j));
// Normalize vector if possible (othw. means colums of A almsost lin. dep.
if( Q.col(j).norm() <= 10e-14 * A.col(j).norm() ) {
std::cerr << "Gram-Schmidt failed because A has lin. dep columns. Bye." << std::endl;
break;
} else {
Q.col(j).normalize();
}
}
return Q;
}
void eigen33(const Matrix& mat, Matrix& evecs, Vector& evals)
{
typedef typename Matrix::Scalar Scalar;
// Scale the matrix so its entries are in [-1,1]. The scaling is applied
// only when at least one matrix entry has magnitude larger than 1.
Scalar scale = mat.cwiseAbs()/*.template triangularView<Lower>()*/.maxCoeff();
scale = std::max(scale,Scalar(1));
Matrix scaledMat = mat / scale;
// Compute the eigenvalues
// scaledMat.setZero();
computeRoots(scaledMat,evals);
// compute the eigen vectors
// **here we assume 3 differents eigenvalues**
// "optimized version" which appears to be slower with gcc!
// Vector base;
// Scalar alpha, beta;
// base << scaledMat(1,0) * scaledMat(2,1),
// scaledMat(1,0) * scaledMat(2,0),
// -scaledMat(1,0) * scaledMat(1,0);
// for(int k=0; k<2; ++k)
// {
// alpha = scaledMat(0,0) - evals(k);
// beta = scaledMat(1,1) - evals(k);
// evecs.col(k) = (base + Vector(-beta*scaledMat(2,0), -alpha*scaledMat(2,1), alpha*beta)).normalized();
// }
// evecs.col(2) = evecs.col(0).cross(evecs.col(1)).normalized();
// // naive version
// Matrix tmp;
// tmp = scaledMat;
// tmp.diagonal().array() -= evals(0);
// evecs.col(0) = tmp.row(0).cross(tmp.row(1)).normalized();
//
// tmp = scaledMat;
// tmp.diagonal().array() -= evals(1);
// evecs.col(1) = tmp.row(0).cross(tmp.row(1)).normalized();
//
// tmp = scaledMat;
// tmp.diagonal().array() -= evals(2);
// evecs.col(2) = tmp.row(0).cross(tmp.row(1)).normalized();
// a more stable version:
if((evals(2)-evals(0))<=Eigen::NumTraits<Scalar>::epsilon())
{
evecs.setIdentity();
}
else
{
Matrix tmp;
tmp = scaledMat;
tmp.diagonal ().array () -= evals (2);
evecs.col (2) = tmp.row (0).cross (tmp.row (1)).normalized ();
tmp = scaledMat;
tmp.diagonal ().array () -= evals (1);
evecs.col(1) = tmp.row (0).cross(tmp.row (1));
Scalar n1 = evecs.col(1).norm();
if(n1<=Eigen::NumTraits<Scalar>::epsilon())
evecs.col(1) = evecs.col(2).unitOrthogonal();
else
evecs.col(1) /= n1;
// make sure that evecs[1] is orthogonal to evecs[2]
evecs.col(1) = evecs.col(2).cross(evecs.col(1).cross(evecs.col(2))).normalized();
evecs.col(0) = evecs.col(2).cross(evecs.col(1));
}
// Rescale back to the original size.
evals *= scale;
}
double ppm(
const Matrix<T,N,1> & Delta,
const Matrix<T,N,1> & Vmax,
const Matrix<T,N,1> & Amax,
Matrix<T,N,1> & Vnew,
Matrix<T,N,1> & Anew,
Matrix<T,N,1> & Dnew,
T requestedMotionTime = 0
)
{
Matrix<T,N,3> Times;
// Iterate over axes
for(unsigned int l = 0; l < N; ++l) {
const T delta = Delta(l,0), // motor position delta
vmax = Vmax(l,0), // maximal velocity
amax = Amax(l,0), // maximal acceleration
dmax = -Amax(l,0); // maximal deceleration
// Velocity value, when the velocity profile is triangular (eq. 3.32)
const T VTriangle = amax*std::sqrt(2*delta*dmax/(amax*(dmax-amax)));
//const T VTriangle = std::sqrt(2*delta*amax*dmax/(dmax-amax));
#if 0
std::cout << "VTriangle(" << l << ") = " << VTriangle << std::endl;
#endif
const bool TriangularProfile = (VTriangle <= vmax);
if(TriangularProfile) {
// tt: total motion time (eq. 3.33)
Times(l,2) = std::sqrt(2*delta*(dmax-amax)/(amax*dmax));
// acceleration and deceleration phase treated as a half of the total motion time
Times(l,0) = Times(l,1) = Times(l,2)/2;
} else {
// ta: time to stop accelerate (eq. 3.35)
Times(l,0) = vmax/amax;
// td: time to start deceleration (eq. 3.42)
Times(l,1) = delta/vmax + vmax/(2*amax) + vmax/(2*dmax);
// Numerically stable version:
//Time(l,1) = (2*delta*amax*dmax+vmax*vmax*amax+vmax*vmax*dmax)/(2*vmax*amax*dmax);
// tt: total motion time (eq. 3.40)
Times(l,2) = delta/vmax + vmax/(2*amax) - vmax/(2*dmax);
// Numerically stable version:
//Time(l,2) = (2*delta*amax*dmax+vmax*vmax*dmax-vmax*vmax*amax)/(2*vmax*amax*dmax);
}
// std::cerr << "VLimit[" << l << "]: " << VTriangle <<
// " => " << (TriangularProfile ? "triangular" : "trapezoidal") << std::endl <<
// "Time[" << l << "]: " << Times(l,0) << " " << Times(l,1) << " " << Times(l,2) <<
// std::endl;
}
Matrix<T,1,3> maxTimes = Times.colwise().maxCoeff();
// std::cerr << "maxTimes: " << maxTimes << std::endl;
// max of acceleration intervals
const T ta = maxTimes(0);
// max of constant velocity intervals
const T tV = (Times.col(1)-Times.col(0)).maxCoeff();
// max of deceleration intervals
const T tD = (Times.col(2)-Times.col(1)).maxCoeff();
// deceleration interval
const T td = ta + tV;
T tt = ta + tV + tD;
if (ta > td) {
tt += (ta - td);
}
// Make the motion longer
if(requestedMotionTime > 0) {
if(tt > requestedMotionTime) {
throw std::runtime_error("requested motion time too short");
}
tt = requestedMotionTime;
}
// std::cout
// << "ta: " << ta << "\t"
// << "td: " << td << "\t"
// << "tt: " << tt
// << std::endl;
// If the total time is zero there will be no motion
if(tt > 0) {
// I guess this can be implemented as a single matrix calculation
for(unsigned int l = 0; l < N; ++l) {
const T delta = Delta(l,0);
// Calculate new parameters if there is motion along an axis
if(delta) {
Anew(l,0) = 2*delta/(ta*(tt+td-ta));
Dnew(l,0) = - (-2*delta/((tt+td-ta)*(tt-td))); // deceleration value (without sign)
//Dnew(l,0) = (2*delta)/(tt*tt-tt*ta+td*ta-td*td); // Numerically stable (?) version
Vnew(l,0) = Anew(l,0)*ta;
//Vnew(l,0) = (2*delta)/(tt+td-ta); // Numerically stable (?) version
} else {
Anew(l,0) = Amax(l,0);
Dnew(l,0) = Amax(l,0);
Vnew(l,0) = Vmax(l,0);
}
}
} else {
Vnew = Vmax;
Anew = Dnew = Amax;
}
// These assertions fail because of floating point inequalities
//assert(Dnew(l,0)<=Amax(l,0));
//assert(Anew(l,0)<=Amax(l,0));
//assert(Vnew(l,0)<=Vmax(l,0));;
// std::cerr <<
// "Vnew:\n" << Vnew << std::endl <<
// "Anew:\n" << Anew << std::endl <<
// "Dnew:\n" << Dnew << std::endl <<
// std::endl;
// std::cerr << "tt: " << tt << std::endl;
return tt;
}
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();
}
IGL_INLINE void igl::cotangent(
const Eigen::PlainObjectBase<DerivedV>& V,
const Eigen::PlainObjectBase<DerivedF>& F,
Eigen::PlainObjectBase<DerivedC>& C)
{
using namespace igl;
using namespace std;
using namespace Eigen;
// simplex size (3: triangles, 4: tetrahedra)
int simplex_size = F.cols();
// Number of elements
int m = F.rows();
// Law of cosines + law of sines
switch(simplex_size)
{
case 3:
{
// Triangles
//Matrix<typename DerivedC::Scalar,Dynamic,3> l;
//edge_lengths(V,F,l);
// edge lengths numbered same as opposite vertices
Matrix<typename DerivedC::Scalar,Dynamic,3> l;
igl::edge_lengths(V,F,l);
// double area
Matrix<typename DerivedC::Scalar,Dynamic,1> dblA;
doublearea(l,dblA);
// cotangents and diagonal entries for element matrices
// correctly divided by 4 (alec 2010)
C.resize(m,3);
for(int i = 0;i<m;i++)
{
C(i,0) = (l(i,1)*l(i,1) + l(i,2)*l(i,2) - l(i,0)*l(i,0))/dblA(i)/4.0;
C(i,1) = (l(i,2)*l(i,2) + l(i,0)*l(i,0) - l(i,1)*l(i,1))/dblA(i)/4.0;
C(i,2) = (l(i,0)*l(i,0) + l(i,1)*l(i,1) - l(i,2)*l(i,2))/dblA(i)/4.0;
}
break;
}
case 4:
{
// edge lengths numbered same as opposite vertices
Matrix<typename DerivedC::Scalar,Dynamic,6> l;
edge_lengths(V,F,l);
Matrix<typename DerivedC::Scalar,Dynamic,4> s;
face_areas(l,s);
Matrix<typename DerivedC::Scalar,Dynamic,6> cos_theta,theta;
dihedral_angles_intrinsic(l,s,theta,cos_theta);
// volume
Matrix<typename DerivedC::Scalar,Dynamic,1> vol;
volume(l,vol);
// Law of sines
// http://mathworld.wolfram.com/Tetrahedron.html
Matrix<typename DerivedC::Scalar,Dynamic,6> sin_theta(m,6);
sin_theta.col(0) = vol.array() / ((2./(3.*l.col(0).array())).array() * s.col(1).array() * s.col(2).array());
sin_theta.col(1) = vol.array() / ((2./(3.*l.col(1).array())).array() * s.col(2).array() * s.col(0).array());
sin_theta.col(2) = vol.array() / ((2./(3.*l.col(2).array())).array() * s.col(0).array() * s.col(1).array());
sin_theta.col(3) = vol.array() / ((2./(3.*l.col(3).array())).array() * s.col(3).array() * s.col(0).array());
sin_theta.col(4) = vol.array() / ((2./(3.*l.col(4).array())).array() * s.col(3).array() * s.col(1).array());
sin_theta.col(5) = vol.array() / ((2./(3.*l.col(5).array())).array() * s.col(3).array() * s.col(2).array());
// http://arxiv.org/pdf/1208.0354.pdf Page 18
C = (1./6.) * l.array() * cos_theta.array() / sin_theta.array();
break;
}
default:
{
fprintf(stderr,
"cotangent.h: Error: Simplex size (%d) not supported\n", simplex_size);
assert(false);
}
}
}
IGL_INLINE void igl::massmatrix(
const Eigen::MatrixBase<DerivedV> & V,
const Eigen::MatrixBase<DerivedF> & F,
const MassMatrixType type,
Eigen::SparseMatrix<Scalar>& M)
{
using namespace Eigen;
using namespace std;
const int n = V.rows();
const int m = F.rows();
const int simplex_size = F.cols();
MassMatrixType eff_type = type;
// Use voronoi of for triangles by default, otherwise barycentric
if(type == MASSMATRIX_TYPE_DEFAULT)
{
eff_type = (simplex_size == 3?MASSMATRIX_TYPE_VORONOI:MASSMATRIX_TYPE_BARYCENTRIC);
}
// Not yet supported
assert(type!=MASSMATRIX_TYPE_FULL);
Matrix<int,Dynamic,1> MI;
Matrix<int,Dynamic,1> MJ;
Matrix<Scalar,Dynamic,1> MV;
if(simplex_size == 3)
{
// Triangles
// edge lengths numbered same as opposite vertices
Matrix<Scalar,Dynamic,3> l(m,3);
// loop over faces
for(int i = 0;i<m;i++)
{
l(i,0) = (V.row(F(i,1))-V.row(F(i,2))).norm();
l(i,1) = (V.row(F(i,2))-V.row(F(i,0))).norm();
l(i,2) = (V.row(F(i,0))-V.row(F(i,1))).norm();
}
Matrix<Scalar,Dynamic,1> dblA;
doublearea(l,dblA);
switch(eff_type)
{
case MASSMATRIX_TYPE_BARYCENTRIC:
// diagonal entries for each face corner
MI.resize(m*3,1); MJ.resize(m*3,1); MV.resize(m*3,1);
MI.block(0*m,0,m,1) = F.col(0);
MI.block(1*m,0,m,1) = F.col(1);
MI.block(2*m,0,m,1) = F.col(2);
MJ = MI;
repmat(dblA,3,1,MV);
MV.array() /= 6.0;
break;
case MASSMATRIX_TYPE_VORONOI:
{
// diagonal entries for each face corner
// http://www.alecjacobson.com/weblog/?p=874
MI.resize(m*3,1); MJ.resize(m*3,1); MV.resize(m*3,1);
MI.block(0*m,0,m,1) = F.col(0);
MI.block(1*m,0,m,1) = F.col(1);
MI.block(2*m,0,m,1) = F.col(2);
MJ = MI;
// Holy shit this needs to be cleaned up and optimized
Matrix<Scalar,Dynamic,3> cosines(m,3);
cosines.col(0) =
(l.col(2).array().pow(2)+l.col(1).array().pow(2)-l.col(0).array().pow(2))/(l.col(1).array()*l.col(2).array()*2.0);
cosines.col(1) =
(l.col(0).array().pow(2)+l.col(2).array().pow(2)-l.col(1).array().pow(2))/(l.col(2).array()*l.col(0).array()*2.0);
cosines.col(2) =
(l.col(1).array().pow(2)+l.col(0).array().pow(2)-l.col(2).array().pow(2))/(l.col(0).array()*l.col(1).array()*2.0);
Matrix<Scalar,Dynamic,3> barycentric = cosines.array() * l.array();
normalize_row_sums(barycentric,barycentric);
Matrix<Scalar,Dynamic,3> partial = barycentric;
partial.col(0).array() *= dblA.array() * 0.5;
partial.col(1).array() *= dblA.array() * 0.5;
partial.col(2).array() *= dblA.array() * 0.5;
Matrix<Scalar,Dynamic,3> quads(partial.rows(),partial.cols());
quads.col(0) = (partial.col(1)+partial.col(2))*0.5;
quads.col(1) = (partial.col(2)+partial.col(0))*0.5;
quads.col(2) = (partial.col(0)+partial.col(1))*0.5;
quads.col(0) = (cosines.col(0).array()<0).select( 0.25*dblA,quads.col(0));
quads.col(1) = (cosines.col(0).array()<0).select(0.125*dblA,quads.col(1));
quads.col(2) = (cosines.col(0).array()<0).select(0.125*dblA,quads.col(2));
quads.col(0) = (cosines.col(1).array()<0).select(0.125*dblA,quads.col(0));
quads.col(1) = (cosines.col(1).array()<0).select(0.25*dblA,quads.col(1));
quads.col(2) = (cosines.col(1).array()<0).select(0.125*dblA,quads.col(2));
quads.col(0) = (cosines.col(2).array()<0).select(0.125*dblA,quads.col(0));
quads.col(1) = (cosines.col(2).array()<0).select(0.125*dblA,quads.col(1));
quads.col(2) = (cosines.col(2).array()<0).select( 0.25*dblA,quads.col(2));
MV.block(0*m,0,m,1) = quads.col(0);
MV.block(1*m,0,m,1) = quads.col(1);
MV.block(2*m,0,m,1) = quads.col(2);
break;
}
case MASSMATRIX_TYPE_FULL:
assert(false && "Implementation incomplete");
break;
default:
assert(false && "Unknown Mass matrix eff_type");
}
}else if(simplex_size == 4)
{
assert(V.cols() == 3);
assert(eff_type == MASSMATRIX_TYPE_BARYCENTRIC);
MI.resize(m*4,1); MJ.resize(m*4,1); MV.resize(m*4,1);
MI.block(0*m,0,m,1) = F.col(0);
MI.block(1*m,0,m,1) = F.col(1);
MI.block(2*m,0,m,1) = F.col(2);
MI.block(3*m,0,m,1) = F.col(3);
MJ = MI;
// loop over tets
for(int i = 0;i<m;i++)
{
// http://en.wikipedia.org/wiki/Tetrahedron#Volume
Matrix<Scalar,3,1> v0m3 = V.row(F(i,0)) - V.row(F(i,3));
Matrix<Scalar,3,1> v1m3 = V.row(F(i,1)) - V.row(F(i,3));
Matrix<Scalar,3,1> v2m3 = V.row(F(i,2)) - V.row(F(i,3));
Scalar v = fabs(v0m3.dot(v1m3.cross(v2m3)))/6.0;
MV(i+0*m) = v/4.0;
MV(i+1*m) = v/4.0;
MV(i+2*m) = v/4.0;
MV(i+3*m) = v/4.0;
}
}else
{
// Unsupported simplex size
assert(false && "Unsupported simplex size");
}
sparse(MI,MJ,MV,n,n,M);
}
T logLikelihood(const Matrix<T,Dynamic,Dynamic>& x, uint32_t i) const
{return logLikelihood(x.col(i));};
typename PointMatcher<T>::ErrorMinimizer::ErrorElements& PointMatcher<T>::ErrorMinimizer::getMatchedPoints(
const DataPoints& requestedPts,
const DataPoints& sourcePts,
const Matches& matches,
const OutlierWeights& outlierWeights)
{
typedef typename Matches::Ids Ids;
typedef typename Matches::Dists Dists;
assert(matches.ids.rows() > 0);
assert(matches.ids.cols() > 0);
assert(matches.ids.cols() == requestedPts.features.cols()); //nbpts
assert(outlierWeights.rows() == matches.ids.rows()); // knn
const int knn = outlierWeights.rows();
const int dimFeat = requestedPts.features.rows();
const int dimReqDesc = requestedPts.descriptors.rows();
// Count points with no weights
const int pointsCount = (outlierWeights.array() != 0.0).count();
if (pointsCount == 0)
throw ConvergenceError("ErrorMnimizer: no point to minimize");
Matrix keptFeat(dimFeat, pointsCount);
Matrix keptDesc;
if(dimReqDesc > 0)
keptDesc = Matrix(dimReqDesc, pointsCount);
Matches keptMatches (Dists(1,pointsCount), Ids(1, pointsCount));
OutlierWeights keptWeights(1, pointsCount);
int j = 0;
int rejectedMatchCount = 0;
int rejectedPointCount = 0;
bool matchExist = false;
this->weightedPointUsedRatio = 0;
for (int i = 0; i < requestedPts.features.cols(); ++i) //nb pts
{
matchExist = false;
for(int k = 0; k < knn; k++) // knn
{
if (outlierWeights(k,i) != 0.0)
{
if(dimReqDesc > 0)
keptDesc.col(j) = requestedPts.descriptors.col(i);
keptFeat.col(j) = requestedPts.features.col(i);
keptMatches.ids(0, j) = matches.ids(k, i);
keptMatches.dists(0, j) = matches.dists(k, i);
keptWeights(0,j) = outlierWeights(k,i);
++j;
this->weightedPointUsedRatio += outlierWeights(k,i);
matchExist = true;
}
else
{
rejectedMatchCount++;
}
}
if(matchExist == false)
{
rejectedPointCount++;
}
}
assert(j == pointsCount);
this->pointUsedRatio = double(j)/double(knn*requestedPts.features.cols());
this->weightedPointUsedRatio /= double(knn*requestedPts.features.cols());
assert(dimFeat == sourcePts.features.rows());
const int dimSourDesc = sourcePts.descriptors.rows();
Matrix associatedFeat(dimFeat, pointsCount);
Matrix associatedDesc;
if(dimSourDesc > 0)
associatedDesc = Matrix(dimSourDesc, pointsCount);
// Fetch matched points
for (int i = 0; i < pointsCount; ++i)
{
const int refIndex(keptMatches.ids(i));
associatedFeat.col(i) = sourcePts.features.block(0, refIndex, dimFeat, 1);
if(dimSourDesc > 0)
associatedDesc.col(i) = sourcePts.descriptors.block(0, refIndex, dimSourDesc, 1);
}
this->lastErrorElements.reading = DataPoints(
keptFeat,
requestedPts.featureLabels,
keptDesc,
requestedPts.descriptorLabels
);
this->lastErrorElements.reference = DataPoints(
associatedFeat,
sourcePts.featureLabels,
associatedDesc,
sourcePts.descriptorLabels
);
this->lastErrorElements.weights = keptWeights;
this->lastErrorElements.matches = keptMatches;
this->lastErrorElements.nbRejectedMatches = rejectedMatchCount;
this->lastErrorElements.nbRejectedPoints = rejectedPointCount;
return this->lastErrorElements;
}
void Logit::compress()
{
// Push everything into a list.
list<double> ylist;
list<Matrix> xlist;
list<double> nlist;
// Our data should not have n_data(i)=0.
for(uint i=0; i < N; ++i){
ylist.push_back(y(i));
xlist.push_back(tX.col(i));
nlist.push_back(n(i));
}
// Merge data.
list<double>::iterator y_i;
list<Matrix>::iterator x_i;
list<double>::iterator n_i;
list<double>::iterator y_j;
list<Matrix>::iterator x_j;
list<double>::iterator n_j;
x_i = xlist.begin();
y_i = ylist.begin();
n_i = nlist.begin();
while(x_i != xlist.end()){
x_j = x_i; y_j = y_i; n_j = n_i;
++x_j; ++y_j; ++n_j;
while(x_j != xlist.end()){
if (*x_i == *x_j) {
double sum = *n_i + *n_j;
*y_i = (*n_i/sum) * *y_i + (*n_j/sum) * *y_j;
*n_i = sum;
// Increment THEN erase!
// Actually, send pointer to erase, then increment, then erase.
xlist.erase(x_j++);
ylist.erase(y_j++);
nlist.erase(n_j++);
}
else {
++x_j; ++y_j; ++n_j;
}
}
++x_i; ++y_i; ++n_i;
}
uint old_N = N; // Record to see if we have changed data.
// Set up y, tX, n.
N = xlist.size();
// cout << "Old N: " << old_N << " N: " << N << "\n";
// Warning...
if (old_N != N) {
printf("Warning: data was combined!\n");
printf("N: %i, P: %i \n", N, P);
}
// Matrix y(N);
y.resize(N);
tX.resize(P, N);
n.resize(N);
x_i = xlist.begin();
y_i = ylist.begin();
n_i = nlist.begin();
for(uint i = 0; i < N; ++i){
y(i) = *y_i++;
tX.col(i) = *x_i++;
n(i) = *n_i++;
}
// cout << "y:\n" << y;
// cout << "tX:\n" << tX;
// cout << "n:\n" << n;
}
void main(void) {
int n, k, r;
//ofstream data_file;
//data_file.open("Answer5.txt");
n = 4;
for(k = 2; k <= n-2; k++) {
// We first loop through all partitions of n
list<Partition> pars;
pars = Par(n);
// for latter use
list<RankSet> ranks;
ranks = ComputeRanks(n, k);
for(list<Partition>::iterator lambda = pars.begin(); lambda != pars.end(); ++lambda) {
Partition theta;
for(int junk = 1; junk <= (*lambda).Size(); junk++) {
theta.Add(1);
}
tableau t = standard(*lambda, theta);
list<Perm> colgroup = C(t);
list<Perm> rowgroup = R(t);
vector<Perm> YS;
vector<int> signs;
int YSl = 0;
// we scroll through col * sign and the row
for(list<Perm>::iterator sigma = colgroup.begin(); sigma != colgroup.end(); ++sigma) {
for (list<Perm>::iterator tau = rowgroup.begin(); tau != rowgroup.end(); ++tau) {
YS.push_back((*sigma) * (*tau));
signs.push_back(sign(*sigma));
YSl++;
}
}
vector<Matrix> MatList(n-1);
vector<bool> exists(n-1);
for(int o = 0; o < n-1; o++) {
exists[o] = false;
}
// Now we loop through all possible ranks-sets
// Answers store our multiplicity calculations
vector<int> Answers(n-1);
// Before we begin we need to get all possible SSYT to create an ordered basis
vector<RankSet> images;
for(list<RankSet>::const_iterator tempiter = ranks.begin(); tempiter != ranks.end(); ++tempiter) {
images.push_back(*tempiter);
}
// get a list of SSYT
// where each element of the list is a SSYT
// of each possible image
vector<chain> posschains;
for(vector<RankSet>::iterator image = images.begin(); image != images.end(); ++image) {
list<tableau> temptablist = SSYT(*lambda, *image);
if(temptablist.size() != 0) {
// convert each tableau to a chain
list<chain> tempchainlist;
for(list<tableau>::const_iterator tabby = temptablist.begin(); tabby != temptablist.end(); ++tabby) {
Partition gamma2;
for (RankSet::const_iterator iter = (*image).begin(); iter != (*image).end(); ++iter) {
gamma2.Add(*iter);
}
tabloid tempoid;
tempoid = TableauToTabloid(*tabby, gamma2);
chain c = ConvertFromTabloid(tempoid);
posschains.push_back(c);
}
}
}
// So now posschains contains all possible image chains
// this stores the dimension of the range
int rangedim = posschains.size();
// for each possible image chain, apply the
// Young symmetrizer. We will get a result of type basis.
vector<basis> vecofimages;
for(vector<chain>::const_iterator c = posschains.begin(); c != posschains.end(); ++c) {
vecofimages.push_back(e(YS, signs, YSl, *c));
}
//vecofimages now contains all the symmetrized images.
for(list<RankSet>::iterator u = ranks.begin(); u != ranks.end(); ++u) {
// We need to get possible ranges of u
list<tableau> tableaulist = SSYT(*lambda, *u);
// convert to chains
list<chain> domain;
Partition gamma;
for (RankSet::const_iterator iter = (*u).begin(); iter != (*u).end(); ++iter) {
gamma.Add(*iter);
}
for(list<tableau>::const_iterator tabby = tableaulist.begin(); tabby != tableaulist.end(); ++tabby) {
domain.push_back(ConvertFromTabloid(TableauToTabloid(*tabby, gamma)));
}
// So now domain contains all chains which we need to compute the image of
// Now get all images of *u
//Test code:
// for each chain in the domain apply delta
vector<basis> domainchains;
list<chain>::iterator c1;
for(c1 = domain.begin(); c1 != domain.end(); ++c1) {
domainchains.push_back(Delta(k, *c1));
}
// then apply the Young symmetrizer of shape lambda
// and standard to the images of delta
vector<basis> Answer;
vector<basis>::const_iterator X;
for(X = domainchains.begin(); X != domainchains.end(); ++X) {
Answer.push_back(e(YS, signs, YSl, *X));
}
// then get the projection of our result onto each
// possible image
if(rangedim == 0) {
Answers[(*u).size()-2] = Answers[(*u).size()-2] + domainchains.size();
// cout << "\nWith lambda = " << *lambda;
// cout << "\nAnd u = " << *u;
// cout << "We add nullity " << domainchains.size();
// cout << "\nPress any key to continue";
// char ch = getchar();
}
else {
Matrix Mat = MatList[(*u).size()-2];
int newrow = Mat.col() + 1;
for(int count = newrow; count < newrow + Answer.size(); count++) {
// store this as a column in our matrix.
vector<double> tempvec = project(Answer[count-newrow], vecofimages);
Mat.ColumnSet(count, tempvec);
}
MatList[(*u).size() - 2] = Mat;
exists[(*u).size()-2] = true;
}
}
for(r = 0; r <= n-2; r++) {
// cout << "\nWith lambda = " << *lambda;
// cout << "\nand r = " << r;
// cout << "\nWe get the matrix" << MatList[r];
// cout << "With nullity = " << MatList[r].Nullity();
// cout << " and rank = " << MatList[r].Rank();
// cout << "\nOther contributions give " << Answers[r];
// we adjust the multiplicity vector
if (exists[r]) {
Answers[r] += MatList[r].Nullity();
Answers[r-1] -= MatList[r].Rank();
}
//
}
for(r = 0; r <= n-2; r++) {
cout << "\nThe multiplicity of ";
cout << (*lambda);
cout << " in H" << r << " is: " << Answers[r];
cout << endl;
}
cout << "\nPress any key to continue";
char ch = getchar();
}
}
// data_file.close();
}
int main()
{
cout << "Testing the matrix classes ..." << endl;
cout << "- the default matrix M (empty):" << endl;
Matrix<double> M;
cout << M;
cout << "- memory consumption of M: " << M.memory_consumption() << endl;
cout << "- a 2x3 matrix N:" << endl;
Matrix<double> N(2,3);
cout << N;
cout << "- memory consumption of N: " << N.memory_consumption() << endl;
cout << "- writing single entries:" << endl;
N(0,0) = 23; N(0,1) = 42; N(0,2) = 3; N(1,1) = -1.5;
cout << N;
cout << "- testing copy constructor:" << endl;
Matrix<double> Ncopy(N);
cout << Ncopy;
cout << "- are the two matrices equal?" << endl;
if (N == Ncopy)
cout << " ... yes!" << endl;
else
cout << " ... no!" << endl;
Matrix<double> O(2,3);
O(0,0) = 3.14; O(1,1) = -7.0;
cout << "- another matrix O:" << endl << O;
swap(N,O);
cout << "- after swapping, N=" << endl << N
<< " ... and O=" << endl << O;
N = O;
cout << "- N=O:" << endl << N;
O.scale(-2.0);
cout << "- O scaled by -2.0:" << endl << O;
Matrix<double> Q;
O.reflect(Q);
cout << "- O reflected:" << endl << Q;
Q.diagonal(4, -12.34);
cout << "- Q is now a diagonal matrix:" << endl << Q;
Vector<double> x(3), y(2);
x(0) = 1; x(1) = -1; x(2) = -2;
cout << "- testing apply(), using the vector x="
<< x << ";" << endl;
cout << " Nx=";
N.apply(x,y);
cout << y << endl;
cout << "- choose another matrix A (constructed from a string):" << endl;
Matrix<double> A(3,3,"1 2 3 4 5 6 7 8 9");
cout << A;
y.resize(3);
cout << " Ax=";
A.apply(x,y);
cout << y << endl;
cout << " A^Tx=";
A.apply_transposed(x,y);
cout << y << endl;
cout << "- testing matrix norms for A:" << endl;
cout << " max. row sum ||A||_infty: " << row_sum_norm(A) << endl;
cout << " max. column sum ||A||_1: " << column_sum_norm(A) << endl;
cout << " Frobenius norm ||A||_F: " << frobenius_norm(A) << endl;
cout << "- A*A:" << endl
<< A*A << endl;
cout << "- A^T:" << endl
<< transpose(A) << endl;
// // cout << "- testing row iterators:" << endl;
// // for (RawMatrix<double>::row_const_iterator it(N.rowbegin());
// // it != N.rowend(); ++it)
// // {
// // cout << " row " << it.index() << ": ";
// // for (RawMatrix<double>::row_const_iterator::entry_const_iterator ite(it.begin());
// // ite != it.end(); ++ite)
// // cout << ite.entry() << " ";
// // cout << endl;
// // }
// // cout << "- testing column iterators:" << endl;
// // for (RawMatrix<double>::col_const_iterator it(N.colbegin());
// // it != N.colend(); ++it)
// // {
// // cout << " column " << it.index() << ": ";
// // for (RawMatrix<double>::col_const_iterator::entry_const_iterator ite(it.begin());
// // ite != it.end(); ++ite)
// // cout << ite.entry() << " ";
// // cout << endl;
// // }
cout << "- a lower triangular default matrix:" << endl;
LowerTriangularMatrix<double> Ldefault;
cout << Ldefault;
cout << "- a lower triangular 3x3 matrix L:" << endl;
LowerTriangularMatrix<double> L(3);
cout << L;
cout << "- constructing a lower triangular matrix from a string:" << endl;
LowerTriangularMatrix<double> Lbyrow(4,4,"1 2 3 4 5 6 7 8 9 10");
cout << Lbyrow;
cout << "- constructing a lower triangular matrix from a string (columnwise):" << endl;
LowerTriangularMatrix<double> Lbycol(4,4,"1 2 4 7 3 5 8 6 9 10", false);
cout << Lbycol;
cout << "- testing apply(), using the vector ";
L = Lbyrow;
x.resize(L.column_dimension());
for (unsigned int i(0); i < x.size(); i++) x(i)=i+1;
cout << x << ", result: ";
y.resize(L.row_dimension());
L.apply(x,y);
cout << y << endl;
cout << "- testing apply_transposed(), result: ";
y.resize(L.column_dimension());
L.apply_transposed(x,y);
cout << y << endl;
cout << "- a lower triangular matrix, again:" << endl;
cout << Lbyrow;
LowerTriangularMatrix<double> Linv;
Lbyrow.inverse(Linv);
cout << "- the inverse of that matrix:" << endl;
cout << Linv;
cout << "- an upper triangular default matrix:" << endl;
UpperTriangularMatrix<double> Rdefault;
cout << Rdefault;
cout << "- an upper triangular 3x3 matrix R:" << endl;
UpperTriangularMatrix<double> R(3);
cout << R;
cout << "- constructing an upper triangular matrix from a string:" << endl;
UpperTriangularMatrix<double> Rbyrow(4,4,"1 2 3 4 5 6 7 8 9 10");
cout << Rbyrow;
cout << "- constructing an upper triangular matrix from a string (columnwise):" << endl;
UpperTriangularMatrix<double> Rbycol(4,4,"1 2 5 3 6 8 4 7 9 10", false);
cout << Rbycol;
cout << "- an upper triangular matrix, again:" << endl;
cout << Rbyrow;
UpperTriangularMatrix<double> Rinv;
Rbyrow.inverse(Rinv);
cout << "- the inverse of that matrix:" << endl;
cout << Rinv;
cout << "- testing apply(), using the vector ";
R = Rbyrow;
x.resize(R.column_dimension());
for (unsigned int i(0); i < x.size(); i++) x(i)=i+1;
cout << x << ", result: ";
y.resize(R.row_dimension());
R.apply(x,y);
cout << y << endl;
cout << "- testing apply_transposed(), result: ";
y.resize(R.column_dimension());
R.apply_transposed(x,y);
cout << y << endl;
cout << "- testing non-quadratic lower triangular matrices:" << endl;
LowerTriangularMatrix<double> Lnonq1(3,4,"1 2 3 4 5 6");
cout << Lnonq1;
cout << " (this has " << Lnonq1.size() << " nonzero entries)" << endl;
LowerTriangularMatrix<double> Lnonq2(4,3,"1 2 3 4 5 6 7 8 9");
cout << Lnonq2;
cout << " (this has " << Lnonq2.size() << " nonzero entries)" << endl;
cout << "- first matrix, testing apply() for x=" << x << ", result: ";
y.resize(Lnonq1.row_dimension());
Lnonq1.apply(x,y);
cout << y << endl;
cout << "- first matrix, testing apply_transposed() for x=";
x.resize(Lnonq1.row_dimension());
for (unsigned int i(0); i < x.size(); i++) x(i)=i+1;
cout << x << ", result: ";
y.resize(Lnonq1.column_dimension());
Lnonq1.apply_transposed(x,y);
cout << y << endl;
cout << "- second matrix, testing apply() for x=";
x.resize(Lnonq2.column_dimension());
for (unsigned int i(0); i < x.size(); i++) x(i)=i+1;
cout << x << ", result: ";
y.resize(Lnonq2.row_dimension());
Lnonq2.apply(x,y);
cout << y << endl;
cout << "- second matrix, testing apply_transposed() for x=";
x.resize(Lnonq2.row_dimension());
for (unsigned int i(0); i < x.size(); i++) x(i)=i+1;
cout << x << ", result: ";
y.resize(Lnonq2.column_dimension());
Lnonq2.apply_transposed(x,y);
cout << y << endl;
cout << "- testing non-quadratic upper triangular matrices:" << endl;
UpperTriangularMatrix<double> Rnonq1(3,4,"1 2 3 4 5 6 7 8 9");
cout << Rnonq1;
cout << " (this has " << Rnonq1.size() << " nonzero entries)" << endl;
UpperTriangularMatrix<double> Rnonq2(4,3,"1 2 3 4 5 6");
cout << Rnonq2;
cout << " (this has " << Rnonq2.size() << " nonzero entries)" << endl;
cout << "- first matrix, testing apply() for x=";
x.resize(Rnonq1.column_dimension());
for (unsigned int i(0); i < x.size(); i++) x(i)=i+1;
cout << x << ", result: ";
y.resize(Rnonq1.row_dimension());
Rnonq1.apply(x,y);
cout << y << endl;
cout << "- first matrix, testing apply_transposed() for x=";
x.resize(Rnonq1.row_dimension());
for (unsigned int i(0); i < x.size(); i++) x(i)=i+1;
cout << x << ", result: ";
y.resize(Rnonq1.column_dimension());
Rnonq1.apply_transposed(x,y);
cout << y << endl;
cout << "- second matrix, testing apply() for x=";
x.resize(Rnonq2.column_dimension());
for (unsigned int i(0); i < x.size(); i++) x(i)=i+1;
cout << x << ", result: ";
y.resize(Rnonq2.row_dimension());
Rnonq2.apply(x,y);
cout << y << endl;
cout << "- second matrix, testing apply_transposed() for x=";
x.resize(Rnonq2.row_dimension());
for (unsigned int i(0); i < x.size(); i++) x(i)=i+1;
cout << x << ", result: ";
y.resize(Rnonq2.column_dimension());
Rnonq2.apply_transposed(x,y);
cout << y << endl;
cout << "- a symmetric default matrix:" << endl;
SymmetricMatrix<double> S;
cout << S;
cout << "- a symmetric 3x3 matrix T:" << endl;
SymmetricMatrix<double> T(3);
cout << T;
cout << "- constructing a symmetric matrix from a string:" << endl;
SymmetricMatrix<double> Sbyrow(4,"1 2 3 4 5 6 7 8 9 10");
cout << Sbyrow;
cout << "- constructing a symmetric matrix from a string (columnwise):" << endl;
SymmetricMatrix<double>
Sbycol(3,"1 2 4 3 5 6",false); // note the permutation!
cout << Sbycol;
cout << "- a sparse default matrix:" << endl;
SparseMatrix<double> W;
cout << W;
cout << "- an empty 2x2 matrix X:" << endl;
SparseMatrix<double> X(2);
cout << X;
cout << "- set some entries to nontrivial values:" << endl;
X.set_entry(0, 0, -23.0);
X.set_entry(1, 0, 42.0);
cout << X;
cout <<"- test copy constructor:" << endl;
SparseMatrix<double> Ycopy(X);
cout << Ycopy;
cout << "- after X.resize(3,2):" << endl;
X.resize(3, 2);
cout << X;
cout << "- refill X again with some values:" << endl;
X.set_entry(0, 0, -3);
X.set_entry(0, 1, 2);
X.set_entry(1, 0, 1);
X.set_entry(1, 1, -1.5);
X.set_entry(2, 1, 3);
cout << X;
x.resize(2);
x[0] = 2; x[1] = 3;
cout << "- applying X to x=" << x << " yields" << endl;
X.apply(x, y);
cout << y << endl;
cout << "- applying X^T to y=" << y << " yields" << endl;
X.apply_transposed(y, x);
cout << x << endl;
cout << "- a 2x2 sparse diagonal matrix:" << endl;
SparseMatrix<double> Y;
Y.diagonal(2, 42.0);
cout << Y;
cout << "- putting different subblocks into this matrix:" << endl;
Matrix<double> sub1(1, 1, "23");
SymmetricMatrix<double> sub2(1, "7");
LowerTriangularMatrix<double> sub3(1, 1, "-1");
Y.set_block(0, 1, sub1);
Y.set_block(1, 0, sub2);
Y.set_block(1, 1, sub3);
cout << Y;
cout << "- putting a block into this matrix in reflect mode:" << endl;
Matrix<double> sub4(2, 2, "1 2 3 4");
Y.set_block(0, 0, sub4, true);
cout << Y;
cout << "- a 3x2 sparse matrix F1:" << endl;
SparseMatrix<double> F1(3, 2);
F1.set_entry(0, 0, 1.0);
F1.set_entry(1, 0, 2.0);
F1.set_entry(1, 1, 3.0);
F1.set_entry(2, 1, 4.0);
cout << F1;
cout << "- a 2x2 sparse matrix F2:" << endl;
SparseMatrix<double> F2(2);
F2.set_entry(0, 0, 1.5);
F2.set_entry(1, 0, 1.0);
F2.set_entry(1, 1, -2.5);
cout << F2;
SparseMatrix<double> F3;
F3 = F1 * F2;
cout << "- matrix product F1*F2:" << endl
<< F3;
SparseMatrix<double> small(2, 2);
small.set_entry(0, 0, 1e-5);
small.set_entry(0, 1, 2e-5);
small.set_entry(1, 0, 3e-5);
small.set_entry(1, 1, -1e-5);
cout << "- a sparse matrix with small entries:" << endl
<< small;
InfiniteVector<double, Vector<double>::size_type> v;
small.get_row(0, v);
cout << "- extracted a row from small as a sparse vector:" << endl
<< v;
small.get_row(1, v, 10);
cout << "- another row, now with offset 10:" << endl << v;
std::list<size_t> indices;
std::list<double> entries;
indices.push_back(1);
entries.push_back(-2.7);
small.set_row(0, indices, entries);
cout << "- the matrix small after setting the first row:" << endl
<< small;
small.compress(1.5e-5);
cout << "- small after compressing with 1.5e-5:" << endl
<< small;
small.get_row(1, v);
cout << "- extract last row again:" << endl << v;
Matrix<double> extract;
small.get_block(0, 1, 2, 1, extract);
cout << "- extracted a sub-block from small:" << endl
<< extract;
cout << "- the matrix F2 again:" << endl
<< F2
<< " and its transpose:" << endl
<< transpose(F2);
cout << "- a tridiagonal default matrix:" << endl;
TridiagonalMatrix<double> T1;
cout << T1;
cout << "- an empty 4x4 tridiagonal matrix T2:" << endl;
TridiagonalMatrix<double> T2(4);
cout << T2;
cout << "- set some entries to nontrivial values:" << endl;
T2.set_entry(0, 0, 1.0);
T2.set_entry(1, 1, 2.0);
T2.set_entry(2, 2, 3.0);
T2.set_entry(3, 3, 4.0);
T2.set_entry(0, 1, -1.0);
T2.set_entry(1, 2, -2.0);
T2.set_entry(2, 3, -3.0);
T2.set_entry(1, 0, -3.5);
T2.set_entry(2, 1, -2.5);
T2.set_entry(3, 2, -1.5);
cout << T2;
cout <<"- test copy constructor:" << endl;
TridiagonalMatrix<double> T2copy(T2);
cout << T2copy;
x.resize(4); y.resize(4);
x[0] = x[1] = x[2] = x[3] = 1;
cout << "- applying T2 to x=" << x << " yields" << endl;
T2.apply(x, y);
cout << y << endl;
cout << "- applying T2^T to x=" << x << " yields" << endl;
T2.apply_transposed(x, y);
cout << y << endl;
typedef Matrix<double> MATRIX;
MATRIX M1(2, 3, "1 2 3 4 5 6"), M2(2, 2, "1 2 0 3");
cout << "- a matrix M1=" << endl << M1;
cout << "- a matrix M2=" << endl << M2;
KroneckerMatrix<double,MATRIX,MATRIX> K(M1,M2,2.0);
cout << "- Kronecker product K of M1 and M2 with factor 2.0:" << endl << K;
x.resize(6); x[3] = 1;
K.apply(x, y);
cout << "- K applied to x=" << x << " yields Kx=" << y << endl;
x.resize(6); x[4] = 1;
K.apply(x, y);
cout << "- K applied to x=" << x << " yields Kx=" << y << endl;
y.resize(6);
x.resize(4); x[0] = 1;
K.apply_transposed(x, y);
cout << "- K^T applied to x=" << x << " yields (K^T)x=" << y << endl;
x.resize(4); x[1] = 1;
K.apply_transposed(x, y);
cout << "- K^T applied to x=" << x << " yields (K^T)x=" << y << endl;
Matrix<double> KM(K);
cout << "- Kronecker product of M1 and M2 as a Matrix<double> again:"
<< endl << KM;
Vector<double> w(3, "1 2 3");
Matrix<double> wMatrix(w);
cout << "- construct a matrix from a vector:" << endl
<< wMatrix;
wMatrix.reshape(1);
cout << "- reshape this matrix:" << endl
<< wMatrix;
M = Matrix<double>(2, 2, "1 3 2 4");
cout << "- another matrix M=" << endl << M;
Vector<double> colM; M.col(colM);
cout << "- col(M)=" << colM << endl;
M.resize(45,67);
M.decol(colM, 2);
cout << "- decol(col(M))=" << endl << M;
#if 1
cout << "- write M to a file..." << endl;
M.matlab_output("Mfile", "M", 0);
#endif
#if 0
F1.resize(5,2);
F1.set_entry(0,0,1);
F1.set_entry(0,1,2);
F1.set_entry(1,1,3);
F1.set_entry(2,0,4);
F1.set_entry(4,1,5);
cout << "- a sparse matrix F1 again:" << endl << F1;
cout << "- write F1 to a file..." << endl;
F1.matlab_output("F1", "F1", 1);
cout << " ... done" << endl;
F1.resize(1,1);
cout << "- resized F1:" << endl << F1;
cout << "- read F1 from the file again..." << endl;
F1.matlab_input("F1");
cout << " ...done, F1=" << endl << F1;
#endif
return 0;
}
void test_fov(bool view=false) {
PR2 brett(view);
rave::EnvironmentBasePtr env = brett.get_env();
sleep(2);
// choose any camera
brett.rarm->set_posture(Arm::Posture::mantis);
Camera* cam = brett.rcam;
// setup particles
rave::KinBodyPtr table = env->GetKinBody("table");
rave::KinBody::LinkPtr base = table->GetLink("base");
rave::Vector extents = base->GetGeometry(0)->GetBoxExtents();
rave::Vector table_pos = table->GetTransform().trans;
double x_min, x_max, y_min, y_max, z_min, z_max;
x_min = table_pos.x - extents.x;
x_max = table_pos.x + extents.x;
y_min = table_pos.y - extents.y;
y_max = table_pos.y + extents.y;
z_min = table_pos.z + extents.z - .1;
z_max = table_pos.z + extents.z + .1;
Matrix<double, 3, M> P;
for(int m=0; m < M; ++m) {
P(0,m) = mm_utils::uniform(x_min, x_max);
P(1,m) = mm_utils::uniform(y_min, y_max);
P(2,m) = mm_utils::uniform(z_min, z_max);
// rave_utils::plot_point(env, P.col(m), {0,1,0});
}
tc.start("beams");
std::vector<std::vector<Beam3d> > beams = cam->get_beams();
tc.stop("beams");
tc.start("border");
std::vector<Triangle3d> border = cam->get_border(beams);
tc.stop("border");
// for(int i=0; i < beams.size(); ++i) {
// for(int j=0; j < beams[i].size(); ++j) {
// beams[i][j].plot(env);
// }
// }
for(int i=0; i < border.size(); ++i) {
border[i].plot(env);
}
tc.start("sd");
Matrix<double,M,1> sd;
for(int m=0; m < M; ++m) {
sd(m) = cam->signed_distance(P.col(m), beams, border);
// std::cout << "sd: " << sd(m) << "\n";
// rave_utils::plot_point(env, P.col(m), {0,1,0});
// std::cin.ignore();
// rave_utils::clear_plots(3);
}
tc.stop("sd");
tc.print_all_elapsed();
std::cin.ignore();
}
void update(int value)
{
frameTimer.start();
// Read the experiment from file, if the file is finished exit suddenly
if ( inputStream.eof() )
{ exit(0);
}
if ( isReading )
{ // This reads a line (frame) in inputStream
readline(inputStream, trialNumber, headCalibration, trialMode, pointMatrix );
headEyeCoords.update(pointMatrix.col(0),pointMatrix.col(1),pointMatrix.col(2));
Affine3d active = headEyeCoords.getRigidStart().getFullTransformation();
eulerAngles.init( headEyeCoords.getRigidStart().getFullTransformation().rotation() );
eyeLeft = headEyeCoords.getLeftEye();
eyeRight= headEyeCoords.getRightEye();
//cerr << eyeRight.transpose() << endl;
cyclopeanEye = (eyeLeft+eyeRight)/2.0;
if ( trialMode == STIMULUSMODE )
stimulusFrames++;
if ( trialMode == FIXATIONMODE )
stimulusFrames=0;
// Projection of view normal on the focal plane
Vector3d directionOfSight = (active.rotation()*Vector3d(0,0,-1)).normalized();
Eigen::ParametrizedLine<double,3> lineOfSightRight = Eigen::ParametrizedLine<double,3>::Through( eyeRight , eyeRight+directionOfSight );
double lineOfSightRightDistanceToFocalPlane = lineOfSightRight.intersection(focalPlane);
//double lenghtOnZ = (active*(center-eyeCalibration )+eyeRight).z();
projPointEyeRight = lineOfSightRightDistanceToFocalPlane *(directionOfSight)+ (eyeRight);
// second projection the fixation point computed with z non constant but perfectly parallel to projPointEyeRight
lineOfSightRightDistanceToFocalPlane= (( active.rotation()*(center)) - eyeRight).norm();
Vector3d secondProjection = lineOfSightRightDistanceToFocalPlane *(directionOfSight)+ (eyeRight);
projPointEyeRight=secondProjection ;
// Compute the translation to move the eye in order to avoid share components
Vector3d posAlongLineOfSight = (headEyeCoords.getRigidStart().getFullTransformation().rotation())*(eyeRight -eyeCalibration);
// GENERATION OF PASSIVE MODE.
// HERE WE MOVE THE SCREEN TO FACE THE OBSERVER's EYE
if ( passiveMode )
{ initProjectionScreen(0, headEyeCoords.getRigidStart().getFullTransformation()*Translation3d(center));
}
else
initProjectionScreen(focalDistance, Affine3d::Identity());
if ( trialMode == STIMULUSMODE )
{
// IMPORTANT Reset the previous status of transformations
objectActiveTransformation[0].setIdentity();
objectActiveTransformation[1].setIdentity();
// PLANE 0 Transformation QUELLO CHE STA SOTTO
alpha = atan( eyeRight.x()/abs(projPointEyeRight.z()) );
if ( overallTilt )
{
instantPlaneSlant = alphaMultiplier*alpha+toRadians(-factors.at("DeltaSlant")-factors.at("StillPlaneSlant"));
AngleAxis<double> aa0( instantPlaneSlant,Vector3d::UnitY());
objectActiveTransformation[0]*=aa0;
double planesYOffset = factors.at("PlanesCentersYDistance")*(whichPlaneDrawUp ? 1 : -1);
objectActiveTransformation[0].translation() = Vector3d(0,planesYOffset,focalDistance);
// PLANE 1 Transformation QUELLO CHE STA SOPRA
AngleAxis<double> aa1(-toRadians(factors.at("StillPlaneSlant")),Vector3d::UnitY());
objectActiveTransformation[1]*=aa1;
objectActiveTransformation[1].translation() = Vector3d(0,-planesYOffset,focalDistance);
}
else
{
instantPlaneSlant = alphaMultiplier*alpha+toRadians(factors.at("DeltaSlant")+factors.at("StillPlaneSlant"));
AngleAxis<double> aa0( instantPlaneSlant,Vector3d::UnitY());
objectActiveTransformation[0]*=aa0;
double planesYOffset = factors.at("PlanesCentersYDistance")*(whichPlaneDrawUp ? 1 : -1);
objectActiveTransformation[0].translation() = Vector3d(0,planesYOffset,focalDistance);
// PLANE 1 Transformation QUELLO CHE STA SOPRA
AngleAxis<double> aa1(toRadians(factors.at("StillPlaneSlant")),Vector3d::UnitY());
objectActiveTransformation[1]*=aa1;
objectActiveTransformation[1].translation() = Vector3d(0,-planesYOffset,focalDistance);
}
objectPassiveTransformation[0] = ( cam.getModelViewMatrix()*objectActiveTransformation[0] );
objectPassiveTransformation[1] = ( cam.getModelViewMatrix()*objectActiveTransformation[1] );
//cout << toDegrees(instantPlaneSlant) << endl;
// **************** COMPUTE THE OPTIC FLOWS **************************
// 1) Project the points to screen by computing their coordinates on focalPlane in passive (quite complicate, see the specific method)
// *********** FOR THE MOVING PLANE *************
vector<Vector3d> projPointsMovingPlane = stimDrawer[0].projectStimulusPoints(objectActiveTransformation[0],headEyeCoords.getRigidStart().getFullTransformation(),cam,focalDistance, screen, eyeCalibration,passiveMode,false);
// 2) Get the angles formed by stimulus and observer
// updating with the latest values
Vector3d oldAlphaMoving = flowsAnglesAlphaMoving,oldBetaMoving=flowsAnglesBetaMoving;
// alpha is the "pitch" angle, beta is the "yaw" angle
// Here me must use the points 4,5,8 of the stimulus
flowsAnglesAlphaMoving(0) = ( atan2(projPointsMovingPlane[4].x(), abs(focalDistance) ) );
flowsAnglesAlphaMoving(1) = ( atan2(projPointsMovingPlane[5].x(), abs(focalDistance) ) );
flowsAnglesAlphaMoving(2) = ( atan2(projPointsMovingPlane[8].x(), abs(focalDistance) ) );
flowsAnglesBetaMoving(0) = ( atan2(projPointsMovingPlane[4].y(), abs(focalDistance) ) );
flowsAnglesBetaMoving(1) = ( atan2(projPointsMovingPlane[5].y(), abs(focalDistance) ) );
flowsAnglesBetaMoving(2) = ( atan2(projPointsMovingPlane[8].y(), abs(focalDistance) ) );
// 3) Fill the matrix of derivatives
MatrixXd angVelocitiesMoving(6,1);
angVelocitiesMoving(0) = flowsAnglesAlphaMoving(0)-oldAlphaMoving(0);
angVelocitiesMoving(1) = flowsAnglesBetaMoving(0)-oldBetaMoving(0);
angVelocitiesMoving(2) = flowsAnglesAlphaMoving(1)-oldAlphaMoving(1);
angVelocitiesMoving(3) = flowsAnglesBetaMoving(1)-oldBetaMoving(1);
angVelocitiesMoving(4) = flowsAnglesAlphaMoving(2)-oldAlphaMoving(2);
angVelocitiesMoving(5) = flowsAnglesBetaMoving(2)-oldBetaMoving(2);
angVelocitiesMoving /= ((double)TIMER_MS/(double)1000);
// 4) Fill the coefficient matrix, to solve the linear system
MatrixXd coeffMatrixMoving(6,6);
coeffMatrixMoving <<
1, flowsAnglesAlphaMoving(0), flowsAnglesBetaMoving(0), 0, 0, 0,
0, 0, 0, 1,flowsAnglesAlphaMoving(0),flowsAnglesBetaMoving(0),
1, flowsAnglesAlphaMoving(1), flowsAnglesBetaMoving(1), 0, 0, 0,
0, 0, 0, 1,flowsAnglesAlphaMoving(1),flowsAnglesBetaMoving(1),
1, flowsAnglesAlphaMoving(2), flowsAnglesBetaMoving(2), 0, 0, 0,
0, 0, 0, 1,flowsAnglesAlphaMoving(2),flowsAnglesBetaMoving(2)
;
// 5) Solve the linear system by robust fullPivHouseholderQR decomposition (see Eigen for details http://eigen.tuxfamily.org/dox/TutorialLinearAlgebra.html )
MatrixXd velocitiesMoving = coeffMatrixMoving.colPivHouseholderQr().solve(angVelocitiesMoving);
// 6) Write the output to file flowsFileMoving
flowsFileMoving << fixed << trialNumber << "\t" << //1
stimulusFrames << " " <<
factors.at("DeltaSlant")<< " " <<
factors.at("StillPlaneSlant") << " " <<
overallTilt << " " <<
projPointsMovingPlane[4].transpose() << " " <<
projPointsMovingPlane[5].transpose() << " " <<
projPointsMovingPlane[8].transpose() << " " <<
angVelocitiesMoving.transpose() << " " <<
velocitiesMoving.transpose() << endl;
// ********************* FLOWS FOR THE STILL PLANE **************
// Here we must repeat the same things for the still plane
vector<Vector3d> projPointsStillPlane = stimDrawer[1].projectStimulusPoints(objectActiveTransformation[1],headEyeCoords.getRigidStart().getFullTransformation(),cam,focalDistance, screen, eyeCalibration,passiveMode,false);
// 2) Get the angles formed by stimulus and observer
// updating with the latest values
Vector3d oldAlphaStill = flowsAnglesAlphaStill,oldBetaStill=flowsAnglesBetaStill;
// alpha is the "pitch" angle, beta is the "yaw" angle
// Here me must use the points 4,5,8 of the stimulus
flowsAnglesAlphaStill(0) = ( atan2(projPointsStillPlane[4].x(), abs(focalDistance) ) );
flowsAnglesAlphaStill(1) = ( atan2(projPointsStillPlane[5].x(), abs(focalDistance) ) );
flowsAnglesAlphaStill(2) = ( atan2(projPointsStillPlane[8].x(), abs(focalDistance) ) );
flowsAnglesBetaStill(0) = ( atan2(projPointsStillPlane[4].y(), abs(focalDistance) ) );
flowsAnglesBetaStill(1) = ( atan2(projPointsStillPlane[5].y(), abs(focalDistance) ) );
flowsAnglesBetaStill(2) = ( atan2(projPointsStillPlane[8].y(), abs(focalDistance) ) );
// 3) Fill the matrix of derivatives
MatrixXd angVelocitiesStill(6,1);
angVelocitiesStill(0) = flowsAnglesAlphaStill(0)-oldAlphaStill(0);
angVelocitiesStill(1) = flowsAnglesBetaStill(0)-oldBetaStill(0);
angVelocitiesStill(2) = flowsAnglesAlphaStill(1)-oldAlphaStill(1);
angVelocitiesStill(3) = flowsAnglesBetaStill(1)-oldBetaStill(1);
angVelocitiesStill(4) = flowsAnglesAlphaStill(2)-oldAlphaStill(2);
angVelocitiesStill(5) = flowsAnglesBetaStill(2)-oldBetaStill(2);
angVelocitiesStill /= ((double)TIMER_MS/(double)1000);
// 4) Fill the coefficient matrix, to solve the linear system
MatrixXd coeffMatrixStill(6,6);
coeffMatrixStill <<
1, flowsAnglesAlphaStill(0), flowsAnglesBetaStill(0), 0, 0, 0,
0, 0, 0, 1,flowsAnglesAlphaStill(0),flowsAnglesBetaStill(0),
1, flowsAnglesAlphaStill(1), flowsAnglesBetaStill(1), 0, 0, 0,
0, 0, 0, 1,flowsAnglesAlphaStill(1),flowsAnglesBetaStill(1),
1, flowsAnglesAlphaStill(2), flowsAnglesBetaStill(2), 0, 0, 0,
0, 0, 0, 1,flowsAnglesAlphaStill(2),flowsAnglesBetaStill(2)
;
// 5) Solve the linear system by robust fullPivHouseholderQR decomposition (see Eigen for details http://eigen.tuxfamily.org/dox/TutorialLinearAlgebra.html )
MatrixXd velocitiesStill = coeffMatrixStill.colPivHouseholderQr().solve(angVelocitiesStill);
// 6) Write the output to file flowsFileStill
flowsFileStill << fixed << trialNumber << "\t" << // 1
stimulusFrames << " " << // 2
factors.at("DeltaSlant")<< " " << // 3
factors.at("StillPlaneSlant") << " " << // 4
overallTilt << " " <<
projPointsStillPlane[4].transpose() << " " << // 5,6,7
projPointsStillPlane[5].transpose() << " " << // 8,9,10
projPointsStillPlane[8].transpose() << " " << // 11,12,13
angVelocitiesStill.transpose() << " " << // 14, 15, 16, 17, 18, 19
velocitiesStill.transpose() << endl; // 20, 21, 22, 23, 24, 25
// **************** END OF OPTIC FLOWS COMPUTATION
}
/*
ofstream outputfile;
outputfile.open("data.dat");
outputfile << "Subject Name: " << parameters.find("SubjectName") << endl;
outputfile << "Passive matrix:" << endl << objectPassiveTransformation.matrix() << endl;
outputfile << "Yaw: " << toDegrees(eulerAngles.getYaw()) << endl <<"Pitch: " << toDegrees(eulerAngles.getPitch()) << endl;
outputfile << "EyeLeft: " << headEyeCoords.getLeftEye().transpose() << endl;
outputfile << "EyeRight: " << headEyeCoords.getRightEye().transpose() << endl << endl;
outputfile << "Factors:" << endl;
for (map<string,double>::iterator iter=factors.begin(); iter!=factors.end(); ++iter)
{ outputfile << "\t\t" << iter->first << "= " << iter->second << endl;
}
*/
}
if ( trialMode == PROBEMODE )
isReading=false;
glutPostRedisplay();
glutTimerFunc(TIMER_MS, update, 0);
}
void update(int value)
{ // Read the experiment from file, if the file is finished exit suddenly
if ( inputStream.eof() )
{ cleanup();
exit(0);
}
if ( isReading )
{ // This reads a line (frame) in inputStream
readline(inputStream, trialNumber, headCalibration, trialMode, pointMatrix );
headEyeCoords.update(pointMatrix.col(0),pointMatrix.col(1),pointMatrix.col(2));
Affine3d active = headEyeCoords.getRigidStart().getFullTransformation();
eulerAngles.init( headEyeCoords.getRigidStart().getFullTransformation().rotation() );
eyeLeft = headEyeCoords.getLeftEye();
eyeRight= headEyeCoords.getRightEye();
cyclopeanEye = (eyeLeft+eyeRight)/2.0;
if ( trialMode == STIMULUSMODE )
stimulusFrames++;
if ( trialMode == FIXATIONMODE )
stimulusFrames=0;
// Projection of view normal on the focal plane
Vector3d directionOfSight = (active.rotation()*Vector3d(0,0,-1)).normalized();
Eigen::ParametrizedLine<double,3> lineOfSightRight = Eigen::ParametrizedLine<double,3>::Through( eyeRight , eyeRight+directionOfSight );
Eigen::ParametrizedLine<double,3> lineOfSightLeft = Eigen::ParametrizedLine<double,3>::Through( eyeLeft, eyeLeft+directionOfSight );
double lineOfSightRightDistanceToFocalPlane = lineOfSightRight.intersection(focalPlane);
double lineOfSightLeftDistanceToFocalPlane = lineOfSightLeft.intersection(focalPlane);
//double lenghtOnZ = (active*(center-eyeCalibration )+eyeRight).z();
projPointEyeRight = lineOfSightRightDistanceToFocalPlane *(directionOfSight)+ (eyeRight);
projPointEyeLeft= lineOfSightLeftDistanceToFocalPlane * (directionOfSight) + (eyeLeft);
// second projection the fixation point computed with z non constant but perfectly parallel to projPointEyeRight
lineOfSightRightDistanceToFocalPlane= (( active.rotation()*(center)) - eyeRight).norm();
Vector3d secondProjection = lineOfSightRightDistanceToFocalPlane *(directionOfSight)+ (eyeRight);
if ( !zOnFocalPlane )
projPointEyeRight=secondProjection ;
// Compute the translation to move the eye in order to avoid share components
Vector3d posAlongLineOfSight = (headEyeCoords.getRigidStart().getFullTransformation().rotation())*(eyeRight -eyeCalibration);
// GENERATION OF PASSIVE MODE.
// HERE WE MOVE THE SCREEN TO FACE THE OBSERVER's EYE
if ( passiveMode )
{
initProjectionScreen(0, headEyeCoords.getRigidStart().getFullTransformation()*Translation3d(center));
}
else
initProjectionScreen(focalDistance, Affine3d::Identity());
objectPassiveTransformation = ( cam.getModelViewMatrix()*objectActiveTransformation );
ofstream outputfile;
outputfile.open("data.dat");
outputfile << "Subject Name: " << parameters.find("SubjectName") << endl;
outputfile << "Passive matrix:" << endl << objectPassiveTransformation.matrix() << endl;
outputfile << "Yaw: " << toDegrees(eulerAngles.getYaw()) << endl <<"Pitch: " << toDegrees(eulerAngles.getPitch()) << endl;
outputfile << "EyeLeft: " << headEyeCoords.getLeftEye().transpose() << endl;
outputfile << "EyeRight: " << headEyeCoords.getRightEye().transpose() << endl << endl;
outputfile << "Slant: " << instantPlaneSlant << endl;
outputfile << "Factors:" << endl;
for (map<string,double>::iterator iter=factors.begin(); iter!=factors.end(); ++iter)
{
outputfile << "\t\t" << iter->first << "= " << iter->second << endl;
}
}
if ( trialMode == PROBEMODE )
isReading=false;
glutPostRedisplay();
glutTimerFunc(TIMER_MS, update, 0);
}
void odom_callback(const nav_msgs::Odometry msg) {
odom.col(0) = odom.col(1);
odom.col(1) << msg.pose.pose.position.x, -msg.pose.pose.position.y, -tf::getYaw(msg.pose.pose.orientation);
};
Matrix<double, k, m> aug_mcl(Matrix<double, k, m> belief, Matrix<double, k, 2> u, double (*meas_model)(Matrix<double, k, 1>), Matrix<double, k, 1> (*g)(Matrix<double, k, 1>, Matrix<double, k, 2>)) {
// Moving averages
static double w_slow = 0.0000001;
static double w_fast = 0;
// Decay parameters
const double a_slow = 0.01;
const double a_fast = 0.5;
// Initialize particle set matrices for prediction and final current belief
auto x_p = Matrix<double, k, m>();
auto x = Matrix<double, k, m>();
auto w = std::vector<double>(m);
double w_avg = 0;
// Iterate over previous belief particles
for(int i = 0; i < m; i++) {
// Use motion model to calculate new prediction
Matrix<double, k, 1> x_p_i = g(belief.col(i), u);
x_p.col(i) = x_p_i;
// Work out weights using measurement model
w[i] = meas_model(x_p_i);
// Is overall likelihood of our measurements given our current beliefs really bad?
w_avg += w[i] / m;
}
// Accumulate weights so it's easy to pick a weighted sample
std::partial_sum(w.begin(), w.end(), w.begin());
double total_w = w.back();
// Slow moving average conforms to overall measurement likelihood slowly, fast moving etc.
w_slow += a_slow * (w_avg - w_slow);
w_fast += a_fast * (w_avg - w_fast);
// Don't resample if no changes greater than a predefined value
// bool shouldResample = ((u.col(1) - u.col(0)).array().abs() > 1e-4).any();
// if (!shouldResample) {
// ROS_INFO("Not resampling!");
// return x_p;
// }
// If overall likelihood of measurement being correct has been low for some time, start relocalizing
double likelihood = std::max(0.0, 1.0 - w_fast / w_slow);
// ROS_INFO("%f, %f, %f, %f", w_avg, w_fast, w_slow, likelihood);
// Update particle set
for(int i = 0; i < m; i++) {
// Re-localize
if (drand() < likelihood) {
x.col(i) << introduce_particle(x_p.rowwise().mean());
// Otherwise, resample from existing particle set
} else {
double rnd = drand() * total_w;
int j = std::lower_bound(w.begin(), w.end(), rnd) - w.begin(); // Find first element with weight greater than rnd
x.col(i) = x_p.col(j);
}
}
return x;
}
GMMDesc initutil::gonzalezForGMM(commonutil::DataSet const& input, idx_type k, std::mt19937& gen, bool use2GMM, fp_type sampleFactor)
{
// std::cout << "sampleFactor = " << sampleFactor << std::endl;
// std::cout << "use2GMM = " << use2GMM << std::endl;
idx_type sampleSize = sampleFactor * input.points.cols();
idx_type d = input.points.rows();
idx_type n = input.points.cols();
initutil::check(k, d, n);
GMMDesc desc;
Vector sample;
fp_type trace;
if(!use2GMM)
{
GMMDesc gmmdesc = gmmutil::optimalGaussian(input);
Matrix covar = gmmdesc.covariances.at(0);
trace = covar.trace()/(10.*d*k);
desc.means.resize(0,0);
desc.weights.resize(0);
}
std::uniform_real_distribution<> urd(0,1);
std::uniform_int_distribution<> uid(0,input.points.cols()-1);
Matrix randMatrix(d,d);
Matrix randOrthonormalMatrix(d,d);
Vector eigenvalues(d);
for (idx_type i=0; i<k; ++i)
{
if (i==0)
// draw first point uniformly
sample = input.points.col(commonutil::randomIndex(input.weights, gen));
else
{
// draw next point w.r.t. current mixture
if(sampleFactor < 1)
{
commonutil::DataSet samples;
samples.points.resize(d,sampleSize);
samples.weights.resize(sampleSize);
idx_type index;
for(idx_type j=0; j<sampleSize; ++j){
index = uid(gen);
// std::cout << "index = " << index << std::endl;
samples.points.col(j) = input.points.col(index);
samples.weights(j) = input.weights(index);
}
Vector densities = gmmutil::adaptiveDensities(samples, desc, 1.);
// std::cout << "densities.size() = " << densities.size() << std::endl;
densities.maxCoeff(&index);
sample = samples.points.col(index);
}
else
{
idx_type index;
Vector densities = gmmutil::adaptiveDensities(input, desc, 1.);
// std::cout << "densities.size() = " << densities.size() << std::endl;
densities.maxCoeff(&index);
sample = input.points.col(index);
}
}
if(use2GMM)
{
Matrix tmpMeans = Matrix::Zero(d,i+1);
if(i>0)
tmpMeans.block(0,0,d,i) = desc.means;
tmpMeans.col(i) = sample;
desc = kmeansutil::meansToGMM(input, tmpMeans);
}
else
{
desc.means.conservativeResize(d,i+1);
desc.means.col(i) = sample;
desc.weights.conservativeResize(i+1);
// random covariance
for (idx_type i=0; i<d; ++i)
for (idx_type j=0; j<d; ++j)
randMatrix(i,j)=urd(gen);
Eigen::HouseholderQR<Matrix> qr(randMatrix);
randOrthonormalMatrix = qr.householderQ();
commonutil::fill(eigenvalues,gen, 1.,10.);
fp_type tmp = trace/eigenvalues.sum();
eigenvalues = tmp * eigenvalues;
randMatrix = randOrthonormalMatrix.transpose()*eigenvalues.asDiagonal()*randOrthonormalMatrix;
desc.covariances.push_back(randMatrix);
}
}
if(!use2GMM)
{
// estimate weights
desc.weights = Vector::Zero(k);
std::vector<idx_type> partition = gmmutil::gaussPartition(input.points, desc);
for(idx_type n=0; n<input.points.cols(); ++n)
++desc.weights(partition.at(n));
desc.weights /= input.points.cols();
}
return desc;
}
/* ************************************************************************* */
pair<Matrix, Vector> GaussianFactorGraph::jacobian(
boost::optional<const Ordering&> optionalOrdering) const {
Matrix augmented = augmentedJacobian(optionalOrdering);
return make_pair(augmented.leftCols(augmented.cols() - 1),
augmented.col(augmented.cols() - 1));
}
/*
* Compute basic calibration parameters for a three axis gyroscope.
* The measurement equation is
* gyro_k = accelSensitivity * \tilde{accel}_k + bias + omega
*
* where, omega is the true angular rate (assumed to be zero)
* bias is the sensor bias
* accelSensitivity is the 3x3 matrix of sensitivity scale factors
* \tilde{accel}_k is the calibrated measurement of the accelerometer
* at time k
*
* After calibration, the optimized gyro measurement is given by
* \tilde{gyro}_k = gyro_k - bias - accelSensitivity * \tilde{accel}_k
*/
void gyroscope_calibration(Vector3f & bias,
Matrix3f & accelSensitivity,
Vector3f gyroSamples[],
Vector3f accelSamples[],
size_t n_samples)
{
// Assume the following measurement model:
// y = H*x
// where x is the vector of unknowns, and y is the measurement vector.
// the vector of unknowns is
// [a_x a_y a_z b_x]
// The measurement vector is
// [gyro_x]
// and the measurement matrix H is
// [accelSample_x accelSample_y accelSample_z 1]
// Note that the individual solutions for x are given by
// (H^T \times H)^-1 \times H^T y
// Everything is identical except for y and x. So, transform it
// into block form X = (H^T \times H)^-1 \times H^T Y
// where each column of X contains the partial solution for each
// column of y.
// Construct the matrix of accelerometer samples. Intermediate results
// are computed in "twice the precision that the source provides and the
// result deserves" by Kahan's thumbrule to prevent numerical problems.
Matrix<double, Dynamic, 4> H(n_samples, 4);
// And the matrix of gyro samples.
Matrix<double, Dynamic, 3> Y(n_samples, 3);
for (unsigned i = 0; i < n_samples; ++i) {
H.row(i) << accelSamples[i].transpose().cast<double>(), 1.0;
Y.row(i) << gyroSamples[i].transpose().cast<double>();
}
#if 1
Matrix<double, 4, 3> result;
// Use the cholesky-based Penrose pseudoinverse method.
(H.transpose() * H).ldlt().solve(H.transpose() * Y, &result);
// Transpose the result and return it to the caller. Cast back to float
// since there really isn't that much accuracy in the result.
bias = result.row(3).transpose().cast<float>();
accelSensitivity = result.block<3, 3>(0, 0).transpose().cast<float>();
#else
// TODO: Compare this result with a total-least-squares model.
size_t n = 4;
Matrix<double, Dynamic, 7> C;
C << H, Y;
SVD<Matrix<double, Dynamic, 7> > usv(C);
Matrix<double, 4, 3> V_ab = usv.matrixV().block<4, 3>(0, n);
Matrix<double, Dynamic, 3> V_bb = usv.matrixV().corner(BottomRight, n_samples - 4, 3);
// X = -V_ab/V_bb;
// X^T = (A * B^-1)^T
// X^T = (B^-1^T * A^T)
// X^T = (B^T^-1 * A^T)
// V_bb is orthgonal but not orthonormal. QR decomposition
// should be very fast in this case.
Matrix<double, 3, 4> result;
V_bb.transpose().qr().solve(-V_ab.transpose(), &result);
// Results only deserve single precision.
bias = result.col(3).cast<float>();
accelSensitivity = result.block<3, 3>(0, 0).cast<float>();
#endif // if 1
}
int contactConstraintsBV(
const RigidBodyTree &r, const KinematicsCache<double> &cache, int nc,
std::vector<double> support_mus,
std::vector<SupportStateElement,
Eigen::aligned_allocator<SupportStateElement>> &supp,
MatrixXd &B, MatrixXd &JB, MatrixXd &Jp, VectorXd &Jpdotv,
MatrixXd &normals) {
int j, k = 0, nq = r.number_of_positions();
B.resize(3, nc * 2 * m_surface_tangents);
JB.resize(nq, nc * 2 * m_surface_tangents);
Jp.resize(3 * nc, nq);
Jpdotv.resize(3 * nc);
normals.resize(3, nc);
Vector3d contact_pos, pos, normal;
MatrixXd J(3, nq);
Matrix<double, 3, m_surface_tangents> d;
for (std::vector<SupportStateElement,
Eigen::aligned_allocator<SupportStateElement>>::iterator
iter = supp.begin();
iter != supp.end(); iter++) {
double mu = support_mus[iter - supp.begin()];
double norm = sqrt(1 + mu * mu); // because normals and ds are orthogonal,
// the norm has a simple form
if (nc > 0) {
for (auto pt_iter = iter->contact_pts.begin();
pt_iter != iter->contact_pts.end(); pt_iter++) {
contact_pos = r.transformPoints(cache, *pt_iter, iter->body_idx, 0);
J = r.transformPointsJacobian(cache, *pt_iter, iter->body_idx, 0, true);
normal = iter->support_surface.head(3);
surfaceTangents(normal, d);
for (j = 0; j < m_surface_tangents; j++) {
B.col(2 * k * m_surface_tangents + j) =
(normal + mu * d.col(j)) / norm;
B.col((2 * k + 1) * m_surface_tangents + j) =
(normal - mu * d.col(j)) / norm;
JB.col(2 * k * m_surface_tangents + j) =
J.transpose() * B.col(2 * k * m_surface_tangents + j);
JB.col((2 * k + 1) * m_surface_tangents + j) =
J.transpose() * B.col((2 * k + 1) * m_surface_tangents + j);
}
// store away kin sols into Jp and Jpdotv
// NOTE: I'm cheating and using a slightly different ordering of J and
// Jdot here
Jp.block(3 * k, 0, 3, nq) = J;
Vector3d pt = (*pt_iter).head(3);
Jpdotv.block(3 * k, 0, 3, 1) =
r.transformPointsJacobianDotTimesV(cache, pt, iter->body_idx, 0);
normals.col(k) = normal;
k++;
}
}
}
return k;
}
#ifndef BTL_AO_NORM_POSE_HEADER
#define BTL_AO_NORM_POSE_HEADER
#include "common/OtherUtil.hpp"
#include <limits>
#include <Eigen/Dense>
#include "NormalAOPoseAdapter.hpp"
#include "AbsoluteOrientation.hpp"
using namespace Eigen;
using namespace std;
template<typename POSE_T, typename POINT_T>
Matrix<POSE_T, 3, 1> find_opt_cc(NormalAOPoseAdapter<POSE_T, POINT_T>& adapter)
{
//the R has been fixed, we need to find optimal cc, camera center, given n pairs of 2-3 correspondences
//Slabaugh, G., Schafer, R., & Livingston, M. (2001). Optimal Ray Intersection For Computing 3D Points From N -View Correspondences.
typedef Matrix<POSE_T, 3, 1> V3;
typedef Matrix<POSE_T, 3, 3> M3;
M3 Rwc = adapter.getRcw().inverse().matrix();
M3 AA; AA.setZero();
V3 bb; bb.setZero();
for (int i = 0; i < adapter.getNumberCorrespondences(); i++)
{
if (adapter.isInlier23(i)){
V3 vr_w = Rwc * adapter.getBearingVector(i).template cast<POSE_T>();
M3 A;
A(0,0) = 1 - vr_w(0)*vr_w(0);
A(1,0) = A(0,1) = - vr_w(0)*vr_w(1);
A(2,0) = A(0,2) = - vr_w(0)*vr_w(2);
A(1,1) = 1 - vr_w(1)*vr_w(1);
A(2,1) = A(1,2) = - vr_w(1)*vr_w(2);
A(2,2) = 1 - vr_w(2)*vr_w(2);
V3 b = A * adapter.getPointGlob(i).template cast<POSE_T>();
AA += A;
bb += b;
}
}
V3 c_w;
if (fabs(AA.determinant()) < POSE_T(0.0001))
c_w = V3(numeric_limits<POSE_T>::quiet_NaN(), numeric_limits<POSE_T>::quiet_NaN(), numeric_limits<POSE_T>::quiet_NaN());
else
c_w = AA.jacobiSvd(ComputeFullU | ComputeFullV).solve(bb);
return c_w;
}
template< typename POSE_T, typename POINT_T >
bool assign_sample(const NormalAOPoseAdapter<POSE_T, POINT_T>& adapter,
const vector<int>& selected_cols_,
Matrix<POINT_T, Dynamic, Dynamic>* p_X_w_, Matrix<POINT_T, Dynamic, Dynamic>* p_N_w_,
Matrix<POINT_T, Dynamic, Dynamic>* p_X_c_, Matrix<POINT_T, Dynamic, Dynamic>* p_N_c_, Matrix<POINT_T, Dynamic, Dynamic>* p_bv_){
int K = (int)selected_cols_.size() - 1;
bool use_shinji = false;
int nValid = 0;
for (int nSample = 0; nSample < K; nSample++) {
p_X_w_->col(nSample) = adapter.getPointGlob(selected_cols_[nSample]);
p_N_w_->col(nSample) = adapter.getNormalGlob(selected_cols_[nSample]);
p_bv_->col(nSample) = adapter.getBearingVector(selected_cols_[nSample]);
if (adapter.isValid(selected_cols_[nSample])){
p_X_c_->col(nSample) = adapter.getPointCurr(selected_cols_[nSample]);
p_N_c_->col(nSample) = adapter.getNormalCurr(selected_cols_[nSample]);
nValid++;
}
}
if (nValid == K)
use_shinji = true;
//assign the fourth elements for
p_X_w_->col(3) = adapter.getPointGlob(selected_cols_[3]);
p_N_w_->col(3) = adapter.getNormalGlob(selected_cols_[3]);
p_bv_->col(3) = adapter.getBearingVector(selected_cols_[3]);
return use_shinji;
}
template<typename POSE_T, typename POINT_T>
void nl_2p( const Matrix<POINT_T,3,1>& pt1_c, const Matrix<POINT_T,3,1>& nl1_c, const Matrix<POINT_T,3,1>& pt2_c,
const Matrix<POINT_T,3,1>& pt1_w, const Matrix<POINT_T,3,1>& nl1_w, const Matrix<POINT_T,3,1>& pt2_w,
SE3Group<POSE_T>* p_solution){
//Drost, B., Ulrich, M., Navab, N., & Ilic, S. (2010). Model globally, match locally: Efficient and robust 3D object recognition. In CVPR (pp. 998?005). Ieee. http://doi.org/10.1109/CVPR.2010.5540108
typedef Matrix<POINT_T, Dynamic, Dynamic> MatrixX;
typedef Matrix<POSE_T, 3, 1> V3;
typedef SO3Group<POSE_T> ROTATION;
typedef SE3Group<POSE_T> RT;
V3 c_w = pt1_w.template cast<POSE_T>(); // c_w is the origin of coordinate g w.r.t. world
POSE_T alpha = acos(nl1_w(0)); // rotation nl1_c to x axis (1,0,0)
V3 axis( 0, nl1_w(2), -nl1_w(1)); //rotation axis between nl1_c to x axis (1,0,0) i.e. cross( nl1_w, x );
axis.normalized();
//verify quaternion and rotation matrix
Quaternion<POSE_T> q_g_f_w(AngleAxis<POSE_T>(alpha, axis));
//cout << q_g_f_w << endl;
ROTATION R_g_f_w(q_g_f_w);
//cout << R_g_f_w << endl;
V3 nl_x = R_g_f_w * nl1_w.template cast<POSE_T>();
axis.normalized();
V3 c_c = pt1_c.template cast<POSE_T>();
POSE_T beta = acos(nl1_c(0)); //rotation nl1_w to x axis (1,0,0)
V3 axis2( 0, nl1_c(2), -nl1_c(1) ); //rotation axis between nl1_m to x axis (1,0,0) i.e. cross( nl1_w, x );
axis2.normalized();
Quaternion<POSE_T> q_gp_f_c(AngleAxis<POSE_T>(beta, axis2));
//cout << q_gp_f_c << endl;
ROTATION R_gp_f_c(q_gp_f_c);
//cout << R_gp_f_c << endl;
//{
// Vector3 nl_x = R_gp_f_c * nl1_c;
// print<T, Vector3>(nl_x);
//}
V3 pt2_g = R_g_f_w * (pt2_w.template cast<POSE_T>() - c_w); pt2_g(0) = POINT_T(0); pt2_g.normalized();
V3 pt2_gp = R_gp_f_c * (pt2_c.template cast<POSE_T>() - c_c); pt2_gp(0) = POINT_T(0); pt2_gp.normalized();
POSE_T gamma = acos(pt2_g.dot(pt2_gp)); //rotate pt2_g to pt2_gp;
V3 axis3(1,0,0);
Quaternion< POSE_T > q_gp_f_g(AngleAxis<POSE_T>(gamma, axis3));
//cout << q_gp_f_g << endl;
ROTATION R_gp_f_g ( q_gp_f_g );
//cout << R_gp_f_g << endl;
ROTATION R_c_f_gp = R_gp_f_c.inverse();
p_solution->so3() = R_c_f_gp * R_gp_f_g * R_g_f_w;
//{
// T3 pt = *R_cfw * (pt2_w - c_w) + c_c;
// cout << norm<T, T3>( pt - pt2_c ) << endl;
//}
//{
// cout << norm<T, T3>(nl1_w) << endl;
// cout << norm<T, T3>(nl1_c) << endl;
// cout << norm<T, T3>(*R_cfw * nl1_w) << endl;
// cout << norm<T, T3>(nl1_c - *R_cfw * nl1_w) << endl;
//}
p_solution->translation() = c_c - p_solution->so3() * c_w;
return;
}
template< typename POSE_T, typename POINT_T > /*Matrix<float,-1,-1,0,-1,-1> = MatrixXf*/
SE3Group<POSE_T> nl_shinji_ls(const Matrix<POINT_T, Dynamic, Dynamic> & Xw_, const Matrix<POINT_T, Dynamic, Dynamic>& Xc_,
const Matrix<POINT_T, Dynamic, Dynamic> & Nw_, const Matrix<POINT_T, Dynamic, Dynamic>& Nc_, const int K) {
typedef SE3Group<POSE_T> RT;
assert(Xw_.rows() == 3);
//Compute the centroid of each point set
Matrix<POINT_T, 3, 1> Cw(0, 0, 0), Cc(0, 0, 0); //Matrix<float,3,1,0,3,1> = Vector3f
for (int nCount = 0; nCount < K; nCount++){
Cw += Xw_.col(nCount);
Cc += Xc_.col(nCount);
}
Cw /= (POINT_T)K;
Cc /= (POINT_T)K;
// transform coordinate
Matrix<POSE_T, 3, 1> Aw, Ac;
Matrix<POSE_T, 3, 3> M; M.setZero();
Matrix<POSE_T, 3, 3> N;
POSE_T sigma_w_sqr = 0;
for (int nC = 0; nC < K; nC++){
Aw = Xw_.col(nC) - Cw; sigma_w_sqr += Aw.squaredNorm();
Ac = Xc_.col(nC) - Cc;
N = Ac * Aw.transpose();
M += N;
}
M /= (POSE_T)K;
sigma_w_sqr /= (POSE_T)K;
Matrix<POSE_T, 3, 3> M_n; M_n.setZero();
for (int nC = 0; nC < K; nC++){
//pure imaginary Shortcuts
Aw = Nw_.col(nC);
Ac = Nc_.col(nC);
N = Ac * Aw.transpose();
M_n += (sigma_w_sqr*N);
}
M_n /= (POSE_T)K;
M += M_n;
JacobiSVD<Matrix<POSE_T, 3,3> > svd(M, ComputeFullU | ComputeFullV);
//[U S V]=svd(M);
//R=U*V';
Matrix<POSE_T, 3, 3> U = svd.matrixU();
Matrix<POSE_T, 3, 3> V = svd.matrixV();
Matrix<POSE_T, 3, 1> D = svd.singularValues();
SO3Group<POSE_T> R_tmp;
if (M.determinant() < 0){
Matrix<POSE_T, 3, 3> I = Matrix<POSE_T, 3, 3>::Identity(); I(2, 2) = -1; D(2) *= -1;
R_tmp = SO3Group<POSE_T>(U*I*V.transpose());
}
else{
R_tmp = SO3Group<POSE_T>(U*V.transpose());
}
//T=ccent'-R*wcent';
Matrix< POSE_T, 3, 1> T_tmp = Cc - R_tmp * Cw;
RT solution = RT( R_tmp, T_tmp) ;
//T tr = D.sum();
//T dE_sqr = sigma_c_sqr - tr*tr / sigma_w_sqr;
//PRINT(dE_sqr);
return solution; // dE_sqr;
}
