MoveParameters MoveParameters::operator *(float factor) const {
return MoveParameters(
x * factor,
y * factor,
z * factor,
roll * factor,
pitch * factor,
yaw * factor);
}
MoveParameters MoveParameters::operator +(const MoveParameters &other) const {
return MoveParameters(
x + other.x,
y + other.y,
z + other.z,
roll + other.roll,
pitch + other.pitch,
yaw + other.yaw);
}
void CollarLinesRegistrationPipeline::pickBestByAverage(const vector<Eigen::Matrix4f> &transformations,
Eigen::Matrix4f &mean_transformation,
cv::Mat &covariance) {
vector<MoveParameters> meassurements;
for(vector<Eigen::Matrix4f>::const_iterator t = transformations.begin();
t < transformations.end(); t++) {
meassurements.push_back(MoveParameters(*t));
}
MoveParameters mean(0, 0, 0, 0, 0, 0);
MoveParameters::getGaussDistributionOf(meassurements, mean, covariance);
mean_transformation = getTransformation(mean.x, mean.y, mean.z,
mean.roll, mean.pitch, mean.yaw).matrix();
}