OMPL官方教程学习Using existing samplers and creating new ones

Available State Samplers

1. 有两种不同类型的采样器：
   1. StateSampler (不考虑状态的有效性？不管障碍物都进行采样？)
   2. ValidStateSampler (考虑状态的有效性，考虑非障碍物区域么？)
   3. 两种采样器都具有三种采样方式：均匀、在一个状态周围进行采样(?)、高斯分布。
   4. ValidStateSampler 使用StateSampler 作为底层实现
   5. ValidStateSampler 使用StateSampler 的方法进行采样，直到碰见无效点或者迭代结束
   6. 采样有效性的衡量是通过SpaceInformation::isValid方法定义的

There are two different kinds of samplers that sound similar, but have different roles: state space samplers (ompl::base::StateSampler) and valid state samplers (ompl::base::ValidStateSampler). For each type of state space there needs to exist a corresponding derived ompl::base::StateSampler class that allows one to generate uniform samples from that state space, generate states near another state from that state space and generate states using a Gaussian distribution. The valid state samplers use the state space samplers as a low level primitive. Typically, they generate a number of state samples using the appropriate state space sampler until a valid state is found or a maximum number of iterations is exceeded. The validity of a state is determined through the ompl::base::SpaceInformation::isValid method.

已经定义好的几个ValidStateSampler：

1. ompl::base::UniformValidStateSampler ： 均匀采样吧

2. ompl::base::ObstacleBasedValidStateSampler：有点类似二分法

   This sampler tries to first find one invalid sample and one valid sample. Next, it interpolates states incrementally from the valid to the invalid state. It returns the last state that is valid before reaching an invalid state. The idea is that samples near obstacles improve the chance of finding samples in narrow passages. Finding such samples is often the crucial problem in solving motion planning queries.

3. ompl::base::GaussianValidStateSampler: 先按均匀分布生成一个点，然后以这个点为中心进行一次高斯采样，如果第一个点有效而第二个点无效则，第一个点是采样点。否则重新采样

4. ompl::base::MaximizeClearanceValidStateSampler ：以Clearance最大进行采样？

   This sampler behaves a lot like ompl::base::UniformValidStateSampler but once it finds a valid state, it attempts to find additional valid states with higher clearance. The reported sample is the one with highest clearance.

1. Using an Existing Sampler

1. 需要我们自己定义一个ValidStateSamplerAllocator 实现相同的功能
2. 实现的功能：输入SpaceInformation 输出 ValidStateSamplerPtr

We cannot set the type of sampler directly in the SimpleSetup or SpaceInformation classes, because each thread needs it own copy of a sampler. Instead, we need to define a ompl::base::ValidStateSamplerAllocator, a function that, given a pointer to an ompl::base::SpaceInformation, returns ompl::base::ValidStateSamplerPtr. This function can also configure the valid state sampler according to the specific space information before returning it.

官方给的代码示例从中可以看出：

1. 我们需要定义一个函数，其输入是SpaceInformation 输出是 ValidStateSamplerPtr
2. 函数叫什么名字无关紧要。
3. ValidStateSamplerPtr 是所有基于Valid采样的父类，而使用过程多是用ValidStateSamplerPtr 这样更加统一(跟之前的状态空间类似)

namespace ob = ompl::base;
namespace og = ompl::geometric;
// 返回的是 ValidStateSamplerPtr对象，输入的是SpaceInformation对象
ob::ValidStateSamplerPtr allocOBValidStateSampler(const ob::SpaceInformation *si)
{
    // we can perform any additional setup / configuration of a sampler here,
    // but there is nothing to tweak in case of the ObstacleBasedValidStateSampler.
    // 创建一个ObstacleBasedValidStateSampler采样对象
    return std::make_shared<ob::ObstacleBasedValidStateSampler>(si);
}

2.Creating a New Valid State Sampler

自己构建一个有效状态采样器，这应用范围就太大了。

官方提供的示例，从中可以看出：

1. 自己定义的有效状态采样器要继承自ValidStateSampler
2. 对应的构造函数需要输入SpaceInformation来构造父类ValidStateSampler
3. 需要用户实现bool sample(ob::State *state)函数。
   1. 将State对象转换成需要的对象
   2. 采样得到State
   3. 调用si_->isValid(state)，si_应该就是传入的SpaceInformation在类对象内部表示

namespace ob = ompl::base;
namespace og = ompl::geometric;
 
 
 
// This is a problem-specific sampler that automatically generates valid
// states; it doesn't need to call SpaceInformation::isValid. This is an
// example of constrained sampling. If you can explicitly describe the set valid
// states and can draw samples from it, then this is typically much more
// efficient than generating random samples from the entire state space and
// checking for validity.

// 自定义的有效状态采样器要继承自ValidStateSampler
class MyValidStateSampler : public ob::ValidStateSampler
{
public:
    // 构造函数输入依然是SpaceInformation，用来构造父类
    MyValidStateSampler(const ob::SpaceInformation *si) : ValidStateSampler(si)
    {
        name_ = "my sampler";
    }
    // Generate a sample in the valid part of the R^3 state space
    // Valid states satisfy the following constraints:
    // -1<= x,y,z <=1
    // if .25 <= z <= .5, then |x|>.8 and |y|>.8
    // 需要自己定义sampl函数，格式应该是固定的，输入状态输出是否有效
    bool sample(ob::State *state) override
    {
        // 将输入的状态装换成需要的类型，并获取其数据，也可能没有数据呢
        double* val = static_cast<ob::RealVectorStateSpace::StateType*>(state)->values;
        double z = rng_.uniformReal(-1,1);
        // 根据上面确定的分段空间来确定采样值
        if (z>.25 && z<.5)
        {
            double x = rng_.uniformReal(0,1.8), y = rng_.uniformReal(0,.2);
            switch(rng_.uniformInt(0,3))
            {
                case 0: val[0]=x-1;  val[1]=y-1;  break;
                case 1: val[0]=x-.8; val[1]=y+.8; break;
                case 2: val[0]=y-1;  val[1]=x-1;  break;
                case 3: val[0]=y+.8; val[1]=x-.8; break;
            }
        }
        else
        {
            val[0] = rng_.uniformReal(-1,1);
            val[1] = rng_.uniformReal(-1,1);
        }
        val[2] = z;
        // 判断采样值是否有效，这个isValid函数也要自己定义的。。。。
        assert(si_->isValid(state));
        return true;
    }
    // We don't need this in the example below.
    bool sampleNear(ob::State* /*state*/, const ob::State* /*near*/, const double /*distance*/) override
    {
        throw ompl::Exception("MyValidStateSampler::sampleNear", "not implemented");
        return false;
    }
protected:
    ompl::RNG rng_;
};

总结来看：

1. 只要使用采样器，就需要实现与ValidStateSamplerAllocator功能相同的函数
2. 要想创建自己的采样器，要继承自ValidStateSampler并实现sample、sampleNear这两个虚函数。