本文整理汇总了C++中Matrix2d::setIdentity方法的典型用法代码示例。如果您正苦于以下问题：C++ Matrix2d::setIdentity方法的具体用法？C++ Matrix2d::setIdentity怎么用？C++ Matrix2d::setIdentity使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类Matrix2d的用法示例。
在下文中一共展示了Matrix2d::setIdentity方法的2个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: main
int main(int argc, char** argv) {
CommandArgs arg;
int nlandmarks;
int nSegments;
int simSteps;
double worldSize;
bool hasOdom;
bool hasPoseSensor;
bool hasPointSensor;
bool hasPointBearingSensor;
bool hasSegmentSensor;
bool hasCompass;
bool hasGPS;
int segmentGridSize;
double minSegmentLenght, maxSegmentLenght;
std::string outputFilename;
arg.param("simSteps", simSteps, 100, "number of simulation steps");
arg.param("nLandmarks", nlandmarks, 1000, "number of landmarks");
arg.param("nSegments", nSegments, 1000, "number of segments");
arg.param("segmentGridSize", segmentGridSize, 50, "number of cells of the grid where to align the segments");
arg.param("minSegmentLenght", minSegmentLenght, 0.5, "minimal lenght of a segment in the world");
arg.param("maxSegmentLenght", maxSegmentLenght, 3, "maximal lenght of a segment in the world");
arg.param("worldSize", worldSize, 25.0, "size of the world");
arg.param("hasOdom", hasOdom, false, "the robot has an odometry" );
arg.param("hasPointSensor", hasPointSensor, false, "the robot has a point sensor" );
arg.param("hasPointBearingSensor", hasPointBearingSensor, false, "the robot has a point bearing sensor" );
arg.param("hasPoseSensor", hasPoseSensor, false, "the robot has a pose sensor" );
arg.param("hasCompass", hasCompass, false, "the robot has a compass");
arg.param("hasGPS", hasGPS, false, "the robot has a GPS");
arg.param("hasSegmentSensor", hasSegmentSensor, false, "the robot has a segment sensor" );
arg.paramLeftOver("graph-output", outputFilename, "simulator_out.g2o", "graph file which will be written", true);
arg.parseArgs(argc, argv);
std::tr1::ranlux_base_01 generator;
OptimizableGraph graph;
World world(&graph);
for (int i=0; i<nlandmarks; i++){
WorldObjectPointXY * landmark = new WorldObjectPointXY;
double x = sampleUniform(-.5,.5, &generator)*worldSize;
double y = sampleUniform(-.5,.5, &generator)*worldSize;
landmark->vertex()->setEstimate(Vector2d(x,y));
world.addWorldObject(landmark);
}
cerr << "nSegments = " << nSegments << endl;
for (int i=0; i<nSegments; i++){
WorldObjectSegment2D * segment = new WorldObjectSegment2D;
int ix = sampleUniform(-segmentGridSize,segmentGridSize, &generator);
int iy = sampleUniform(-segmentGridSize,segmentGridSize, &generator);
int ith = sampleUniform(0,3, &generator);
double th= (M_PI/2)*ith;
th=atan2(sin(th),cos(th));
double xc = ix*(worldSize/segmentGridSize);
double yc = iy*(worldSize/segmentGridSize);
double l2 = sampleUniform(minSegmentLenght, maxSegmentLenght, &generator);
double x1 = xc + cos(th)*l2;
double y1 = yc + sin(th)*l2;
double x2 = xc - cos(th)*l2;
double y2 = yc - sin(th)*l2;
segment->vertex()->setEstimateP1(Vector2d(x1,y1));
segment->vertex()->setEstimateP2(Vector2d(x2,y2));
world.addWorldObject(segment);
}
Robot2D robot(&world, "myRobot");
world.addRobot(&robot);
stringstream ss;
ss << "-ws" << worldSize;
ss << "-nl" << nlandmarks;
ss << "-steps" << simSteps;
if (hasOdom) {
SensorOdometry2D* odometrySensor=new SensorOdometry2D("odometry");
robot.addSensor(odometrySensor);
Matrix3d odomInfo = odometrySensor->information();
odomInfo.setIdentity();
odomInfo*=100;
odomInfo(2,2)=1000;
odometrySensor->setInformation(odomInfo);
ss << "-odom";
}
if (hasPoseSensor) {
SensorPose2D* poseSensor = new SensorPose2D("poseSensor");
robot.addSensor(poseSensor);
Matrix3d poseInfo = poseSensor->information();
poseInfo.setIdentity();
poseInfo*=100;
//.........这里部分代码省略.........
示例2: main
int main(int argc, char** argv) {
CommandArgs arg;
int nlandmarks;
int simSteps;
double worldSize;
bool hasOdom;
bool hasPoseSensor;
bool hasPointSensor;
bool hasPointBearingSensor;
bool hasSegmentSensor;
bool hasCompass;
bool hasGPS;
std::string outputFilename;
arg.param("simSteps", simSteps, 100, "number of simulation steps");
arg.param("worldSize", worldSize, 25.0, "size of the world");
arg.param("hasOdom", hasOdom, false, "the robot has an odometry" );
arg.param("hasPointSensor", hasPointSensor, false, "the robot has a point sensor" );
arg.param("hasPointBearingSensor", hasPointBearingSensor, false, "the robot has a point bearing sensor" );
arg.param("hasPoseSensor", hasPoseSensor, false, "the robot has a pose sensor" );
arg.param("hasCompass", hasCompass, false, "the robot has a compass");
arg.param("hasGPS", hasGPS, false, "the robot has a GPS");
arg.param("hasSegmentSensor", hasSegmentSensor, false, "the robot has a segment sensor" );
arg.paramLeftOver("graph-output", outputFilename, "simulator_out.g2o", "graph file which will be written", true);
arg.parseArgs(argc, argv);
std::tr1::ranlux_base_01 generator;
OptimizableGraph graph;
World world(&graph);
for (int i=0; i<nlandmarks; i++){
WorldObjectPointXY * landmark = new WorldObjectPointXY;
double x = sampleUniform(-.5,.5, &generator)*worldSize;
double y = sampleUniform(-.5,.5, &generator)*worldSize;
landmark->vertex()->setEstimate(Vector2d(x,y));
world.addWorldObject(landmark);
}
Robot2D robot(&world, "myRobot");
world.addRobot(&robot);
stringstream ss;
ss << "-ws" << worldSize;
ss << "-nl" << nlandmarks;
ss << "-steps" << simSteps;
if (hasOdom) {
SensorOdometry2D* odometrySensor=new SensorOdometry2D("odometry");
robot.addSensor(odometrySensor);
Matrix3d odomInfo = odometrySensor->information();
odomInfo.setIdentity();
odomInfo*=100;
odomInfo(2,2)=1000;
odometrySensor->setInformation(odomInfo);
ss << "-odom";
}
if (hasPoseSensor) {
SensorPose2D* poseSensor = new SensorPose2D("poseSensor");
robot.addSensor(poseSensor);
Matrix3d poseInfo = poseSensor->information();
poseInfo.setIdentity();
poseInfo*=100;
poseInfo(2,2)=1000;
poseSensor->setInformation(poseInfo);
ss << "-pose";
}
if (hasPointSensor) {
SensorPointXY* pointSensor = new SensorPointXY("pointSensor");
robot.addSensor(pointSensor);
Matrix2d pointInfo = pointSensor->information();
pointInfo.setIdentity();
pointInfo*=100;
pointSensor->setInformation(pointInfo);
ss << "-pointXY";
}
if (hasPointBearingSensor) {
SensorPointXYBearing* bearingSensor = new SensorPointXYBearing("bearingSensor");
robot.addSensor(bearingSensor);
bearingSensor->setInformation(bearingSensor->information()*1000);
ss << "-pointBearing";
}
robot.move(SE2());
double pStraight=0.7;
SE2 moveStraight; moveStraight.setTranslation(Vector2d(1., 0.));
double pLeft=0.15;
SE2 moveLeft; moveLeft.setRotation(Rotation2Dd(M_PI/2));
//double pRight=0.15;
SE2 moveRight; moveRight.setRotation(Rotation2Dd(-M_PI/2));
for (int i=0; i<simSteps; i++){
cerr << "m";
//.........这里部分代码省略.........