Moment& Moment::operator=(const Moment& source) { if (this != &source) // protect against invalid self-assignment { m_numStates = source.m_numStates; setMean(source.mean()); setCovariance(source.covariance()); } // by convention, always return *this return *this; }
void trig::getVoPose(geometry_msgs::PoseWithCovariance *ret, SpottedBeacon left, SpottedBeacon right, geometry_msgs::PoseWithCovariance prev){ ROS_INFO("Left: %f %f", left.distance, left.angle); ROS_INFO("Prev x %f y %f z %f", prev.pose.position.x, prev.pose.position.y, prev.pose.orientation.z); if (right.beacon != NULL) ROS_INFO("Right: %f %f", right.distance, right.angle); if (left.beacon == NULL){ //No beacons spotted ret->pose.position.x = prev.pose.position.x; ret->pose.position.y = prev.pose.position.y; ret->pose.position.z = prev.pose.position.z; ret->pose.orientation.x = prev.pose.orientation.x; ret->pose.orientation.y = prev.pose.orientation.y; ret->pose.orientation.z = prev.pose.orientation.z; ret->pose.orientation.w = prev.pose.orientation.w; ret->covariance = setCovariance(NO_BEACONS); //ROS_INFO("Trig Pose 0 x: %f, y:%f z:%f", ret->pose.position.x, ret->pose.position.y, ret->pose.orientation.z); return; } else if (right.beacon == NULL) { //One beacon spotted //Get position closest to the last known position on the circle around the beacon //Get angle from this assumed position //getSinglePoint(&ret->pose.position, left.beacon->position, left.distance, prev); ret->pose.position.x = prev.pose.position.x; ret->pose.position.y = prev.pose.position.y; ret->pose.position.z = prev.pose.position.z; getOrientation(&ret->pose.orientation, &ret->pose.position, *left.beacon, left.angle); ret->covariance = setCovariance(ONE_BEACON); //ROS_INFO("Trig Pose 1 x: %f, y:%f z:%f", ret->pose.position.x, ret->pose.position.y, ret->pose.orientation.z); return; } getPoint(&ret->pose.position, left.beacon->position, left.distance, right.beacon->position, right.distance); getOrientation(&ret->pose.orientation, &ret->pose.position, *left.beacon, left.angle); ret->covariance = setCovariance(LOW_COV); //ROS_INFO("Trig Pose 2 x: %f, y:%f z:%f", ret->pose.position.x, ret->pose.position.y, ret->pose.orientation.z); }
void TrackPoint::Init (double x, double y, double area, const double cov[], int t, int ID) { this->x = x; this->y = y; this->frameNum = t; this->idNum = ID; this->area = area; if (cov == NULL) { this->cov[0] = this->cov[1] = this->cov[2] = this->cov[3] = 0; } else { setCovariance(cov); } mh = NULL; }
void Covariance::onDeserialize(soam::InputStreamPtr &stream) throw (soam::SoamException) { reset(); try { SIZE_TYPE size; stream->read(size); for (SIZE_TYPE i = 0; i < size; ++i) { Investment::ID_TYPE i1, i2; double covariance; stream->read(i1); stream->read(i2); stream->read(covariance); setCovariance(i1, i2, covariance); } } catch (...) { reset(); throw; } }
Estimator2D<Mahalanobis2D>::Estimator2D(const VECTOR& X, const VECTOR& Y) : X_(&X), Y_(&Y) { setBandwidth(scottBandwidth(X.size(), 2.0, 1.0)); setCovariance(var(X), var(Y), covar(X, Y)); }
Estimator<Mahalanobis>::Estimator(const MATRIX& X) : X_(&X) { setBandwidth(scottBandwidth(X.rows(), X.columns(), 1.0)); setCovariance(X.transpose()*X); }
void CKroneckerCF::setColCovariance(PCovarianceFunction cov) { setCovariance(0,cov); }
void CKroneckerCF::setRowCovariance(PCovarianceFunction cov) { setCovariance(1,cov); }
UncertainVector<D>::UncertainVector(const value_t & v, const covariance_t & E) : _v(v) { setCovariance(E); }
Gaussian<T>::Gaussian(const MatrixT& sigma, const VectorT& _mu) { setCovariance(sigma); mu = _mu; }