Beispiel #1
0
//==============================================================================
void SE2::logMap(const StateSpace::State* _in, Eigen::VectorXd& _tangent) const
{
  if (_tangent.rows() != 3)
    _tangent.resize(3);

  auto in = static_cast<const State*>(_in);

  Isometry2d transform = getIsometry(in);
  _tangent.head<2>() = transform.translation();
  Eigen::Rotation2Dd rotation = Eigen::Rotation2Dd::Identity();
  rotation.fromRotationMatrix(transform.rotation());
  _tangent[2] = rotation.angle();
}
void GraphSimulator::simulate(int samples, int trajectories, bool interClosures, bool lookForClosures, const Isometry2d& offset)
{
    // grid size
    int size = 50;

    // some parameters for the generation of the samples
    int forwardSteps = 3;
    double stepLength = 1.0;
    Isometry2d maxStepTransform(utility::v2t(Vector3d(forwardSteps * stepLength, 0, 0)));

    // Fake sensor for loop-closure detection
    double fov = (forwardSteps - 1) << 1;
    cout << "FOV: " << fov << endl;

    Vector2d grid(size >> 1, size >> 1);
    cout << "Grid: " << grid.x() << ", " << grid.y() << endl;

    VectorXd probLimits(POSSIBLE_MOTIONS);
    for(int i = 0; i < probLimits.size(); ++i)
    {
        probLimits[i] = (i + 1) / (double) POSSIBLE_MOTIONS;
    }

    Matrix3d covariance;
    covariance.fill(0.);
    covariance(0, 0) = _noise[0] * _noise[0];
    covariance(1, 1) = _noise[1] * _noise[1];
    covariance(2, 2) = _noise[2] * _noise[2];
    Matrix3d information = covariance.inverse();

    SimNode start;
    start.id = 0;
    start.real_pose = offset;
    start.noisy_pose = offset;

    for(short int k = 0; k < trajectories; ++k)
    {
        _trajectories.push_back(SimGraph());
        SimGraph& traj = _trajectories.back();

        Poses& poses = traj._poses;
        poses.clear();
        poses.push_back(start);

        // Nodes
        while((int) poses.size() < samples)
        {
            // go straight for some steps ...
            for(int i = 1; i < forwardSteps && (int) poses.size() < samples; ++i)
            {
                SimNode nextPose = generatePose(poses.back(), utility::v2t(Vector3d(stepLength, 0, 0)));
                poses.push_back(nextPose);
            }
            if((int) poses.size() == samples)
            {
                break;
            }

            // ... now some other direction
            double uniform_value = Noise::uniform(0., 1.);
            int direction = 0;
            while(probLimits[direction] < uniform_value && direction + 1 < POSSIBLE_MOTIONS)
            {
                direction++;
            }
            Isometry2d nextMotion = generateMotion(direction, stepLength);
            SimNode nextPose = generatePose(poses.back(), nextMotion);

            Isometry2d nextStepFinalPose = nextPose.real_pose * maxStepTransform;
            if(fabs(nextStepFinalPose.translation().x()) >= grid[0] || fabs(nextStepFinalPose.translation().y()) >= grid[1])
            {
                for(int i = 0; i < POSSIBLE_MOTIONS; ++i)
                {
                    nextMotion = generateMotion(i, stepLength);
                    nextPose = generatePose(poses.back(), nextMotion);
                    nextStepFinalPose = nextPose.real_pose * maxStepTransform;
                    if(fabs(nextStepFinalPose.translation().x()) < grid[0] && fabs(nextStepFinalPose.translation().y()) < grid[1])
                    {
                        break;
                    }
                }
            }
            poses.push_back(nextPose);
        }
        cout << "Added Nodes" << endl;

        // Edges
        Edges& edges = traj._edges;
        edges.clear();
        for(size_t i = 1; i < poses.size(); ++i)
        {
            SimNode& prev = poses[i-1];
            SimNode& curr = poses[i];

            SimEdge* edge = new SimEdge;
            edge->from_id = prev.id;
            edge->to_id = curr.id;
            edge->real_transform = prev.real_pose.inverse() * curr.real_pose;
            edge->noisy_transform = prev.noisy_pose.inverse() * curr.noisy_pose;
            edge->information = information;

            edges.insert(edge);
            prev._connections.insert(edge);
        }
        cout << "Added Edges" << endl;

        // Loop Closures
        if(lookForClosures)
        {
            // Closures
            for(int i = poses.size()-1; i >= 0; i--)
            {
                SimNode& sp = poses[i];
//                for(int j = 0; j < i; j+=20)
                for(int j = 0; j < i; j+=5)
                {
                    SimNode& candidate = poses[j];
                    Isometry2d transform = sp.real_pose.inverse() * candidate.real_pose;
                    double distance = utility::t2v(transform).squaredNorm();
                    if(fabs(distance) <= fov)
                    {
                        SimEdge* loopClosure = new SimEdge;
                        loopClosure->from_id = sp.id;
                        loopClosure->to_id = candidate.id;
                        loopClosure->real_transform = transform;
                        loopClosure->noisy_transform = transform;
                        loopClosure->information = information;

                        edges.insert(loopClosure);
                        sp._connections.insert(loopClosure);
                    }
                }
            }
        }
    }
    cout << "Added Loop Closures" << endl;

    // Inter Graph Closures
    if(interClosures)
    {
        Edges& closures = _closures;

        for(uint i = 1; i < _trajectories.size(); ++i)
        {
            SimGraph& t1 = _trajectories[i-1];
            SimGraph& t2 = _trajectories[i];
            const Poses& g1_poses = t1.poses();
            const Poses& g2_poses = t2.poses();
            for(uint i = 0; i < g2_poses.size(); i+=5)
            {
                const SimNode& sp = g2_poses[i];
                for(uint j = 0; j < g1_poses.size(); j+=5)
                {
                    const SimNode& candidate = g1_poses[j];
                    Isometry2d transform = sp.real_pose.inverse() * candidate.real_pose;
                    double distance = utility::t2v(transform).squaredNorm();
                    if(fabs(distance) <= fov)
                    {
                        SimEdge* graphClosure = new SimEdge;
                        graphClosure->from_id = sp.id;
                        graphClosure->to_id = candidate.id;
                        graphClosure->real_transform = transform;
                        graphClosure->noisy_transform = transform;

                        graphClosure->information = information;

                        closures.insert(graphClosure);
                    }
                }
            }
        }
        cout << "Added Inter Graph Closures" << endl;
    }
}