예제 #1
0
ompl::base::ProjectionMatrix::Matrix ompl::base::ProjectionMatrix::ComputeRandom(const unsigned int from,
                                                                                 const unsigned int to,
                                                                                 const std::vector<double> &scale)
{
    namespace nu = boost::numeric::ublas;

    RNG rng;
    Matrix projection(to, from);

    for (unsigned int j = 0; j < from; ++j)
    {
        if (scale.size() == from && fabs(scale[j]) < std::numeric_limits<double>::epsilon())
            nu::column(projection, j) = nu::zero_vector<double>(to);
        else
            for (unsigned int i = 0; i < to; ++i)
                projection(i, j) = rng.gaussian01();
    }

    for (unsigned int i = 0; i < to; ++i)
    {
        nu::matrix_row<Matrix> row(projection, i);
        for (unsigned int j = 0; j < i; ++j)
        {
            nu::matrix_row<Matrix> prevRow(projection, j);
            // subtract projection
            row -= inner_prod(row, prevRow) * prevRow;
        }
        // normalize
        row /= norm_2(row);
    }

    assert(scale.size() == from || scale.size() == 0);
    if (scale.size() == from)
    {
        unsigned int z = 0;
        for (unsigned int i = 0; i < from; ++i)
        {
            if (fabs(scale[i]) < std::numeric_limits<double>::epsilon())
                z++;
            else
                nu::column(projection, i) /= scale[i];
        }
        if (z == from)
            OMPL_WARN("Computed projection matrix is all 0s");
    }
    return projection;
}
ompl::base::ProjectionMatrix::Matrix ompl::base::ProjectionMatrix::ComputeRandom(const unsigned int from, const unsigned int to, const std::vector<double> &scale)
{
    using namespace boost::numeric::ublas;

    RNG rng;
    Matrix projection(to, from);

    for (unsigned int i = 0 ; i < to ; ++i)
    {
        for (unsigned int j = 0 ; j < from ; ++j)
            projection(i, j) = rng.gaussian01();
    }

    for (unsigned int i = 0 ; i < to ; ++i)
    {
        matrix_row<Matrix> row(projection, i);
        for (unsigned int j = 0 ; j < i ; ++j)
        {
            matrix_row<Matrix> prevRow(projection, j);
            // subtract projection
            row -= inner_prod(row, prevRow) * prevRow;
        }
        // normalize
        row /= norm_2(row);
    }

    assert(scale.size() == from || scale.size() == 0);
    if (scale.size() == from)
        for (unsigned int i = 0 ; i < from ; ++i)
        {
            if (fabs(scale[i]) < std::numeric_limits<double>::epsilon())
                throw Exception("Scaling factor must be non-zero");
            boost::numeric::ublas::column(projection, i) /= scale[i];
        }
    return projection;
}