bool EdgeProjectXYZ2UV::read(std::istream& is)
{
for (int i=0; i<2; i++){
is >> measurement()[i];
}
inverseMeasurement()[0] = -measurement()[0];
inverseMeasurement()[1] = -measurement()[1];
for (int i=0; i<2; i++)
for (int j=i; j<2; j++) {
is >> information()(i,j);
if (i!=j)
information()(j,i)=information()(i,j);
}
return true;
}
bool EdgeSE2::read(std::istream& is)
{
Vector3d p;
is >> p[0] >> p[1] >> p[2];
measurement().fromVector(p);
inverseMeasurement() = measurement().inverse();
for (int i = 0; i < 3; ++i)
for (int j = i; j < 3; ++j) {
is >> information()(i, j);
if (i != j)
information()(j, i) = information()(i, j);
}
return true;
}
bool EdgeSE3Expmap::read(std::istream& is)
{
SE3Quat cam2world;
for (int i=0; i<7; i++)
is >> cam2world[i];
measurement() = cam2world.inverse();
inverseMeasurement() = cam2world;
//TODO: Convert information matrix!!
for (int i=0; i<6; i++)
for (int j=i; j<6; j++) {
is >> information()(i,j);
if (i!=j)
information()(j,i)=information()(i,j);
}
return true;
}
bool G2oEdgeSim3
::read(std::istream& is)
{
Vector7d v7;
for (int i=0; i<7; i++)
{
is >> v7[i];
}
Sim3 cam2world = Sim3::exp(v7);
measurement() = cam2world.inverse();
inverseMeasurement() = cam2world;
for (int i=0; i<7; i++)
for (int j=i; j<7; j++)
{
is >> information()(i,j);
if (i!=j)
information()(j,i) = information()(i,j);
}
return is;
}
