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; }