void GaussianDistribution2D::merge(const GaussianDistribution2D& other) { //Compute new mean and values for new covariance matrix: double p1(probabilityAtMean()); double p2(other.probabilityAtMean()); double pSum(p1+p2); double w1(p1/pSum); double w2(p2/pSum); Vector2<double> newMean(mean*w1 + other.mean*w2); Vector2<double> diffMean(mean - other.mean); Matrix2x2<double> meanMatrix; meanMatrix.c[0].x = diffMean.x * diffMean.x; meanMatrix.c[0].y = meanMatrix.c[1].x = diffMean.x*diffMean.y; meanMatrix.c[1].y = diffMean.y * diffMean.y; //Set new values: mean = newMean; covariance = (covariance*w1 + other.covariance*w2 + meanMatrix*w1*w2); }
void GaussianDistribution3D::merge(const GaussianDistribution3D& other) { //Compute new mean and values for new covariance matrix: float p1(probabilityAtMean()); float p2(other.probabilityAtMean()); float pSum(p1 + p2); float w1(p1 / pSum); float w2(p2 / pSum); Vector3BH<> newMean(mean * w1 + other.mean * w2); Vector3BH<> diffMean(mean - other.mean); Matrix3x3BH<> meanMatrix; meanMatrix.c0.x = diffMean.x * diffMean.x; meanMatrix.c0.y = diffMean.x * diffMean.y; meanMatrix.c0.z = diffMean.x * diffMean.z; meanMatrix.c1.x = diffMean.x * diffMean.y; meanMatrix.c1.y = diffMean.y * diffMean.y; meanMatrix.c1.z = diffMean.z * diffMean.y; meanMatrix.c1.x = diffMean.x * diffMean.z; meanMatrix.c1.y = diffMean.z * diffMean.y; meanMatrix.c1.z = diffMean.z * diffMean.z; //Set new values: mean = newMean; covariance = (covariance * w1 + other.covariance * w2 + meanMatrix * w1 * w2); }