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;
}