MatrixHelpers.h

Go to the documentation of this file.

/*
* This file is part of ArmarX.
*
* ArmarX is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* ArmarX is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
* @package    ArmarX::Core
* @author     Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
* @date       2015
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#pragma once
 
#include <math.h>
 
#include <Eigen/Eigen>
 
namespace armarx::math
{
    class MatrixHelpers
    {
    public:
        static void
        SetRowToValue(Eigen::MatrixXf& matrix, int rowNr, float value)
        {
            for (int i = 0; i < matrix.cols(); i++)
            {
                matrix(rowNr, i) = value;
            }
        }
 
        static Eigen::Vector3f
        CalculateCog3D(const Eigen::MatrixXf& points)
        {
            Eigen::Vector3f sum(0, 0, 0);
 
            for (int i = 0; i < points.cols(); i++)
            {
                sum += points.block<3, 1>(0, i);
            }
 
            return sum / points.cols();
        }
 
        static Eigen::MatrixXf
        SubtractVectorFromAllColumns3D(const Eigen::MatrixXf& points, const Eigen::Vector3f& vec)
        {
            Eigen::MatrixXf matrix(3, points.cols());
 
            for (int i = 0; i < points.cols(); i++)
            {
                matrix.block<3, 1>(0, i) = points.block<3, 1>(0, i) - vec;
            }
 
            return matrix;
        }
    };
} // namespace armarx::math