/** * 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); } }