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