/**
 * 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);
}
Esempio n. 2
0
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);
  }
}