C++ Matrix3d::fill方法代码示例

	inline bool addObservation(Point2d observation, double xFasher,
			double yFasher, LandmarkType type)
	{

		{
			EdgeSE2 * e = new EdgeSE2;

			e->vertices()[0] = optimizer.vertex(type);
			e->vertices()[1] = optimizer.vertex(CurrentVertexId);

			switch (type)
			{
			case RightL:
				observation.y += B2;
				break;
			case FrontL:
				observation.x -= A2;
				break;
			case LeftL:
				observation.y -= B2;
				break;
			case BackL:
				observation.x += A2;
				break;
			default:
				break;
			}
			e->setMeasurement(SE2(observation.x, observation.y, 0));
			Matrix3d information;
			information.fill(0.);
			information(0, 0) = xFasher;
			information(1, 1) = yFasher;
			information(2, 2) = 1;
			e->setInformation(information);

			g2o::RobustKernelCauchy* rk = new g2o::RobustKernelCauchy;
			e->setRobustKernel(rk);

			optimizer.addEdge(e);
		}
		atLeastOneObservation = true;
		return true;
	}

	inline void updateVertexIdx()
	{
		if ((ros::Time::now() - lastSavedNodeTime).toSec() >= 0.03)
		{
			nodeCounter++;
			lastSavedNodeTime = ros::Time::now();
			PreviousVertexId = CurrentVertexId;
			CurrentVertexId++;
			if (CurrentVertexId - LandmarkCount >= 100)
			{
				CurrentVertexId = LandmarkCount;
			}

			{
				VertexSE2 * r = new VertexSE2;
				r->setEstimate(Eigen::Vector3d(location.x, location.y, 0));
				r->setFixed(false);
				r->setId(CurrentVertexId);
				if (optimizer.vertex(CurrentVertexId) != NULL)
				{
					optimizer.removeVertex(optimizer.vertex(CurrentVertexId));
				}

				optimizer.addVertex(r);
			}

			{
				EdgeSE2 * e = new EdgeSE2;
				e->vertices()[0] = optimizer.vertex(PreviousVertexId);
				e->vertices()[1] = optimizer.vertex(CurrentVertexId);
				Point2d dead_reck = getOdometryFromLastGet();
				e->setMeasurement(SE2(dead_reck.x, dead_reck.y, 0));
				Matrix3d information;
				information.fill(0.);
				information(0, 0) = 200;
				information(1, 1) = 200;
				information(2, 2) = 1;
				e->setInformation(information);
				optimizer.addEdge(e);
			}
		}
	}

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;
//.........这里部分代码省略.........