void parseInertial(shared_ptr<RigidBody> body, XMLElement* node, RigidBodyTree * model)
{
Isometry3d T = Isometry3d::Identity();
XMLElement* origin = node->FirstChildElement("origin");
if (origin)
poseAttributesToTransform(origin, T.matrix());
XMLElement* mass = node->FirstChildElement("mass");
if (mass)
parseScalarAttribute(mass, "value", body->mass);
body->com << T(0, 3), T(1, 3), T(2, 3);
Matrix<double, TWIST_SIZE, TWIST_SIZE> I = Matrix<double, TWIST_SIZE, TWIST_SIZE>::Zero();
I.block(3, 3, 3, 3) << body->mass * Matrix3d::Identity();
XMLElement* inertia = node->FirstChildElement("inertia");
if (inertia) {
parseScalarAttribute(inertia, "ixx", I(0, 0));
parseScalarAttribute(inertia, "ixy", I(0, 1));
I(1, 0) = I(0, 1);
parseScalarAttribute(inertia, "ixz", I(0, 2));
I(2, 0) = I(0, 2);
parseScalarAttribute(inertia, "iyy", I(1, 1));
parseScalarAttribute(inertia, "iyz", I(1, 2));
I(2, 1) = I(1, 2);
parseScalarAttribute(inertia, "izz", I(2, 2));
}
auto bodyI = transformSpatialInertia(T, static_cast<Gradient<Isometry3d::MatrixType, Eigen::Dynamic>::type*>(NULL), I);
body->I = bodyI.value();
}
Transform3 eigen2Xform(const Isometry3d& B) {
Eigen::Matrix4d me = B.matrix();
mat4_t<real> mz;
for (int i=0; i<4; ++i) {
for (int j=0; j<4; ++j) {
mz(i,j) = me(i,j);
}
}
Transform3 rval;
rval.setFromMatrix(mz);
return rval;
}
Pointcloud::Ptr joint(camera &cam, Pointcloud::Ptr points, Isometry3d &T, frame &newframe)
{
Pointcloud::Ptr cloud = map2point(cam, newframe.rgb, newframe.depth);
cout << "combining clouds together" << endl;
Pointcloud::Ptr output(new Pointcloud());
//transform cloud points into the same coordinate system
transformPointCloud(*points, *output, T.matrix());
//put the points together
*output += *cloud;
return output;
}
void test()
{
Isometry3d T = Eigen::Isometry3d::Identity();
T(0,0)=1;
T(0,1)=0;
T(0,2)=0;
T(1,0)=0;
T(1,1)=0.707;
T(1,2)=-0.707;
T(2,0)=0;
T(2,1)=0.707;
T(2,2)=0.707;
T(0,3)=0;
T(1,3)=0;
T(2,3)=0.5;
cout<<T.matrix()<<endl;
for (int i = 2; i <=8; i++)
{
g2o::EdgeSE3* edge = new g2o::EdgeSE3();
g2o::VertexSE3 *v = new g2o::VertexSE3();
v->setId(i);
v->setEstimate( Eigen::Isometry3d::Identity() );
m_globalOptimizer.addVertex(v);
edge->vertices() [0] = m_globalOptimizer.vertex( i-1 );
edge->vertices() [1] = m_globalOptimizer.vertex( i );
Eigen::Matrix<double, 6, 6> information = Eigen::Matrix< double, 6,6 >::Identity();
information(0,0) = information(1,1) = information(2,2) = 100;
information(3,3) = information(4,4) = information(5,5) = 100;
edge->setInformation( information );
edge->setMeasurement( T );
m_globalOptimizer.addEdge(edge);
}
#if 0
T = Eigen::Isometry3d::Identity();
//T(2,3)=1;
//cout<<T.matrix()<<endl;
g2o::EdgeSE3* edge = new g2o::EdgeSE3();
g2o::VertexSE3 *v = new g2o::VertexSE3();
v->setId(8);
v->setEstimate( Eigen::Isometry3d::Identity() );
m_globalOptimizer.addVertex(v);
edge->vertices() [0] = m_globalOptimizer.vertex( 1 );
edge->vertices() [1] = m_globalOptimizer.vertex( 8 );
Eigen::Matrix<double, 6, 6> information = Eigen::Matrix< double, 6,6 >::Identity();
information(0,0) = information(1,1) = information(2,2) = 100;
information(3,3) = information(4,4) = information(5,5) = 100;
edge->setInformation( information );
edge->setMeasurement( T );
m_globalOptimizer.addEdge(edge);
#endif
m_globalOptimizer.save("pa.g2o");
m_globalOptimizer.initializeOptimization();
//m_globalOptimizer.optimize(50);
m_globalOptimizer.save("pa2.g2o");
cout<<"f**k"<<endl;
VertexSE3* vertex = dynamic_cast<VertexSE3*>(m_globalOptimizer.vertex(2));
Isometry3d pose = vertex -> estimate();
//cout << "matrix:" << endl;
cout << pose.matrix() << endl;
}
SE3Quat toSE3Quat(const Isometry3d& t)
{
SE3Quat result(t.matrix().topLeftCorner<3,3>(), t.translation());
return result;
}
bool AlignmentAlgorithmPlaneLinear::operator()(AlignmentAlgorithmPlaneLinear::TransformType& transform, const CorrespondenceVector& correspondences, const IndexVector& indices)
{
double err=0;
//If my correspondaces indices are less then a minimum threshold, stop it please
if ((int)indices.size()<minimalSetSize()) return false;
//My initial guess for the transformation it's the identity matrix
//maybe in the future i could have a less rough guess
transform = Isometry3d::Identity();
//Unroll the matrix to a vector
Vector12d x=homogeneous2vector(transform.matrix());
Matrix9x1d rx=x.block<9,1>(0,0);
//Initialization of variables, melting fat, i've so much space
Matrix9d H;
H.setZero();
Vector9d b;
b.setZero();
Matrix3x9d A;
//iteration for each correspondace
for (size_t i=0; i<indices.size(); i++)
{
A.setZero();
const Correspondence& c = correspondences[indices[i]];
const EdgePlane* edge = static_cast<const EdgePlane*>(c.edge());
//estraggo i vertici dagli edge
const VertexPlane* v1 = static_cast<const VertexPlane*>(edge->vertex(0));
const VertexPlane* v2 = static_cast<const VertexPlane*>(edge->vertex(1));
//recupero i dati dei piani
const AlignmentAlgorithmPlaneLinear::PointEstimateType& pi= v1->estimate();
const AlignmentAlgorithmPlaneLinear::PointEstimateType& pj= v2->estimate();
//recupeo le normali, mi servono per condizionare la parte rotazionale
Vector3d ni;
Vector3d nj;
Vector4d coeffs_i=pi.toVector();
Vector4d coeffs_j=pj.toVector();
ni=coeffs_i.head(3);
nj=coeffs_j.head(3);
//inizializzo lo jacobiano, solo la parte rotazionale (per ora)
A.block<1,3>(0,0)=nj.transpose();
A.block<1,3>(1,3)=nj.transpose();
A.block<1,3>(2,6)=nj.transpose();
if(DEBUG) {
cerr << "[DEBUG] PI"<<endl;
cerr << coeffs_i<<endl;
cerr << "[DEBUG] PJ "<<endl;
cerr << coeffs_j<<endl;
cerr << "[ROTATION JACOBIAN][PLANE "<<i<<"]"<<endl;
cerr << A<<endl;
}
//errore
//inizializzo errore
Vector3d ek;
ek.setZero();
ek=A*x.block<9,1>(0,0)-coeffs_i.head(3);
H+=A.transpose()*A;
b+=A.transpose()*ek;
err+=abs(ek.dot(ek));
if(DEBUG)
cerr << "[ROTATIONAL Hessian for plane "<<i<<"]"<<endl<<H<<endl;
if(DEBUG)
cerr << "[ROTATIONAL B for plane "<<i<<"]"<<endl<<b<<endl;
}
LDLT<Matrix9d> ldlt(H);
if (ldlt.isNegative()) return false;
rx=ldlt.solve(-b); // using a LDLT factorizationldlt;
x.block<3,1>(0,0)+=rx.block<3,1>(0,0);
x.block<3,1>(3,0)+=rx.block<3,1>(3,0);
x.block<3,1>(6,0)+=rx.block<3,1>(6,0);
if(DEBUG) {
cerr << "Solving the linear system"<<endl;
cerr << "------------>H"<<endl;
cerr << H<<endl;
cerr << "------------>b"<<endl;
cerr << b<<endl;
cerr << "------------>rx"<<endl;
cerr << rx<<endl;
cerr << "------------>xTEMP"<<endl;
cerr << vector2homogeneous(x)<<endl<<endl;
cerr << "łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł"<<endl;
cerr << "łłłłłłłłłłł Ringraziamo Cthulhu la parte rotazionale è finitałłłłłłłłłłł"<<endl;
cerr << "łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł"<<endl;
}
Matrix4d Xtemp=vector2homogeneous(x);
//now the problem si solved but i could have (and probably i have!)
//a not orthogonal rotation matrix, so i've to recondition it
Matrix3x3d R=Xtemp.block<3,3>(0,0);
// recondition the rotation
JacobiSVD<Matrix3x3d> svd(R, Eigen::ComputeThinU | Eigen::ComputeThinV);
if (svd.singularValues()(0)<.5) return false;
R=svd.matrixU()*svd.matrixV().transpose();
Isometry3d X = Isometry3d::Identity();
X.linear()=R;
X.translation()= x.block<3,1>(0,3);
if(DEBUG)
cerr << "X dopo il ricondizionamento appare come:"<<endl;
if(DEBUG)
cerr << X.matrix()<<endl;
Matrix3d H2=Matrix3d::Zero();
Vector3d b2=Vector3d::Zero();
Vector3d A2=Vector3d::Zero();
//at this point rotation is cured, now it's time to work on the translation
for (size_t i=0; i<indices.size(); i++)
{
if(DEBUG)
cerr << "[TRANSLATION JACOBIAN START][PLANE "<<i<<"]"<<endl;
const Correspondence& c = correspondences[indices[i]];
const EdgePlane* edge = static_cast<const EdgePlane*>(c.edge());
//estraggo i vertici dagli edge
const VertexPlane* v1 = static_cast<const VertexPlane*>(edge->vertex(0));
const VertexPlane* v2 = static_cast<const VertexPlane*>(edge->vertex(1));
//recupero i dati dei piani
const AlignmentAlgorithmPlaneLinear::PointEstimateType& pi= v1->estimate();
const AlignmentAlgorithmPlaneLinear::PointEstimateType& pj= v2->estimate();
//recupeo le normali, mi servono per condizionare la parte rotazionale
Vector3d ni;
Vector3d nj;
Vector4d coeffs_i=pi.toVector();
Vector4d coeffs_j=pj.toVector();
double di;
double dj;
ni=coeffs_i.head(3);
nj=coeffs_j.head(3);
di=coeffs_i(3);
dj=coeffs_j(3);
if(DEBUG)
cerr << "[TRANSLATION JACOBIAN][ JACOBIAN IS: ]"<<endl;
A2=(-nj.transpose()*R.transpose());
if(DEBUG)
cerr << A2<<endl;
double ek;
ek=dj+A2.transpose()*X.translation()-di;
H2+=A2*A2.transpose();
b2+= (A2.transpose()*ek);
err += abs(ek*ek);
if(DEBUG)
cerr << "[TRANSLATIONAL Hessian for plane "<<i<<"]"<<endl<<H2<<endl;
if(DEBUG)
cerr << "[TRANSLATIONAL B for plane "<<i<<"]"<<endl<<b2<<endl;
}
if(DEBUG)
cerr << "[FINAL][TRANSLATIONAL Hessian]"<<endl<<H2<<endl;
if(DEBUG)
cerr << "[FINAL][TRANSLATIONAL B]"<<endl<<b2<<endl;
//solving the system
Vector3d dt;
LDLT<Matrix3d> ldlt2(H2);
dt=ldlt2.solve(-b2); // using a LDLT factorizationldlt;
if(DEBUG)
cerr << "[FINAL][TRANSLATIONAL DT]"<<endl<<dt<<endl;
X.translation()+=dt;
transform = X;
if(DEBUG)
{
cerr << "TRANSFORM found: " << endl<< endl<< endl;
cerr << g2o::internal::toVectorMQT(X) << endl;;
cerr << transform.matrix()<< endl;;
}
return true;
}
