bool G2oEdgeSE3
::read(std::istream& is)
{
// assert(false);
std::cout << "read G2oEdgeSE3 called" << "\n";
Vector3d vec;
for(int i=0; i<3; i++) is >> vec[i];
double q_in [4];
for(int i=0; i<4; i++) is >> q_in[i];
Quaterniond q(q_in[0], q_in[1], q_in[2], q_in[3]);
Sophus::SE3 se3(q, vec);
setMeasurement(se3);
if (is.bad()) {
return false;
}
for ( int i=0; i<information().rows() && is.good(); i++)
for (int j=i; j<information().cols() && is.good(); j++){
is >> information()(i,j);
if (i!=j)
information()(j,i)=information()(i,j);
}
if (is.bad()) {
// we overwrite the information matrix with the Identity
information().setIdentity();
}
return true;
}
bool EdgeSE3Offset::read(std::istream& is) {
int pidFrom, pidTo;
is >> pidFrom >> pidTo ;
if (! setParameterId(0,pidFrom))
return false;
if (! setParameterId(1,pidTo))
return false;
Vector7d meas;
for (int i=0; i<7; i++)
is >> meas[i];
// normalize the quaternion to recover numerical precision lost by storing as human readable text
Vector4d::MapType(meas.data()+3).normalize();
setMeasurement(internal::fromVectorQT(meas));
if (is.bad()) {
return false;
}
for ( int i=0; i<information().rows() && is.good(); i++)
for (int j=i; j<information().cols() && is.good(); j++){
is >> information()(i,j);
if (i!=j)
information()(j,i)=information()(i,j);
}
if (is.bad()) {
// we overwrite the information matrix with the Identity
information().setIdentity();
}
return true;
}
bool EdgeSE3::setMeasurementFromState() {
VertexSE3 *from = static_cast<VertexSE3*>(_vertices[0]);
VertexSE3 *to = static_cast<VertexSE3*>(_vertices[1]);
Eigen::Isometry3d delta = from->estimate().inverse() * to->estimate();
setMeasurement(delta);
return true;
}
bool EdgeSE3PointXYZDisparity::read(std::istream& is) {
// measured keypoint
int pid;
is >> pid;
setParameterId(0,pid);
Vector3d meas;
for (int i=0; i<3; i++) is >> meas[i];
setMeasurement(meas);
if (is.bad())
return false;
for ( int i=0; i<information().rows() && is.good(); i++)
for (int j=i; j<information().cols() && is.good(); j++){
is >> information()(i,j);
if (i!=j)
information()(j,i)=information()(i,j);
}
if (is.bad()) {
// we overwrite the information matrix
information().setIdentity();
information()(2,2)=1000.;
}
//_cacheIds[0] = _paramId;
return true;
}
bool EdgeSE2DistanceOrientation::read(std::istream& is) {
Vector2d p;
is >> p[0] >> p[1];
setMeasurement(p);
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 EdgePointXYZ::read(std::istream& is)
{
Vector3D p;
is >> p[0] >> p[1] >> p[2];
setMeasurement(p);
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 EdgeLine3D::read(std::istream& is)
{
Vector6d v;
for (int i = 0; i < 6; ++i)
is >> v[i];
setMeasurement(v);
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 EdgeSE2::read(std::istream& is)
{
Vector3 p;
is >> p[0] >> p[1] >> p[2];
setMeasurement(SE2(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 EdgeSE2OdomDifferentialCalib::read(std::istream& is)
{
double vl, vr, dt;
is >> vl >> vr >> dt;
VelocityMeasurement vm(vl, vr, dt);
setMeasurement(vm);
for (int i = 0; i < information().rows(); ++i)
for (int j = i; j < information().cols(); ++j) {
is >> information()(i, j);
if (i != j)
information()(j, i) = information()(i, j);
}
return true;
}
void RDocumentVariables::setKnownVariable(RS::KnownVariable key, const QVariant& value) {
if (key==RS::INSUNITS) {
setUnit((RS::Unit)value.toInt());
return;
}
else if (key==RS::MEASUREMENT) {
setMeasurement((RS::Measurement)value.toInt());
return;
}
else if (key==RS::LTSCALE) {
setLinetypeScale(value.toDouble());
return;
}
knownVariables.insert(key, value);
}
bool EdgeSE3Expmap::read(std::istream& is) {
Vector7d meas;
for (int i=0; i<7; i++)
is >> meas[i];
SE3Quat cam2world;
cam2world.fromVector(meas);
setMeasurement(cam2world.inverse());
//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;
}
// Construct with measurement and variance.
SpeedAndBiasError::SpeedAndBiasError(const okvis::SpeedAndBiases& measurement,
double speedVariance,
double gyrBiasVariance,
double accBiasVariance) {
setMeasurement(measurement);
information_t information;
information.setZero();
information.topLeftCorner<3, 3>() = Eigen::Matrix3d::Identity() * 1.0
/ speedVariance;
information.block<3, 3>(3, 3) = Eigen::Matrix3d::Identity() * 1.0
/ gyrBiasVariance;
information.bottomRightCorner<3, 3>() = Eigen::Matrix3d::Identity() * 1.0
/ accBiasVariance;
setInformation(information);
}
bool EdgeSim3::read(std::istream& is)
{
Vector7d v7;
for (int i=0; i<7; i++){
is >> v7[i];
}
Sim3 cam2world(v7);
setMeasurement(cam2world.inverse());
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 true;
}
bool EdgePoseLandmarkReproject
::read(std::istream& is)
{
double u, v;
is >> u;
is >> v;
setMeasurement(Eigen::Vector2d(u,v));
if (is.bad()) {
return false;
}
for ( int i=0; i<information().rows() && is.good(); i++)
for (int j=0; j<information().cols() && is.good(); j++)
is >> information()(i,j);
if (is.bad()) {
// we overwrite the information matrix with the Identity
information().setIdentity();
}
return true;
}
bool G2oEdgeProjectPSI2UVU
::read(std::istream& is)
{
// assert(false);
Vector3d vec;
for(int i=0; i<3; i++) is >> vec[i];
setMeasurement(vec);
if (is.bad()) {
return false;
}
for ( int i=0; i<information().rows() && is.good(); i++)
for (int j=i; j<information().cols() && is.good(); j++){
is >> information()(i,j);
if (i!=j)
information()(j,i)=information()(i,j);
}
if (is.bad()) {
// we overwrite the information matrix with the Identity
information().setIdentity();
}
return true;
}
bool EdgeSE3PointXYZ::read(std::istream& is) {
int pId;
is >> pId;
setParameterId(0, pId);
// measured keypoint
Vector3d meas;
for (int i=0; i<3; i++) is >> meas[i];
setMeasurement(meas);
// information matrix is the identity for features, could be changed to allow arbitrary covariances
if (is.bad()) {
return false;
}
for ( int i=0; i<information().rows() && is.good(); i++)
for (int j=i; j<information().cols() && is.good(); j++){
is >> information()(i,j);
if (i!=j)
information()(j,i)=information()(i,j);
}
if (is.bad()) {
// we overwrite the information matrix
information().setIdentity();
}
return true;
}
bool EdgeSE3Prior::setMeasurementFromState(){
setMeasurement(_cache->n2w());
return true;
}
// Construct with measurement and information matrix
SpeedAndBiasError::SpeedAndBiasError(const okvis::SpeedAndBias & measurement,
const information_t & information) {
setMeasurement(measurement);
setInformation(information);
}
/*
* Read system locales using org.freedesktop.locale1 dbus interface
*/
void
EnabledLocalesModel::updateSystemLocales()
{
QDBusInterface dbusInterface( "org.freedesktop.locale1",
"/org/freedesktop/locale1",
"org.freedesktop.locale1",
QDBusConnection::systemBus() );
QStringList localeEnviromentVariables = dbusInterface.property( "Locale" ).toStringList();
QModelIndex defaultIndex = QModelIndex();
for ( QString lev : localeEnviromentVariables )
{
if ( lev.startsWith( "LANG=" ) )
{
int defaultLocaleRow = findKey( lev.section( '=', 1 ) );
defaultIndex = index( defaultLocaleRow,0 );
}
}
setAddress( defaultIndex );
setCollate( defaultIndex );
setCtype( defaultIndex );
setIdentification( defaultIndex );
setLang( defaultIndex );
setLanguage( defaultIndex );
setMeasurement( defaultIndex );
setMonetary( defaultIndex );
setMessages( defaultIndex );
setName( defaultIndex );
setNumeric( defaultIndex );
setPaper( defaultIndex );
setTelephone( defaultIndex );
setTime( defaultIndex );
for ( QString lev : localeEnviromentVariables )
{
QString value = lev.section( '=', 1 );
if ( lev.startsWith( "LC_ADDRESS=" ) )
{
if ( value.isEmpty() )
setAddress( defaultIndex );
else
{
int row = findKey( value );
setAddress( index( row, 0 ) );
}
}
else if ( lev.startsWith( "LC_COLLATE=" ) )
{
if ( value.isEmpty() )
setCollate( defaultIndex );
else
{
int row = findKey( value );
setCollate( index( row, 0 ) );
}
}
else if ( lev.startsWith( "LC_CTYPE=" ) )
{
if ( value.isEmpty() )
setCtype( defaultIndex );
else
{
int row = findKey( value );
setCtype( index( row, 0 ) );
}
}
else if ( lev.startsWith( "LC_IDENTIFICATION=" ) )
{
if ( value.isEmpty() )
setIdentification( defaultIndex );
else
{
int row = findKey( value );
setIdentification( index( row, 0 ) );
}
}
else if ( lev.startsWith( "LANG=" ) )
{
if ( value.isEmpty() )
setLang( defaultIndex );
else
{
int row = findKey( value );
setLang( index( row, 0 ) );
}
}
else if ( lev.startsWith( "LANGUAGE=" ) )
{
if ( value.isEmpty() )
setLanguage( defaultIndex );
else
{
int row = findKey( value );
setLanguage( index( row, 0 ) );
}
}
else if ( lev.startsWith( "LC_MEASUREMENT=" ) )
{
if ( value.isEmpty() )
setMeasurement( defaultIndex );
else
{
int row = findKey( value );
setMeasurement( index( row, 0 ) );
}
}
else if ( lev.startsWith( "LC_MESSAGES=" ) )
{
if ( value.isEmpty() )
setMessages( defaultIndex );
else
{
int row = findKey( value );
setMessages( index( row, 0 ) );
}
}
else if ( lev.startsWith( "LC_MONETARY=" ) )
{
if ( value.isEmpty() )
setMonetary( defaultIndex );
else
{
int row = findKey( value );
setMonetary( index( row, 0 ) );
}
}
else if ( lev.startsWith( "LC_NAME=" ) )
{
if ( value.isEmpty() )
setName( defaultIndex );
else
{
int row = findKey( value );
setName( index( row, 0 ) );
}
}
else if ( lev.startsWith( "LC_NUMERIC=" ) )
{
if ( value.isEmpty() )
setNumeric( defaultIndex );
else
{
int row = findKey( value );
setNumeric( index( row, 0 ) );
}
}
else if ( lev.startsWith( "LC_PAPER=" ) )
{
if ( value.isEmpty() )
setPaper( defaultIndex );
else
{
int row = findKey( value );
setPaper( index( row, 0 ) );
}
}
else if ( lev.startsWith( "LC_TELEPHONE=" ) )
{
if ( value.isEmpty() )
setTelephone( defaultIndex );
else
{
int row = findKey( value );
setTelephone( index( row, 0 ) );
}
}
else if ( lev.startsWith( "LC_TIME=" ) )
{
if ( value.isEmpty() )
setTime( defaultIndex );
else
{
int row = findKey( value );
setTime( index( row, 0 ) );
}
}
}
}
bool EdgeSE3Offset::setMeasurementFromState(){
Eigen::Isometry3d delta = _cacheFrom->w2n() * _cacheTo->n2w();
setMeasurement(delta);
return true;
}
