Vector3d transformPoint(Vector3d& point,const MatrixXd& transform)
{
Vector4d tmp;
tmp.head(3)=point;
tmp(3)=1;
tmp.head(3)=transform*tmp;
return tmp.head(3);
}
Vector6d gc_asd_to_av(Vector4d asd) {
Vector6d av;
Vector3d aa = asd.head(3);
// double d_inv = asd(3);
// double sig_d_inv = (1.0 - exp(-asd(3))) / (2.0 * (1.0 + exp(-asd(3))));
// double sig_d_inv = -log(1.0/asd(3) - 1.0);
// double sig_d_inv = log( (2.0 * asd(3) + 1.0) / (1.0 - 2.0*asd(3)) );
// double sig_d_inv = atan(asd(3)) / 2.0;
// double sig_d_inv = atan2(asd(3), 1.0) / 2.0;
// double sig_d_inv = atan2(asd(3), 1.0);
// double sig_d_inv = atan2(asd(3), 1.0) * 1.0;
// double sig_d_inv = tan(4.0 * asd(3));
double sig_d_inv = log(asd(3));
// cout << "sig_d_inv = " << sig_d_inv << endl;
// double sig_d_inv = cos(asd(3)) / sin(asd(3));
// double sig_d_inv = sin(asd(3)) / cos(asd(3));
// double sig_d_inv = sin(asd(3)) / cos(asd(3));
Matrix3d R = gc_Rodriguez(aa);
// av.head(3) = R.col(2) / sig_d_inv;
av.head(3) = R.col(2) * sig_d_inv;
av.tail(3) = R.col(0);
return av;
}
Vector6d gc_aid_to_av(Vector4d aid) {
Vector6d av;
Vector3d aa = aid.head(3);
double d = 1.0 / aid(3);
Matrix3d R = gc_Rodriguez(aa);
av.head(3) = R.col(2) * d;
av.tail(3) = R.col(0);
// Vector6d av;
// double a = aid[0];
// double b = aid[1];
// double g = aid[2];
// double t = aid[3];
//
// double s1 = sin(a);
// double c1 = cos(a);
// double s2 = sin(b);
// double c2 = cos(b);
// double s3 = sin(g);
// double c3 = cos(g);
//
// Matrix3d R;
// R <<
// c2 * c3, s1 * s2 * c3 - c1 * s3, c1 * s2 * c3 + s1 * s3,
// c2 * s3, s1 * s2 * s3 + c1 * c3, c1 * s2 * s3 - s1 * c3,
// -s2, s1 * c2, c1 * c2;
//
// double d = 1.0 / t;
// av.head(3) = -R.col(2) * d;
// av.tail(3) = R.col(1);
return av;
}
void
addnode(SysSPA &spa, int n,
// node translation
std::vector< Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > ntrans,
// node rotation
std::vector< Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > nqrot,
// constraint indices
std::vector< Eigen::Vector2i, Eigen::aligned_allocator<Eigen::Vector2i> > cind,
// constraint local translation
std::vector< Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > ctrans,
// constraint local rotation as quaternion
std::vector< Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > cqrot,
// constraint covariance
std::vector< Eigen::Matrix<double,6,6>, Eigen::aligned_allocator<Eigen::Matrix<double,6,6> > > cvar)
{
Node nd;
// rotation
Quaternion<double> frq;
frq.coeffs() = nqrot[n];
frq.normalize();
if (frq.w() <= 0.0) frq.coeffs() = -frq.coeffs();
nd.qrot = frq.coeffs();
// translation
Vector4d v;
v.head(3) = ntrans[n];
v(3) = 1.0;
nd.trans = v;
nd.setTransform(); // set up world2node transform
nd.setDr(true);
// add to system
spa.nodes.push_back(nd);
// add in constraints
for (int i=0; i<(int)ctrans.size(); ++i)
{
ConP2 con;
con.ndr = cind[i].x();
con.nd1 = cind[i].y();
if ((con.ndr == n && con.nd1 <= n-1) ||
(con.nd1 == n && con.ndr <= n-1))
{
con.tmean = ctrans[i];
Quaternion<double> qr;
qr.coeffs() = cqrot[i];
qr.normalize();
con.qpmean = qr.inverse(); // inverse of the rotation measurement
con.prec = cvar[i]; // ??? should this be inverted ???
// need a boost for noise-offset system
//con.prec.block<3,3>(3,3) *= 10.0;
spa.p2cons.push_back(con);
}
}
}
// Project a 3D point into this Pose.
Vector2d Pose::ProjectTo2D(const Vector3d& pt3d) {
Vector4d pt3d_h = Vector4d::Constant(1.0);
pt3d_h.head(3) = pt3d;
const Vector4d proj_h = Rt_ * pt3d_h;
Vector2d proj = proj_h.head(2);
proj /= proj_h(2);
return proj;
}
Vector3d rigidBodyDynamics::diffQuaternion(Vector4d& q, Vector4d& qprev, double dt) const {
/*
qm = qinv.*qcurr;
M = [ qm(4) qm(3) -qm(2) -qm(1);
-qm(3) qm(4) qm(1) -qm(2);
qm(2) -qm(1) qm(4) -qm(3);
qm(1) qm(2) qm(3) qm(4)];
w = 2*M*dq'
*/
Vector4d dq = (q - qprev) / dt;
Matrix4d M;
M << qprev(3) , qprev(2), -qprev(1), -qprev(0),
-qprev(2), qprev(3), qprev(0), -qprev(1),
qprev(1), -qprev(0), qprev(3), -qprev(2),
qprev(0), qprev(1), qprev(2), qprev(3);
Vector4d wp = 2*M*dq;
Vector3d w = wp.head(3);
return w;
}
int main(int argc, char **argv)
{
char *fin;
if (argc < 2)
{
cout << "Arguments are: <input filename> [<number of nodes to use>]" << endl;
return -1;
}
// number of nodes to use
int nnodes = -1;
if (argc > 2)
nnodes = atoi(argv[2]);
int doiters = 10;
if (argc > 3)
doiters = atoi(argv[3]);
fin = argv[1];
// node translation
std::vector< Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > ntrans;
// node rotation
std::vector< Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > nqrot;
// constraint indices
std::vector< Eigen::Vector2i, Eigen::aligned_allocator<Eigen::Vector2i> > cind;
// constraint local translation
std::vector< Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > ctrans;
// constraint local rotation as quaternion
std::vector< Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > cqrot;
// constraint covariance
std::vector< Eigen::Matrix<double,6,6>, Eigen::aligned_allocator<Eigen::Matrix<double,6,6> > > cvar;
// tracks
std::vector<struct tinfo> tracks;
ReadSPAFile(fin,ntrans,nqrot,cind,ctrans,cqrot,cvar,tracks);
cout << "# [ReadSPAFile] Found " << (int)ntrans.size() << " nodes and "
<< (int)cind.size() << " constraints" << endl;
if (nnodes < 0)
nnodes = ntrans.size();
if (nnodes > (int)ntrans.size()) nnodes = ntrans.size();
// system
SysSPA spa;
spa.verbose=false;
// spa.useCholmod(true);
spa.csp.useCholmod = true;
// add first node
Node nd;
// rotation
Quaternion<double> frq;
frq.coeffs() = nqrot[0];
frq.normalize();
if (frq.w() <= 0.0) frq.coeffs() = -frq.coeffs();
nd.qrot = frq.coeffs();
// translation
Vector4d v;
v.head(3) = ntrans[0];
v(3) = 1.0;
nd.trans = v;
nd.setTransform(); // set up world2node transform
nd.setDr(true);
// add to system
spa.nodes.push_back(nd);
double cumtime = 0.0;
//cout << nd.trans.transpose() << endl << endl;
// add in nodes
for (int i=0; i<nnodes-1; i+=doiters)
{
for (int j=0; j<doiters; ++j)
if (i+j+1 < nnodes)
addnode(spa, i+j+1, ntrans, nqrot, cind, ctrans, cqrot, cvar);
long long t0, t1;
spa.nFixed = 1; // one fixed frame
t0 = utime();
// spa.doSPA(1,1.0e-4,SBA_SPARSE_CHOLESKY);
spa.doSPA(1,1.0e-4,SBA_BLOCK_JACOBIAN_PCG,1.0e-8,15);
t1 = utime();
cumtime += t1 - t0;
cerr
<< "nodes= " << spa.nodes.size()
<< "\tedges= " << spa.p2cons.size()
<< "\t chi2= " << spa.calcCost()
<< "\t time= " << (t1 - t0) * 1e-6
<< "\t cumTime= " << cumtime*1e-6
<< endl;
}
printf("[TestSPA] Compute took %0.2f ms/node, total squared cost %0.2f ms\n", 0.001*(double)cumtime/(double)nnodes, cumtime*0.001);
//ofstream ofs("sphere-ground.txt");
//for (int i=0; i<(int)ntrans.size(); ++i)
//ofs << ntrans[i].transpose() << endl;
//ofs.close();
//ofstream ofs2("sphere-opt.txt");
//for (int i=0; i<(int)spa.nodes.size(); ++i)
//ofs2 << spa.nodes[i].trans.transpose().head(3) << endl;
//ofs2.close();
//spa.writeSparseA("sphere-sparse.txt",true);
return 0;
}
int
PatchFit::fit()
{
// first check whether cloud subsampling is required
RandomSample<PointXYZ> *rs;
rs = new RandomSample<PointXYZ>();
if (this->ssmax_)
{
rs->setInputCloud(cloud_);
rs->setSample(ssmax_);
rs->setSeed(rand());
rs->filter(*cloud_);
}
delete (rs);
//remove NAN points from the cloud
std::vector<int> indices;
removeNaNFromPointCloud(*cloud_, *cloud_, indices); //is_dense should be false
// Step 1: fit plane by lls
Vector4d centroid;
compute3DCentroid(*cloud_, centroid);
p_->setC(centroid.head(3));
cloud_vec.resize(cloud_->points.size(),3);
fit_cloud_vec.resize(cloud_->points.size(),3);
int vec_counter = 0;
for (size_t i = 0; i<cloud_->points.size (); i++)
{
cloud_vec.row(vec_counter) = cloud_->points[i].getVector3fMap ().cast<double>();
// for fitting sub the centroid
fit_cloud_vec.row(vec_counter) = cloud_vec.row(vec_counter) - centroid.head(3).transpose();
vec_counter++;
}
cloud_vec.conservativeResize(vec_counter-1, 3); //normal resize does not keep old values
VectorXd b(fit_cloud_vec.rows());
b.fill(0.0);
Vector3d zl = lls(fit_cloud_vec,b);
p_->setR(zh.cross(zl));
double ss = p_->getR().norm();
double cc = zh.adjoint()*zl;
double t = atan2(ss,cc);
double th = sqrt(sqrt(EPS))*10.0;
double aa;
if (t>th)
aa = t/ss;
else
aa = 6.0/(6.0-t*t);
p_->setR(aa*p_->getR());
// Save the zl and the centroid p.c, for patch center constraint's use
plane_n = zl;
plane_c = p_->getC();
// Step 2: continue surface fitting if not plane
VectorXd a;
if (st_a) // general paraboloid
{
if (ccon_)
{
a.resize(6);
a << 0.0, 0.0, p_->getR(), 0.0;
}
else
{
a.resize(8);
a << 0.0, 0.0, p_->getR(), p_->getC();
}
a = wlm(cloud_vec, a);
// update the patch
p_->setR(a.segment(2,3));
if (ccon_)
p_->setC(plane_c + a(5)*plane_n);
else
p_->setC(a.segment(5,3));
Vector2d k;
k << a(0), a(1);
// refine into a specific paraboloid type
if (k.cwiseAbs().maxCoeff() < this->ktol_)
{
// fitting planar paraboloid
p_->setName("planar paraboloid");
p_->setS(Patch::s_p);
Matrix<double,1,1> k_p;
k_p << 0.0;
p_->setK(k_p);
p_->setR(p_->rcanon2(p_->getR(),2,3)); //TBD not updating correctly
}
else if (k.cwiseAbs().minCoeff() < this->ktol_)
{
// fitting cylindric paraboloid
p_->setName("cylindric paraboloid");
p_->setS(Patch::s_y);
p_->setB(Patch::b_r);
kx = k(0);
ky = k(1);
if (fabs(kx) > fabs(ky))
{
// swap curvatures
kx = k(1);
ky = k(0);
Matrix3d ww;
ww << yh, -xh, zh;
Matrix3d rr;
rr = p_->rexp(p_->getR());
rr = rr*ww;
p_->setR(p_->rlog(rr));
}
Eigen::Matrix<double,1,1> k_c_p;
k_c_p << ky;
p_->setK(k_c_p);
}
else if (fabs(k(0)-k(1)) < this->ktol_)
{
// fitting circular paraboloid
p_->setName("circular paraboloid");
p_->setS(Patch::s_o);
p_->setB(Patch::b_c);
Matrix<double,1,1> k_c_p;
k_c_p << k.mean();
p_->setK(k_c_p);
p_->setR(p_->rcanon2(p_->getR(),2,3));
}
else
{
if (sign(k(0),k(1)))
{
// fitting elliptic paraboloid
p_->setName("elliptic paraboloid");
p_->setS(Patch::s_e);
}
else
{
// fitting hyperbolic paraboloid
p_->setName("hyperbolic paraboloid");
p_->setS(Patch::s_h);
}
p_->setB(Patch::b_e);
p_->setK(k);
kx = k(0);
ky = k(1);
if (fabs(kx) > fabs(ky))
{
// swap curvatures
Vector2d k_swap;
k_swap << ky, kx;
p_->setK(k_swap);
Matrix3d ww;
ww << yh, -xh, zh;
Matrix3d rr;
rr = p_->rexp(p_->getR());
rr = rr*ww;
p_->setR(p_->rlog(rr));
}
}
}
else
{
// plane fitting
p_->setS(Patch::s_p);
p_->setB(this->bt_);
Matrix<double,1,1> k_p;
k_p << 0.0;
p_->setK(k_p);
}
// Step 5: fit boundary; project to local frame XY plane
Xform3 proj;
Matrix3d rrinv;
rrinv = p_->rexp(-p_->getR());
MatrixXd ux(cloud_vec.rows(),1), uy(cloud_vec.rows(),1), uz(cloud_vec.rows(),1);
ux = cloud_vec.col(0);
uy = cloud_vec.col(1);
uz = cloud_vec.col(2);
proj.transform(ux,uy,uz,p_->rexp(-p_->getR()), (-rrinv*p_->getC()),0,0);
// (normalized) moments
double mx = ux.mean();
double my = uy.mean();
double vx = (ux.array().square()).mean();
double vy = (uy.array().square()).mean();
double vxy = (ux.array() * uy.array()).mean();
// Step 5
lambda = sqrt(2.0) * boost::math::erf_inv(this->bcp_);
//STEP 6: cylindric parab: aa rect bound. This step also sets p.c as the 1D
// data centroid along the local frame x axis, which is the symmetry axis of
// the cylinder.
if (p_->getS() == Patch::s_y)
{
// fitting boundary: cyl parab, aa rect
VectorXd d(2);
d << lambda * sqrt(vx-mx*mx) , lambda * (sqrt(vy));
p_->setD(d);
p_->setC(p_->rexp(p_->getR())*mx*xh + p_->getC());
}
// STEP 7: circ parab: circ bound
if (p_->getS() == Patch::s_o)
{
// fitting boundary: cir parab, circ
VectorXd d(1);
d << lambda * max(sqrt(vx),sqrt(vy));
p_->setD(d);
}
// STEP 8: ell or hyp parab: ell bound
if (p_->getS() == Patch::s_e || p_->getS() == Patch::s_h)
{
// fitting boundary: ell/hyb parab, ell
VectorXd d(2);
d << lambda * sqrt(vx), lambda * sqrt(vy);
p_->setD(d);
}
// STEP 9: plane bounds
if (p_->getS() == Patch::s_p)
{
// fitting boundary: plane
Matrix3d rr = p_->rexp(p_->getR());
p_->setC(rr*(mx*xh+my*yh) + p_->getC());
double a = vx-mx*mx;
double b = 2.0*(vxy-mx*my);
double c = vy-my*my;
lambda = -log(1.0-this->bcp_);
double d = sqrt(b*b+(a-c)*(a-c));
double wp = a+c+d;
double wn = a+c-d;
Vector2d l;
l << sqrt(lambda*wp), sqrt(lambda*wn);
VectorXd d_p(1);
switch (p_->getB())
{
case Patch::b_c:
d_p << l.maxCoeff();
p_->setD(d_p);
break;
case Patch::b_e:
p_->setD(l);
break;
case Patch::b_r:
p_->setD(l);
break;
default:
cerr << "Invalid pach boundary for plane" << endl;
}
if (p_->getB() != Patch::b_c)
{
double t = 0.5 * atan2(b,a-c);
Matrix3d rr = p_->rexp(p_->getR());
Vector3d xl = rr.col(0);
Vector3d yl = rr.col(1);
Vector3d zl = rr.col(2);
xl = (cos(t)*xl) + (sin(t)*yl);
yl = zl.cross(xl);
Matrix3d xyzl;
xyzl << xl, yl, zl;
p_->setR(p_->rlog(xyzl));
}
} //plane boundary
return (1);
}
// Set up spanning tree initialization
void SysSPA::spanningTree(int node)
{
int nnodes = nodes.size();
// set up an index from nodes to their constraints
vector<vector<int> > cind;
cind.resize(nnodes);
for(size_t pi=0; pi<p2cons.size(); pi++)
{
ConP2 &con = p2cons[pi];
int i0 = con.ndr;
int i1 = con.nd1;
cind[i0].push_back(i1);
cind[i1].push_back(i0);
}
// set up breadth-first algorithm
VectorXd dist(nnodes);
dist.setConstant(1e100);
if (node >= nnodes)
node = 0;
dist[node] = 0.0;
multimap<double,int> open; // open list, priority queue - can have duplicates
open.insert(std::make_pair<double,int>(0.0,int(node)));
// do breadth-first computation
while (!open.empty())
{
// get top node, remove it
int ni = open.begin()->second;
double di = open.begin()->first;
open.erase(open.begin());
if (di > dist[ni]) continue; // already dealt with
// update neighbors
Node &nd = nodes[ni];
Matrix<double,3,4> n2w;
transformF2W(n2w,nd.trans,nd.qrot); // from node to world coords
vector<int> &nns = cind[ni];
for (int i=0; i<(int)nns.size(); i++)
{
ConP2 &con = p2cons[nns[i]];
double dd = con.tmean.norm(); // incremental distance
// neighbor node index
int nn = con.nd1;
if (nn == ni)
nn = con.ndr;
Node &nd2 = nodes[nn];
Vector3d tmean = con.tmean;
Quaterniond qpmean = con.qpmean;
if (nn == con.ndr) // wrong way, reverse
{
qpmean = qpmean.inverse();
tmean = nd.qrot.toRotationMatrix().transpose()*nd2.qrot.toRotationMatrix()*tmean;
}
if (dist[nn] > di + dd) // is neighbor now closer?
{
// set priority queue
dist[nn] = di+dd;
open.insert(std::make_pair<double,int>(di+dd,int(nn)));
// update initial pose
Vector4d trans;
trans.head(3) = tmean;
trans(3) = 1.0;
nd2.trans.head(3) = n2w*trans;
nd2.qrot = qpmean*nd.qrot;
nd2.normRot();
nd2.setTransform();
nd2.setDr(true);
}
}
}
}
// Current code only works for the left leg with only one constraint
VectorXd MyWorld::updateGradients() {
// compute c(q)
mC = mSkel->getMarker(mConstrainedMarker)->getWorldPosition() - mTarget;
// compute J(q)
Vector4d offset;
offset << mSkel->getMarker(mConstrainedMarker)->getLocalPosition(), 1; // Create a vector in homogeneous coordinates
// w.r.t ankle dofs
BodyNode *node = mSkel->getMarker(mConstrainedMarker)->getBodyNode();
Joint *joint = node->getParentJoint();
Matrix4d worldToParent = node->getParentBodyNode()->getTransform().matrix();
Matrix4d parentToJoint = joint->getTransformFromParentBodyNode().matrix();
Matrix4d dR = joint->getTransformDerivative(0); // Doesn't need .matrix() because it returns a Matrix4d instead of Isometry3d
Matrix4d R = joint->getTransform(1).matrix();
Matrix4d jointToChild = joint->getTransformFromChildBodyNode().inverse().matrix();
Vector4d jCol = worldToParent * parentToJoint * dR * R * jointToChild * offset;
int colIndex = joint->getIndexInSkeleton(0);
mJ.col(colIndex) = jCol.head(3); // Take the first 3 elelemtns of jCol
dR = joint->getTransformDerivative(1);
R = joint->getTransform(0).matrix();
jCol = worldToParent * parentToJoint * R * dR * jointToChild * offset;
colIndex = joint->getIndexInSkeleton(1);
mJ.col(colIndex) = jCol.head(3);
offset = parentToJoint * joint->getTransform(0).matrix() * joint->getTransform(1).matrix() * jointToChild * offset; // Update offset so it stores the chain below the parent joint
// w.r.t knee dof
node = node->getParentBodyNode(); // return NULL if node is the root node
joint = node->getParentJoint();
worldToParent = node->getParentBodyNode()->getTransform().matrix();
parentToJoint = joint->getTransformFromParentBodyNode().matrix();
dR = joint->getTransformDerivative(0); // Doesn't need .matrix() because it returns a Matrix4d instead of Isometry3d
jointToChild = joint->getTransformFromChildBodyNode().inverse().matrix();
jCol = worldToParent * parentToJoint * dR * jointToChild * offset;
colIndex = joint->getIndexInSkeleton(0);
mJ.col(colIndex) = jCol.head(3); // Take the first 3 elelemtns of jCol
offset = parentToJoint * joint->getTransform(0).matrix() * jointToChild * offset;
// w.r.t hip dofs
node = node->getParentBodyNode();
joint = node->getParentJoint();
worldToParent = node->getParentBodyNode()->getTransform().matrix();
parentToJoint = joint->getTransformFromParentBodyNode().matrix();
dR = joint->getTransformDerivative(0); // Doesn't need .matrix() because it returns a Matrix4d instead of Isometry3d
Matrix4d R1 = joint->getTransform(1).matrix();
Matrix4d R2 = joint->getTransform(2).matrix();
jointToChild = joint->getTransformFromChildBodyNode().inverse().matrix();
jCol = worldToParent * parentToJoint * dR * R1 * R2 * jointToChild * offset;
colIndex = joint->getIndexInSkeleton(0);
mJ.col(colIndex) = jCol.head(3); // Take the first 3 elelemtns of J
R1 = joint->getTransform(0).matrix();
dR = joint->getTransformDerivative(1);
R2 = joint->getTransform(2).matrix();
jCol = worldToParent * parentToJoint * R1 * dR * R2 * jointToChild * offset;
colIndex = joint->getIndexInSkeleton(1);
mJ.col(colIndex) = jCol.head(3);
R1 = joint->getTransform(0).matrix();
R2 = joint->getTransform(1).matrix();
dR = joint->getTransformDerivative(2);
jCol = worldToParent * parentToJoint * R1 * R2 * dR * jointToChild * offset;
colIndex = joint->getIndexInSkeleton(2);
mJ.col(colIndex) = jCol.head(3);
// compute gradients
VectorXd gradients = 2 * mJ.transpose() * mC;
return gradients;
}
// Current code only works for the left leg with only one constraint
VectorXd MyWorld::updateGradients() {
mJ.setZero();
mC.setZero();
// compute c(q)
//std::cout << "HAMYDEBUG: mConstrainedMarker = " << getMarker(mConstrainedMarker) << std::endl;
mC = getMarker(mConstrainedMarker)->getWorldPosition() - mTarget;
std::cout << "C(q) = " << mC << std::endl;
// compute J(q)
Vector4d offset;
offset << getMarker(mConstrainedMarker)->getLocalPosition(), 1; // Create a vector in homogeneous coordinates
//Setup vars
BodyNode *node = getMarker(mConstrainedMarker)->getBodyNode();
Joint *joint = node->getParentJoint();
Matrix4d worldToParent;
Matrix4d parentToJoint;
//Declare Vars
Matrix4d dR; // Doesn't need .matrix() because it returns a Matrix4d instead of Isometry3d
Matrix4d R;
Matrix4d R1;
Matrix4d R2;
Matrix4d jointToChild;
Vector4d jCol;
int colIndex;
//TODO: Might want to change this to check if root using given root fcn
//Iterate until we get to the root node
while(true) {
//std::cout << "HAMY DEBUG: Beginning new looop" << std::endl;
if(node->getParentBodyNode() == NULL) {
worldToParent = worldToParent.setIdentity();
} else {
worldToParent = node->getParentBodyNode()->getTransform().matrix();
}
parentToJoint = joint->getTransformFromParentBodyNode().matrix();
// Doesn't need .matrix() because it returns a Matrix4d instead of Isometry3d
jointToChild = joint->getTransformFromChildBodyNode().inverse().matrix();
//TODO: R1, R2, ... Rn code depending on DOFs
int nDofs = joint->getNumDofs();
//std::cout << "HAMY: nDofs=" << nDofs << std::endl;
//Can only have up to 3 DOFs on any one piece
switch(nDofs){
case 1: //std::cout << "HAMY: 1 nDOF" << std::endl;
dR = joint->getTransformDerivative(0);
jCol = worldToParent * parentToJoint * dR * jointToChild * offset;
colIndex = joint->getIndexInSkeleton(0);
mJ.col(colIndex) = jCol.head(3); // Take the first 3 elelemtns of jCol
offset = parentToJoint * joint->getTransform(0).matrix() * jointToChild * offset;
break;
case 2: //std::cout << "HAMY: 2 nDOF" << std::endl;
dR = joint->getTransformDerivative(0); // Doesn't need .matrix() because it returns a Matrix4d instead of Isometry3d
R = joint->getTransform(1).matrix();
jointToChild = joint->getTransformFromChildBodyNode().inverse().matrix();
jCol = worldToParent * parentToJoint * dR * R * jointToChild * offset;
colIndex = joint->getIndexInSkeleton(0);
mJ.col(colIndex) = jCol.head(3); // Take the first 3 elelemtns of jCol
dR = joint->getTransformDerivative(1);
R = joint->getTransform(0).matrix();
jCol = worldToParent * parentToJoint * R * dR * jointToChild * offset;
colIndex = joint->getIndexInSkeleton(1);
mJ.col(colIndex) = jCol.head(3);
offset = parentToJoint * joint->getTransform(0).matrix() * joint->getTransform(1).matrix() * jointToChild * offset; // Upd
break;
case 3: //std::cout << "HAMY: 3 nDOF" << std::endl;
dR = joint->getTransformDerivative(0); // Doesn't need .matrix() because it returns a Matrix4d instead of Isometry3d
R1 = joint->getTransform(1).matrix();
R2 = joint->getTransform(2).matrix();
jointToChild = joint->getTransformFromChildBodyNode().inverse().matrix();
jCol = worldToParent * parentToJoint * dR * R1 * R2 * jointToChild * offset;
colIndex = joint->getIndexInSkeleton(0);
mJ.col(colIndex) = jCol.head(3); // Take the first 3 elelemtns of J
R1 = joint->getTransform(0).matrix();
dR = joint->getTransformDerivative(1);
R2 = joint->getTransform(2).matrix();
jCol = worldToParent * parentToJoint * R1 * dR * R2 * jointToChild * offset;
colIndex = joint->getIndexInSkeleton(1);
mJ.col(colIndex) = jCol.head(3);
R1 = joint->getTransform(0).matrix();
R2 = joint->getTransform(1).matrix();
dR = joint->getTransformDerivative(2);
jCol = worldToParent * parentToJoint * R1 * R2 * dR * jointToChild * offset;
colIndex = joint->getIndexInSkeleton(2);
mJ.col(colIndex) = jCol.head(3);
offset = parentToJoint * joint->getTransform(0).matrix() * joint->getTransform(1).matrix() * joint->getTransform(2).matrix() * jointToChild * offset;
break;
default: //std::cout << "HAMY: Unexpected nDOF = " << nDofs << std::endl;
break;
}
if(node != mSkel->getRootBodyNode()) {
//std::cout << "HAMY DEBUG: Not root, continue loop" << std::endl;
node = node->getParentBodyNode(); // return NULL if node is the root node
joint = node->getParentJoint();
} else {
break;
}
}
// compute gradients
VectorXd gradients = 2 * mJ.transpose() * mC;
return gradients;
}
int main(int argc, char **argv)
{
// args
double lscale = 0.0;
double con_weight = 1.0;
printf("Args: <lscale 0.0> <scale weight 1.0>\n");
if (argc > 1)
lscale = atof(argv[1]);
if (argc > 2)
con_weight = atof(argv[2]);
// set up markers for visualization
ros::init(argc, argv, "VisBundler");
ros::NodeHandle n ("~");
ros::Publisher link_pub = n.advertise<visualization_msgs::Marker>("links", 0);
ros::Publisher cam_pub = n.advertise<visualization_msgs::Marker>("cameras", 0);
// set up system
SysSPA spa;
Node::initDr(); // set up fixed jacobians
initPrecs();
vector<Matrix<double,6,1>,Eigen::aligned_allocator<Matrix<double,6,1> > > cps;
double kfang = 5.0;
double kfrad = kfang*M_PI/180.0;
// create a spiral test trajectory
// connections are made between a frame and its three successors
spa.nFixed = 1; // three fixed frames
spa_spiral_setup(spa, true, cps, // use cross links
#if 1
n2prec, n2vprec, n2aprec, n2bprec, // rank-deficient
#else
diagprec, diagprec, diagprec, diagprec,
#endif
kfang, M_PI/2.0-3*kfrad, 220*kfang/360.0, // angle per node, init angle, total nodes
0.01, 1.0, 0.1, 0.1, 5.0); // node noise (m,deg), scale noise (increment),
cout << "[SPA Spiral] Initial cost is " << spa.calcCost() << endl;
cout << "[SPA Spiral] Number of constraints is " << spa.p2cons.size() << endl;
#if 0
// write out pose results and originals
cout << "[SPAsys] Writing pose file" << endl;
ofstream ofs3("P400.init.txt");
for (int i=0; i<(int)cps.size(); i++)
{
Vector3d tpn = spa.nodes[i].trans.head(3);
ofs3 << tpn.transpose() << endl;
}
ofs3.close();
#endif
// add in distance constraint
// works with either n0 -> ni or ni -> ni+1 constraints
#if 1
ConScale con;
con.w = con_weight; // weight
for (int i=0; i<(int)cps.size()-3; i++)
{
int k = i;
if (i > 200)
{
k = 0;
con.nd0 = i; // first node
con.nd1 = i+1; // second node
con.sv = k; // scale index
con.ks = (cps[con.nd1].head(3) - cps[con.nd0].head(3)).squaredNorm(); // measured distance
spa.scons.push_back(con);
con.nd0 = i+1; // first node
con.nd1 = i+2; // second node
con.sv = k; // scale index
con.ks = (cps[con.nd1].head(3) - cps[con.nd0].head(3)).squaredNorm(); // measured distance
spa.scons.push_back(con);
con.nd0 = i+2; // first node
con.nd1 = i+3; // second node
con.sv = k; // scale index
con.ks = (cps[con.nd1].head(3) - cps[con.nd0].head(3)).squaredNorm(); // measured distance
spa.scons.push_back(con);
con.nd0 = i; // first node
con.nd1 = i+3; // second node
con.sv = k; // scale index
con.ks = (cps[con.nd1].head(3) - cps[con.nd0].head(3)).squaredNorm(); // measured distance
spa.scons.push_back(con);
}
else
{
spa.scales.push_back(1.0);
Node nd0;
Node nd1;
con.nd0 = i; // first node
con.nd1 = i+1; // second node
con.sv = k; // scale index
nd0 = spa.nodes[con.nd0];
nd1 = spa.nodes[con.nd1];
con.ks = (nd1.trans.head(3) - nd0.trans.head(3)).squaredNorm(); // measured distance
spa.scons.push_back(con);
con.nd0 = i+1; // first node
con.nd1 = i+2; // second node
con.sv = k; // scale index
nd0 = spa.nodes[con.nd0];
nd1 = spa.nodes[con.nd1];
con.ks = (nd1.trans.head(3) - nd0.trans.head(3)).squaredNorm(); // measured distance
spa.scons.push_back(con);
con.nd0 = i+2; // first node
con.nd1 = i+3; // second node
con.sv = k; // scale index
nd0 = spa.nodes[con.nd0];
nd1 = spa.nodes[con.nd1];
con.ks = (nd1.trans.head(3) - nd0.trans.head(3)).squaredNorm(); // measured distance
spa.scons.push_back(con);
con.nd0 = i; // first node
con.nd1 = i+3; // second node
con.sv = k; // scale index
nd0 = spa.nodes[con.nd0];
nd1 = spa.nodes[con.nd1];
con.ks = (nd1.trans.head(3) - nd0.trans.head(3)).squaredNorm(); // measured distance
spa.scons.push_back(con);
}
#if 0 // doesn't seem to help...
if (i>2)
{
con.nd0 = i-3; // first node
con.nd1 = i+1; // second node
con.sv = 0; // scale index
con.ks = (cps[con.nd1].head(3) - cps[con.nd0].head(3)).squaredNorm(); // measured distance
spa.scons.push_back(con);
}
#endif
}
#endif
// add in cross-link distance constraint
// not much effect here...
#if 0
{
ConScale con;
con.w = 0.1; // weight
for (int i=72; i<(int)cps.size()-20; i++)
{
con.nd0 = i; // first node
con.nd1 = i-2; // second node
con.sv = i-72; // scale index
Node nd0 = spa.nodes[con.nd0];
Node nd1 = spa.nodes[con.nd1];
con.ks = (nd1.trans.head(3) - nd0.trans.head(3)).squaredNorm(); // measured distance
con.ks = (cps[con.nd1].head(3) - cps[con.nd0].head(3)).squaredNorm(); // measured distance
spa.scons.push_back(con);
con.nd0 = i-72;
con.nd1 = i;
con.sv = i-72;
nd0 = spa.nodes[con.nd0];
nd1 = spa.nodes[con.nd1];
con.ks = (nd1.trans.head(3) - nd0.trans.head(3)).squaredNorm(); // measured distance
con.ks = (cps[con.nd1].head(3) - cps[con.nd0].head(3)).squaredNorm(); // measured distance
spa.scons.push_back(con);
}
}
#endif
// this adds in a global constraint connecting the two sides
// and connecting first and last points
if (lscale > 0.0)
{
// cross-links
#if 0
{
ConP2 con;
con.ndr = 0;
con.nd1 = 36;
Node nd0 = spa.nodes[con.ndr];
Node nd1 = spa.nodes[con.nd1];
Vector4d trans;
trans.head(3) = cps[con.nd1].head(3);
trans(3) = 1.0;
con.prec = 1000*diagprec;
con.tmean = lscale * nd0.w2n * trans; // translation offset
Quaternion<double> q0,q1;
q0.coeffs() = nd0.qrot;
q1.coeffs() = nd1.qrot;
con.qpmean = (q0.inverse()*q1).inverse();
spa.p2cons.push_back(con);
}
{
ConP2 con;
con.ndr = 72;
con.nd1 = 36+72;
Node nd0 = spa.nodes[con.ndr];
Node nd1 = spa.nodes[con.nd1];
Vector4d trans;
trans.head(3) = cps[con.nd1].head(3);
trans(3) = 1.0;
con.prec = 1000*diagprec;
Quaternion<double> q0,q1;
q0.vec() = cps[con.ndr].segment(3,3);
q0.w() = sqrt(1.0 - q0.vec().squaredNorm());
q1.vec() = cps[con.nd1].segment(3,3);
q1.w() = sqrt(1.0 - q1.vec().squaredNorm());
Matrix<double,3,4> w2n;
Vector4d t0;
t0.head(3) = cps[con.ndr].head(3);
t0(3) = 1.0;
transformW2F(w2n,t0,q0);
con.tmean = lscale * w2n * trans; // translation offset
con.qpmean = (q0.inverse()*q1).inverse();
spa.p2cons.push_back(con);
}
#endif // global cross-loop constraint
// global
{
ConP2 con;
con.ndr = 0;
con.nd1 = 3*72;
Node nd0 = spa.nodes[con.ndr];
Node nd1 = spa.nodes[con.nd1];
Vector4d trans;
trans.head(3) = cps[con.nd1].head(3);
trans(3) = 1.0;
con.prec = 1000*diagprec;
con.tmean = lscale * nd0.w2n * trans; // translation offset
Quaternion<double> q0,q1;
q0 = nd0.qrot;
q1 = nd1.qrot;
con.qpmean = (q0.inverse()*q1).inverse();
spa.p2cons.push_back(con);
}
} // end of global constraints
{
// test results
double sqerr = 0.0;
double asqerr = 0.0;
for (int i=0; i<(int)cps.size(); i++)
{
Matrix<double,6,1> &cp = cps[i]; // old camera pose
Vector3d tp = cp.head(3);
Vector3d tpn = spa.nodes[i].trans.head(3);
// printf("\n[TestSPA] Cam %d orig: %0.2f %0.2f %0.2f\n", i, tp[0], tp[1], tp[2]);
// printf("[TestSPA] Cam %d new: %0.2f %0.2f %0.2f\n", i, tpn[0], tpn[1], tpn[2]);
Vector3d err = tp-tpn;
sqerr += err.squaredNorm();
Quaternion<double> qr;
qr.vec() = cp.block<3,1>(3,0);
qr.w() = sqrt(1.0 - qr.vec().squaredNorm());
Quaternion<double> qn;
qn = spa.nodes[i].qrot;
double da = qr.angularDistance(qn);
asqerr += da;
}
sqerr = sqerr / (double)(cps.size());
asqerr = asqerr / (double)(cps.size());
printf("\n[TestSPA] RMSE pos is %0.3f m, angle is %0.3f deg\n",
sqrt(sqerr), sqrt(asqerr));
}
// why do we have to draw twice????
cout << endl << "drawing..." << endl << endl;
drawgraph(spa,cam_pub,link_pub);
sleep(1);
drawgraph(spa,cam_pub,link_pub);
printf("Press <ret> to continue\n");
getchar();
// optimize
int iters = 20;
long long t0, t1;
double ttime = 0.0;
for (int i=0; i<iters; i++)
{
t0 = utime();
int niters = spa.doSPA(1,0.0);
t1 = utime();
ttime += (double)(t1-t0);
drawgraph(spa,cam_pub,link_pub);
sleep(1);
}
printf("[TestSPA] Compute took %0.2f ms/iter\n", 0.001*ttime/(double)iters);
#if 0
// write out A matrix
spa.setupSys(0.0);
cout << "[SPAsys] Writing file" << endl;
spa.writeSparseA("A400mono.txt");
#endif
#if 0
// write out pose results and originals
cout << "[SPAsys] Writing pose file" << endl;
ofstream ofs1("P400.ground.txt");
ofstream ofs2("P400.optim.txt");
for (int i=0; i<(int)cps.size(); i++)
{
Matrix<double,6,1> &cp = cps[i]; // old camera pose
Vector3d tp = cp.head(3);
Vector3d tpn = spa.nodes[i].trans.head(3);
// printf("\n[TestSPA] Cam %d orig: %0.2f %0.2f %0.2f\n", i, tp[0], tp[1], tp[2]);
// printf("[TestSPA] Cam %d new: %0.2f %0.2f %0.2f\n", i, tpn[0], tpn[1], tpn[2]);
ofs1 << tp.transpose() << endl;
ofs2 << tpn.transpose() << endl;
}
ofs1.close();
ofs2.close();
#endif
// test results
double sqerr = 0.0;
double asqerr = 0.0;
for (int i=0; i<(int)cps.size(); i++)
{
Matrix<double,6,1> &cp = cps[i]; // old camera pose
Vector3d tp = cp.head(3);
Vector3d tpn = spa.nodes[i].trans.head(3);
// printf("\n[TestSPA] Cam %d orig: %0.2f %0.2f %0.2f\n", i, tp[0], tp[1], tp[2]);
// printf("[TestSPA] Cam %d new: %0.2f %0.2f %0.2f\n", i, tpn[0], tpn[1], tpn[2]);
Vector3d err = tp-tpn;
sqerr += err.squaredNorm();
Quaternion<double> qr;
qr.vec() = cp.block<3,1>(3,0);
qr.w() = sqrt(1.0 - qr.vec().squaredNorm());
Quaternion<double> qn;
qn = spa.nodes[i].qrot;
double da = qr.angularDistance(qn);
asqerr += da;
}
sqerr = sqerr / (double)(cps.size());
asqerr = asqerr / (double)(cps.size());
printf("\n[TestSPA] RMSE pos is %0.3f m, angle is %0.3f deg\n",
sqrt(sqerr), sqrt(asqerr));
// draw graph
cout << endl << "drawing..." << endl << endl;
drawgraph(spa,cam_pub,link_pub);
return 0;
}
