/**
* Creates a massvector representing the centre of gravity from a given list of mass vectors
* \param masses: The mass vectors, that will be combined
*/
static BITBOTS_INLINE Eigen::Vector4d create_mass_offset(const Eigen::Matrix4Xd& masses) {
Eigen::Matrix4Xd offsets(4,masses.cols());
for(uint i = 0; i < (uint)masses.cols(); ++i) {
offsets.col(i) << masses(3, i) * masses.col(i).head<3>(), masses(3, i);
}
return mass_from_offsets(offsets);
}
void PinholeCamera<DISTORTION_T>::projectHomogeneousBatch(
const Eigen::Matrix4Xd & points, Eigen::Matrix2Xd * imagePoints,
std::vector<ProjectionStatus> * stati) const
{
const int numPoints = points.cols();
for (int i = 0; i < numPoints; ++i) {
Eigen::Vector4d point = points.col(i);
Eigen::Vector2d imagePoint;
CameraBase::ProjectionStatus status = projectHomogeneous(point, &imagePoint);
imagePoints->col(i) = imagePoint;
if(stati)
stati->push_back(status);
}
}