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

void IK_SetTransform(IK_Segment *seg, float start[3], float rest[][3], float basis[][3], float length)
{
	IK_QSegment *qseg = (IK_QSegment *)seg;

	Vector3d mstart(start[0], start[1], start[2]);
	// convert from blender column major
	Matrix3d mbasis = CreateMatrix(basis[0][0], basis[1][0], basis[2][0],
	                               basis[0][1], basis[1][1], basis[2][1],
	                               basis[0][2], basis[1][2], basis[2][2]);
	Matrix3d mrest = CreateMatrix(rest[0][0], rest[1][0], rest[2][0],
	                              rest[0][1], rest[1][1], rest[2][1],
	                              rest[0][2], rest[1][2], rest[2][2]);
	double mlength(length);

	if (qseg->Composite()) {
		Vector3d cstart(0, 0, 0);
		Matrix3d cbasis;
		cbasis.setIdentity();
		
		qseg->SetTransform(mstart, mrest, mbasis, 0.0);
		qseg->Composite()->SetTransform(cstart, cbasis, cbasis, mlength);
	}
	else
		qseg->SetTransform(mstart, mrest, mbasis, mlength);
}

const Matrix3d VectorMath::TMatrix(const Vector3d &v)
{
    double vnormsq = v.dot(v);
    Matrix3d I;
    I.setIdentity();
    if(vnormsq == 0)
        return I;

    Matrix3d R = rotationMatrix(v);
    return (v*v.transpose() + (R.transpose()-I)*crossProductMatrix(v))/vnormsq;
}

const Matrix3d VectorMath::rotationMatrix(const Vector3d &axisAngle)
{
    double theta = axisAngle.norm();
    Vector3d thetahat = axisAngle/theta;

    if(theta == 0)
        thetahat.setZero();

    Matrix3d result;
    result.setIdentity();
    result = cos(theta)*result + sin(theta)*crossProductMatrix(thetahat) + (1-cos(theta))*thetahat*thetahat.transpose();
    return result;
}

const Vector3d VectorMath::axisAngle(const Matrix3d &rotationMatrix)
{
    Matrix3d RminusI;
    RminusI.setIdentity();
    RminusI = rotationMatrix - RminusI;

    JacobiSVD<MatrixXd> svd(RminusI, ComputeFullV);
    //assert(fabs(svd.singularValues()[2]) < 1e-8);
    Vector3d axis = svd.matrixV().col(2);
    Vector3d testAxis = perpToAxis(axis);
    Vector3d resultAxis = rotationMatrix*testAxis;
    double theta = atan2(testAxis.cross(resultAxis).dot(axis), testAxis.dot(resultAxis));
    return theta*axis;
}

/*
 * deviator of a tensor
 */
static Matrix3d Deviator(Matrix3d M) {
	Matrix3d eye;
	eye.setIdentity();
	eye *= M.trace() / 3.0;
	return M - eye;
}

  // Set up sparse linear system for Delayed Sparse Information Filter
  void SysSPA2d::setupSparseDSIF(int newnode)
  {
    cout << "[SetupDSIF] at " << newnode << endl;

    // set matrix sizes and clear
    // assumes scales vars are all free
    int nFree = nodes.size() - nFixed;

    //    long long t0, t1, t2, t3;
    //    t0 = utime();

    // don't erase old stuff here, the delayed filter just grows in size
    csp.setupBlockStructure(nFree,false); // initialize CSparse structures

    //    t1 = utime();

    // loop over P2 constraints
    for(size_t pi=0; pi<p2cons.size(); pi++)
      {
        Con2dP2 &con = p2cons[pi];

        // don't consider old constraints
        if (con.ndr < newnode && con.nd1 < newnode)
          continue;

        con.setJacobians(nodes);
        Matrix3d J;
        J.setIdentity();
        Vector2d dRdth = nodes[con.ndr].dRdx.transpose() * con.tmean;
        J(0,2) = dRdth(0);
        J(1,2) = dRdth(1);
        Matrix3d Jt = J.transpose();
        Vector3d u;
        u.head(2) = nodes[con.ndr].trans.head(2);
        u(2) = nodes[con.ndr].arot;
        Vector3d f;
        f.head(2) = u.head(2) + nodes[con.ndr].w2n.transpose().block<2,2>(0,0) * con.tmean;
        f(2) = u(2) + con.amean;
        if (f(2) > M_PI) f(2) -= 2.0*M_PI;
        if (f(2) < M_PI) f(2) += 2.0*M_PI;

        
        cout << "[SetupDSIF] u  = " << u.transpose() << endl;
        cout << "[SetupDSIF] f  = " << f.transpose() << endl;
        cout << "[SetupDSIF] fo = " << nodes[con.nd1].trans.transpose() << endl;


        // add in 4 blocks of A; actually just need upper triangular
        int i0 = con.ndr-nFixed; // will be negative if fixed
        int i1 = con.nd1-nFixed; // will be negative if fixed
        
        if (i0 != i1-1) continue; // just sequential nodes for now

        if (i0>=0)
          {
            Matrix<double,3,3> m = Jt*con.prec*J;
            csp.addDiagBlock(m,i0);
          }
        if (i1>=0)
          {
            Matrix<double,3,3> m = con.prec;
            csp.addDiagBlock(m,i1);

            if (i0>=0)
              {
                Matrix<double,3,3> m2 = -con.prec * J;


                if (i1 < i0)
                  {
                    m = m2.transpose();
                    csp.addOffdiagBlock(m,i1,i0);
                  }
                else
                  {
                    csp.addOffdiagBlock(m2,i0,i1);
                  }
              }
          }

        // add in 2 blocks of B
        // Jt Gamma (J u - e)
        Vector3d Juf = J*u - f;
        if (Juf(2) > M_PI) Juf(2) -= 2.0*M_PI;
        if (Juf(2) < M_PI) Juf(2) += 2.0*M_PI;
        if (i0>=0)
          {
            csp.B.block<3,1>(i0*3,0) += Jt * con.prec * Juf;
          }
        if (i1>=0)
          {
            csp.B.block<3,1>(i1*3,0) -= con.prec * Juf;
          }

      } // finish P2 constraints

    //    t2 = utime();

    csp.Bprev = csp.B;          // save for next iteration
//.........这里部分代码省略.........

  /// Run the Delayed Sparse Information Filter (Eustice et al.)
  /// <newnode> is the index of the first new node added since the last iteration
  /// <useCSparse> is true for sparse Cholesky.
  void SysSPA2d::doDSIF(int newnode, bool useCSparse)
  {
    // number of nodes
    int nnodes = nodes.size();

    // set number of constraints
    int ncons = p2cons.size();

    // check for fixed frames
    if (nFixed <= 0)
      {
        cout << "[doDSIF] No fixed frames" << endl;
        return;
      }

    // check for newnode being ok
    if (newnode >= nnodes)
      {
        cout << "[doDSIF] no new nodes to add" << endl;
        return;
      }

    nFixed = 0;

    for (int i=0; i<nnodes; i++)
      {
        Node2d &nd = nodes[i];
        if (i >= nFixed)
          nd.isFixed = false;
        else 
          nd.isFixed = true;
        nd.setTransform();      // set up world-to-node transform for cost calculation
        nd.setDr();             // always use local angles
      }

    // initialize vars
    double cost = calcCost();
    if (verbose)
      cout << " Initial squared cost: " << cost << " which is " 
           << sqrt(cost/ncons) << " rms error" << endl;

    if (newnode == 1)
      {
        // set up first system with node 0
        csp.setupBlockStructure(1,false); // initialize CSparse structures
        Matrix3d m;
        m.setIdentity();
        m = m*1000000;
        csp.addDiagBlock(m,0);
        Vector3d u;
        u.head(2) = nodes[0].trans.head(2);
        u(2) = nodes[0].arot;
        csp.B.head(3) = u*1000000;
        csp.Bprev = csp.B;              // save for next iteration
        cout << "[doDSIF] B = " << csp.B.transpose() << endl;    
      }

    // set up and solve linear system
    // NOTE: shouldn't need to redo all calcs in setupSys if we 
    //   got here from a bad update

    long long t0, t1, t2, t3;
    t0 = utime();
    if (useCSparse)
      setupSparseDSIF(newnode); // set up sparse linear system
    else
      {}

#if 1
    cout << "[doDSIF] B = " << csp.B.transpose() << endl;
    csp.uncompress(A);
    cout << "[doDSIF] A = " << endl << A << endl;
#endif

    //        cout << "[SPA] Solving...";
    t1 = utime();
    if (useCSparse)
      {
        bool ok = csp.doChol();
        if (!ok)
          cout << "[doDSIF] Sparse Cholesky failed!" << endl;
      }
    else
      A.ldlt().solveInPlace(B); // Cholesky decomposition and solution
    t2 = utime();
    //        cout << "solved" << endl;

    // get correct result vector
    VectorXd &BB = useCSparse ? csp.B : B;

    cout << "[doDSIF] RES  = " << BB.transpose() << endl;

    // update the frames
    int ci = 0;
    for(int i=0; i < nnodes; i++)
      {
        Node2d &nd = nodes[i];
//.........这里部分代码省略.........

bool scso3explog_tests()
{
  double pi = 3.14159265;
  vector<ScSO3> omegas;
  omegas.push_back(ScSO3::exp(Vector4d(0.2, 0.5, 0.0, 1.)));
  omegas.push_back(ScSO3::exp(Vector4d(0.2, 0.5, -1.0, 1.1)));
  omegas.push_back(ScSO3::exp(Vector4d(0., 0., 0., 1.1)));
  omegas.push_back(ScSO3::exp(Vector4d(0., 0., 0.00001, 0.)));
  omegas.push_back(ScSO3::exp(Vector4d(0., 0., 0.00001, 0.00001)));
  omegas.push_back(ScSO3::exp(Vector4d(0., 0., 0.00001, 0)));
  omegas.push_back(ScSO3::exp(Vector4d(pi, 0, 0, 0.9)));
  omegas.push_back(ScSO3::exp(Vector4d(0.2, 0.5, 0.0,0))
                   *ScSO3::exp(Vector4d(pi, 0, 0,0.0))
                   *ScSO3::exp(Vector4d(-0.2, -0.5, -0.0,0)));
  omegas.push_back(ScSO3::exp(Vector4d(0.3, 0.5, 0.1,0))
                   *ScSO3::exp(Vector4d(pi, 0, 0,0))
                   *ScSO3::exp(Vector4d(-0.3, -0.5, -0.1,0)));

  bool failed = false;

  for (size_t i=0; i<omegas.size(); ++i)
  {
    Matrix3d sR1 = omegas[i].matrix();
    Matrix3d sR2 = ScSO3::exp(omegas[i].log()).matrix();
    Matrix3d DiffR = sR1-sR2;
    double nrm = DiffR.norm();

    //// ToDO: Force ScSO3 to be more accurate!
    if (isnan(nrm) || nrm>SMALL_EPS)
    {
      cerr << "ScSO3 - exp(log(ScSO3))" << endl;
      cerr  << "Test case: " << i << endl;
      cerr << sR1 << endl;
      cerr << omegas[i].log() << endl;
      cerr << sR2 << endl;
      cerr << DiffR <<endl;
      cerr << endl;
      failed = true;
    }

  }

  for (size_t i=0; i<omegas.size(); ++i)
  {
    Vector3d p(1,2,4);
    Matrix3d sR = omegas[i].matrix();
    Vector3d res1 = omegas[i]*p;
    Vector3d res2 = sR*p;

    double nrm = (res1-res2).norm();

    if (isnan(nrm) || nrm>SMALL_EPS)
    {
      cerr << "Transform vector" << endl;
      cerr  << "Test case: " << i << endl;
      cerr << (res1-res2) <<endl;
      cerr << endl;
      failed = true;
    }
  }

  for (size_t i=0; i<omegas.size(); ++i)
  {
    Matrix3d q = omegas[i].matrix();
    Matrix3d inv_q = omegas[i].inverse().matrix();
    Matrix3d res = q*inv_q ;
    Matrix3d I;
    I.setIdentity();

    double nrm = (res-I).norm();

    if (isnan(nrm) || nrm>SMALL_EPS)
    {
      cerr << "Inverse" << endl;
      cerr  << "Test case: " << i << endl;
      cerr << (res-I) <<endl;
      cerr << endl;
      failed = true;
    }
    Matrix3d R = omegas[i].rotationMatrix();
    cerr << R*R.transpose() << endl;

  }
  return failed;
}

void Omega3::AcquisitionTask::run(){
    while(1){

        //! qDebug()<< "try to open the first available device";
        if (drdOpen () < 0) {
            //qDebug()<<"no device available...";
            //dhdSleep (2.0);
            omegaDetected = false;
        }
        else{
            omegaDetected = true;
        }

        if(omegaDetected){
            break;
        }

        sleep(1);
    }

    done  = 0;
    double p[DHD_MAX_DOF];
    double r[3][3];
    double v[DHD_MAX_DOF];
    double f[DHD_MAX_DOF];
    double normf, normt;

    double t0    = dhdGetTime ();
    double t1    = t0;
    bool   base  = false;
    bool   wrist = false;
    bool   grip  = false;

    //! Eigen objects (mapped to the arrays above)
    Map<Vector3d> position(&p[0], 3);
    Map<Vector3d> force   (&f[0], 3);
    Map<Vector3d> torque  (&f[3], 3);
    Map<Vector3d> velpos  (&v[0], 3);
    Map<Vector3d> velrot  (&v[3], 3);
    Matrix3d center;
    Matrix3d rotation;

    //! center of workspace
    center.setIdentity ();                           // rotation (matrix)
    double nullPose[DHD_MAX_DOF] = { 0.0, 0.0, 0.0,  // translation
                                     0.0, 0.0, 0.0,  // rotation (joint angles)
                                     0.0 };          // gripper

    //! print out device identifier
    if (!drdIsSupported ()) {
      dhdSleep (2.0);
      drdClose ();
    }

    //! perform auto-initialization
    if (!drdIsInitialized () && drdAutoInit () < 0) {
      qDebug()<<"error: auto-initialization failed"<<dhdErrorGetLastStr ();
      dhdSleep (2.0);
    }
    else if (drdStart () < 0) {
      printf ("error: regulation thread failed to start (%s)\n", dhdErrorGetLastStr ());
      dhdSleep (2.0);
    }

    //! move to center
    drdMoveTo (nullPose);

    // request a null force (only gravity compensation will be applied)
    // this will only apply to unregulated axis
    for (int i=0; i<DHD_MAX_DOF; i++) f[i] = 0.0;
    drdSetForceAndTorqueAndGripperForce (f);

    // disable all axis regulation (but leave regulation thread running)
    drdRegulatePos  (base);
    drdRegulateRot  (wrist);
    drdRegulateGrip (grip);

    //! save current pos
    drdGetPositionAndOrientation (p, r);
    for (int i=0; i<3; i++) rotation.row(i) = Vector3d::Map(&r[i][0], 3);

    this->omega->patientHandling->setOmegaPosition(p[0],p[1],p[2]);

    long long cpt = 0;

    // loop while the button is not pushed
    while (!done) {

      // synchronize with regulation thread
      drdWaitForTick ();

      // get position/orientation/gripper and update Eigen rotation matrix
      drdGetPositionAndOrientation (p, r);
      for (int i=0; i<3; i++) rotation.row(i) = Vector3d::Map(&r[i][0], 3);

      if(cpt%31==0){
          this->omega->patientHandling->setOmegaPosition(p[0],p[1],p[2]);
      }
      else{
          cpt += 1;
//.........这里部分代码省略.........

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

bool se2explog_tests()
{
  double pi = 3.14159265;
  vector<SE2> omegas;
  omegas.push_back(SE2(SO2(0.0),Vector2d(0,0)));
  omegas.push_back(SE2(SO2(0.2),Vector2d(10,0)));
  omegas.push_back(SE2(SO2(0.),Vector2d(0,100)));
  omegas.push_back(SE2(SO2(-1.),Vector2d(20,-1)));
  omegas.push_back(SE2(SO2(0.00001),Vector2d(-0.00000001,0.0000000001)));
  omegas.push_back(SE2(SO2(0.2),Vector2d(0,0))
                   *SE2(SO2(pi),Vector2d(0,0))
                   *SE2(SO2(-0.2),Vector2d(0,0)));
  omegas.push_back(SE2(SO2(0.3),Vector2d(2,0))
                   *SE2(SO2(pi),Vector2d(0,0))
                   *SE2(SO2(-0.3),Vector2d(0,6)));

  bool failed = false;

  for (size_t i=0; i<omegas.size(); ++i)
  {
    Matrix3d R1 = omegas[i].matrix();
    Matrix3d R2 = SE2::exp(omegas[i].log()).matrix();
    Matrix3d DiffR = R1-R2;
    double nrm = DiffR.norm();

    if (isnan(nrm) || nrm>SMALL_EPS)
    {
      cerr << "SE2 - exp(log(SE2))" << endl;
      cerr  << "Test case: " << i << endl;
      cerr << DiffR <<endl;
      cerr << endl;
      failed = true;
    }
  }
  for (size_t i=0; i<omegas.size(); ++i)
  {
    Vector2d p(1,2);
    Matrix3d T = omegas[i].matrix();
    Vector2d res1 = omegas[i]*p;
    Vector2d res2 = T.topLeftCorner<2,2>()*p + T.topRightCorner<2,1>();

    double nrm = (res1-res2).norm();

    if (isnan(nrm) || nrm>SMALL_EPS)
    {
      cerr << "Transform vector" << endl;
      cerr  << "Test case: " << i << endl;
      cerr << (res1-res2) <<endl;
      cerr << endl;
      failed = true;
    }
  }

  for (size_t i=0; i<omegas.size(); ++i)
  {
    Matrix3d q = omegas[i].matrix();
    Matrix3d inv_q = omegas[i].inverse().matrix();
    Matrix3d res = q*inv_q ;
    Matrix3d I;
    I.setIdentity();

    double nrm = (res-I).norm();

    if (isnan(nrm) || nrm>SMALL_EPS)
    {
      cerr << "Inverse" << endl;
      cerr  << "Test case: " << i << endl;
      cerr << (res-I) <<endl;
      cerr << endl;
      failed = true;
    }
  }
  return failed;

}

void CFullRelPose::writeEdge(std::ostream & toroGraphFile, const bool b2d) const
{
	if(IS_DEBUG) CHECK(!hasPosition(), "Node is disconnected?? Or too bad");

	const double dLength = length();
	//double dVar = sqr(dLength/4);// scale.scaleVarHacked();
	double dVar = variance();
	dVar = std::max<double>(1e-5, dVar);

	normalisedPose.scale(dLength).write(toroGraphFile, b2d);

	Vector3d t, x_axis; x_axis.setZero();
	normalisedPose.t.asVector(t);

	if(b2d)
	{
		//double inf_ff, inf_fs, inf_ss, inf_rr, inf_fr, inf_sr;
		Vector2d t2d(t(0), t(2));
		Vector2d t2d_perp(t(2), -t(0));

		Matrix2d Q, LAMBDA;
		LAMBDA.setZero();

		Q << t2d, t2d_perp;

		LAMBDA(0,0) = dVar; //dVar may be zero
		LAMBDA(1,1) = dVar * sqr(normalisedPose.SD.cameraMotionAngleSD());

		CHECK(isnan(LAMBDA.sum()), "writeEdge: nan")

		const Matrix2d C = Q * LAMBDA * Q.transpose();
		const Matrix2d & I = C.inverse();
		//cout << I << endl;

		//THROW("Not complete...")
		double dRotInf = 1.0/sqr(normalisedPose.SD.relOrientationSD());
		toroGraphFile << I(0,0) << ' ' << I(1,0) << ' ' << I(1,1) << ' ' << dRotInf << " 0 0";

		return;
	}

	if(t(0) < 0.9) //check t isn't x axis aligned (x is arbitrary)
		x_axis(0) = 1;
	else
		x_axis(1) = 1;

	Matrix3d Q;

	if(t.sum() == 0) //Pure rotation. Should be arbitrary
	{
		Q.setIdentity();
	}
	else
	{
		Vector3d tperp1 = t.cross(x_axis);
		tperp1.normalize();

		Vector3d tperp2 = tperp1.cross(t);

		Q << t, tperp1, tperp2;
	}

	CHECK(isnan(Q.sum()), "writeEdge: nan")

	Matrix3d LAMBDA; LAMBDA.setZero();
	LAMBDA(0,0) = dVar; //dVar may be zero
	LAMBDA(1,1) = LAMBDA(2,2) = dVar * sqr(normalisedPose.SD.cameraMotionAngleSD());

	CHECK(isnan(LAMBDA.sum()), "writeEdge: nan")

	const Matrix3d C = Q * LAMBDA * Q.transpose();

	//std::cout << "Covariance: " << std::endl << C << std::endl;
	CHECK(isnan(C.sum()), "writeEdge: nan")

	Matrix3d Crpy; Crpy.setZero();
	Crpy.diagonal().setConstant(sqr(normalisedPose.SD.relOrientationSD()));
	Matrix<double, 6, 6> Cfull; Cfull << C, Matrix3d::Zero(), Matrix3d::Zero(), Crpy;

	CHECK(isnan(Cfull.sum()), "writeEdge: nan")

	if(Cfull.trace() < 0.0001)
	{
		std::cout << "Warning: Covariance matrix near-singular, adjusting diagonal\n";
		Cfull.diagonal().array() += 0.0001;
	}

	const Matrix<double, 6, 6> & Ifull = Cfull.inverse();
	//std::cout << "Information: " << std::endl << Ifull << std::endl;

	//cout << "Warning: Not inverting information matrix\n"; Yes the information mat does work a little better.
	//Possibly ok for 1 edge...if(IS_DEBUG) CHECK(Cfull.determinant()<0.0001, "CFullRelPose::writeEdge: Singular covariance matrix");

	CHECK(isnan(Ifull.sum()), "writeEdge: nan")

	for (int nRow = 0; nRow <6; nRow++)
	{
		for(int nCol = nRow; nCol < 6; nCol++)
			toroGraphFile << Ifull(nRow, nCol) << ' ';
	}
//.........这里部分代码省略.........

bool so3explog_tests()
{

  vector<SO3> omegas;
  omegas.push_back(SO3(Quaterniond(0.1e-11, 0., 1., 0.)));
  omegas.push_back(SO3(Quaterniond(-1,0.00001,0.0,0.0)));
  omegas.push_back(SO3::exp(Vector3d(0.2, 0.5, 0.0)));
  omegas.push_back(SO3::exp(Vector3d(0.2, 0.5, -1.0)));
  omegas.push_back(SO3::exp(Vector3d(0., 0., 0.)));
  omegas.push_back(SO3::exp(Vector3d(0., 0., 0.00001)));
  omegas.push_back(SO3::exp(Vector3d(M_PI, 0, 0)));
  omegas.push_back(SO3::exp(Vector3d(0.2, 0.5, 0.0))
                   *SO3::exp(Vector3d(M_PI, 0, 0))
                   *SO3::exp(Vector3d(-0.2, -0.5, -0.0)));
  omegas.push_back(SO3::exp(Vector3d(0.3, 0.5, 0.1))
                   *SO3::exp(Vector3d(M_PI, 0, 0))
                   *SO3::exp(Vector3d(-0.3, -0.5, -0.1)));

  bool failed = false;

  for (size_t i=0; i<omegas.size(); ++i)
  {
    Matrix3d R1 = omegas[i].matrix();
    double theta;
    Matrix3d R2 = SO3::exp(SO3::logAndTheta(omegas[i],&theta)).matrix();

    Matrix3d DiffR = R1-R2;
    double nrm = DiffR.norm();

    if (isnan(nrm) || nrm>SMALL_EPS)
    {
      cerr << "SO3 - exp(log(SO3))" << endl;
      cerr  << "Test case: " << i << endl;
      cerr << DiffR <<endl;
      cerr << endl;
      failed = true;
    }

    if (theta>M_PI || theta<-M_PI)
    {
      cerr << "log theta not in [-pi,pi]" << endl;
      cerr  << "Test case: " << i << endl;
      cerr << theta <<endl;
      cerr << endl;
      failed = true;
    }

  }

  for (size_t i=0; i<omegas.size(); ++i)
  {
    Vector3d p(1,2,4);
    Matrix3d sR = omegas[i].matrix();
    Vector3d res1 = omegas[i]*p;
    Vector3d res2 = sR*p;

    double nrm = (res1-res2).norm();

    if (isnan(nrm) || nrm>SMALL_EPS)
    {
      cerr << "Transform vector" << endl;
      cerr  << "Test case: " << i << endl;
      cerr << (res1-res2) <<endl;
      cerr << endl;
      failed = true;
    }
  }

  for (size_t i=0; i<omegas.size(); ++i)
  {
    Matrix3d q = omegas[i].matrix();
    Matrix3d inv_q = omegas[i].inverse().matrix();
    Matrix3d res = q*inv_q ;
    Matrix3d I;
    I.setIdentity();

    double nrm = (res-I).norm();

    if (isnan(nrm) || nrm>SMALL_EPS)
    {
      cerr << "Inverse" << endl;
      cerr  << "Test case: " << i << endl;
      cerr << (res-I) <<endl;
      cerr << endl;
      failed = true;
    }
  }
  return failed;
}

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