C++ SparseOptimizer::edges方法代码示例

void MainWindow::setRobustKernel()
{
  SparseOptimizer* optimizer = viewer->graph;
  bool robustKernel = cbRobustKernel->isChecked();
  double huberWidth = leKernelWidth->text().toDouble();  
  //odometry edges are those whose node ids differ by 1
  
  bool onlyLoop = cbOnlyLoop->isChecked();

  if (robustKernel) {
    QString strRobustKernel = coRobustKernel->currentText();
    AbstractRobustKernelCreator* creator = RobustKernelFactory::instance()->creator(strRobustKernel.toStdString());
    if (! creator) {
      cerr << strRobustKernel.toStdString() << " is not a valid robust kernel" << endl;
      return;
    }
    for (SparseOptimizer::EdgeSet::const_iterator it = optimizer->edges().begin(); it != optimizer->edges().end(); ++it) {
      OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
      if (onlyLoop) {
        if (e->vertices().size() >= 2 && std::abs(e->vertex(0)->id() - e->vertex(1)->id()) != 1) {
          e->setRobustKernel(creator->construct());
          e->robustKernel()->setDelta(huberWidth);
        }
      } else {
        e->setRobustKernel(creator->construct());
        e->robustKernel()->setDelta(huberWidth);
      }
    }    
  } else {
    for (SparseOptimizer::EdgeSet::const_iterator it = optimizer->edges().begin(); it != optimizer->edges().end(); ++it) {
      OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
      e->setRobustKernel(0);
    }
  }
}

void MainWindow::setRobustKernel()
{
  SparseOptimizer* optimizer = viewer->graph;
  bool robustKernel = cbRobustKernel->isChecked();
  double huberWidth = leKernelWidth->text().toDouble();

  if (robustKernel) {
    QString strRobustKernel = coRobustKernel->currentText();
    AbstractRobustKernelCreator* creator = RobustKernelFactory::instance()->creator(strRobustKernel.toStdString());
    if (! creator) {
      cerr << strRobustKernel.toStdString() << " is not a valid robust kernel" << endl;
      return;
    }
    for (SparseOptimizer::EdgeSet::const_iterator it = optimizer->edges().begin(); it != optimizer->edges().end(); ++it) {
      OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
      e->setRobustKernel(creator->construct());
      e->robustKernel()->setDelta(huberWidth);
    }
  } else {
    for (SparseOptimizer::EdgeSet::const_iterator it = optimizer->edges().begin(); it != optimizer->edges().end(); ++it) {
      OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
      e->setRobustKernel(0);
    }
  }
}

//.........这里部分代码省略.........
  for (SparseOptimizer::VertexIDMap::const_iterator it = optimizer.vertices().begin(); it != optimizer.vertices().end(); ++it) {
    OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(it->second);
    if (v->fixed()) {
      cerr << "\t fixed vertex " << it->first << endl;
    }
  }

  VertexSE2* laserOffset = dynamic_cast<VertexSE2*>(optimizer.vertex(Gm2dlIO::ID_LASERPOSE));
  VertexOdomDifferentialParams* odomParamsVertex = dynamic_cast<VertexOdomDifferentialParams*>(optimizer.vertex(Gm2dlIO::ID_ODOMCALIB));

  if (fixLaser) {
    cerr << "Fix position of the laser offset" << endl;
    laserOffset->setFixed(true);
  }

  signal(SIGINT, sigquit_handler);
  cerr << "Doing full estimation" << endl;
  optimizer.initializeOptimization();
  optimizer.computeActiveErrors();
  cerr << "Initial chi2 = " << FIXED(optimizer.chi2()) << endl;

  int i=optimizer.optimize(maxIterations);
  if (maxIterations > 0 && !i){
    cerr << "optimize failed, result might be invalid" << endl;
  }

  if (laserOffset) {
    cerr << "Calibrated laser offset (x, y, theta):" << laserOffset->estimate().toVector().transpose() << endl;
  }

  if (odomParamsVertex) {
    cerr << "Odometry parameters (scaling factors (v_l, v_r, b)): " << odomParamsVertex->estimate().transpose() << endl;
  }

  cerr << "vertices: " << optimizer.vertices().size() << endl;
  cerr << "edges: " << optimizer.edges().size() << endl;

  if (dumpGraphFilename.size() > 0) {
    cerr << "Writing " << dumpGraphFilename << " ... ";
    optimizer.save(dumpGraphFilename.c_str());
    cerr << "done." << endl;
  }

  // optional input of a seperate file for applying the odometry calibration
  if (odomTestFilename.size() > 0) {

    DataQueue testRobotLaserQueue;
    int numTestOdom = Gm2dlIO::readRobotLaser(odomTestFilename, testRobotLaserQueue);
    if (numTestOdom == 0) {
      cerr << "Unable to read test data" << endl;
    } else {

      ofstream rawStream("odometry_raw.txt");
      ofstream calibratedStream("odometry_calibrated.txt");
      const Vector3d& odomCalib = odomParamsVertex->estimate();
      RobotLaser* prev = dynamic_cast<RobotLaser*>(testRobotLaserQueue.buffer().begin()->second);
      SE2 prevCalibratedPose = prev->odomPose();

      for (DataQueue::Buffer::const_iterator it = testRobotLaserQueue.buffer().begin(); it != testRobotLaserQueue.buffer().end(); ++it) {
        RobotLaser* cur = dynamic_cast<RobotLaser*>(it->second);
        assert(cur);

        double dt = cur->timestamp() - prev->timestamp();
        SE2 motion = prev->odomPose().inverse() * cur->odomPose();

        // convert to velocity measurment
        MotionMeasurement motionMeasurement(motion.translation().x(), motion.translation().y(), motion.rotation().angle(), dt);
        VelocityMeasurement velocityMeasurement = OdomConvert::convertToVelocity(motionMeasurement);

        // apply calibration
        VelocityMeasurement calibratedVelocityMeasurment = velocityMeasurement;
        calibratedVelocityMeasurment.setVl(odomCalib(0) * calibratedVelocityMeasurment.vl());
        calibratedVelocityMeasurment.setVr(odomCalib(1) * calibratedVelocityMeasurment.vr());
        MotionMeasurement mm = OdomConvert::convertToMotion(calibratedVelocityMeasurment, odomCalib(2));

        // combine calibrated odometry with the previous pose
        SE2 remappedOdom;
        remappedOdom.fromVector(mm.measurement());
        SE2 calOdomPose = prevCalibratedPose * remappedOdom;

        // write output
        rawStream << prev->odomPose().translation().x() << " " << prev->odomPose().translation().y() << " " << prev->odomPose().rotation().angle() << endl;
        calibratedStream << calOdomPose.translation().x() << " " << calOdomPose.translation().y() << " " << calOdomPose.rotation().angle() << endl;

        prevCalibratedPose = calOdomPose;
        prev = cur;
      }
    }

  }

  if (outputfilename.size() > 0) {
    Gm2dlIO::updateLaserData(optimizer);
    cerr << "Writing " << outputfilename << " ... ";
    bool writeStatus = Gm2dlIO::writeGm2dl(outputfilename, optimizer);
    cerr << (writeStatus ? "done." : "failed") << endl;
  }

  return 0;
}

//.........这里部分代码省略.........
      cerr << "Failure while updating the solver properties from the given string" << endl;
    }
  }
  if (solverProperties.size() > 0 || printSolverProperties) {
    optimizer.solver()->printProperties(cerr);
  }

  // Loading the input data
  if (loadLookup.size() > 0) {
    optimizer.setRenamedTypesFromString(loadLookup);
  }
  if (inputFilename.size() == 0) {
    cerr << "No input data specified" << endl;
    return 0;
  } else if (inputFilename == "-") {
    cerr << "Read input from stdin" << endl;
    if (!optimizer.load(cin)) {
      cerr << "Error loading graph" << endl;
      return 2;
    }
  } else {
    cerr << "Read input from " << inputFilename << endl;
    ifstream ifs(inputFilename.c_str());
    if (!ifs) {
      cerr << "Failed to open file" << endl;
      return 1;
    }
    if (!optimizer.load(ifs)) {
      cerr << "Error loading graph" << endl;
      return 2;
    }
  }
  cerr << "Loaded " << optimizer.vertices().size() << " vertices" << endl;
  cerr << "Loaded " << optimizer.edges().size() << " edges" << endl;

  if (optimizer.vertices().size() == 0) {
    cerr << "Graph contains no vertices" << endl;
    return 1;
  }

  set<int> vertexDimensions = optimizer.dimensions();
  if (! optimizer.isSolverSuitable(solverProperty, vertexDimensions)) {
    cerr << "The selected solver is not suitable for optimizing the given graph" << endl;
    return 3;
  }
  assert (optimizer.solver());
  //optimizer.setMethod(str2method(strMethod));
  //optimizer.setUserLambdaInit(lambdaInit);

 
  // check for vertices to fix to remove DoF
  bool gaugeFreedom = optimizer.gaugeFreedom();
  OptimizableGraph::Vertex* gauge=0;
  if (gaugeList.size()){
    cerr << "Fixing gauges: ";
    for (size_t i=0; i<gaugeList.size(); i++){
      int id=gaugeList[i];
      OptimizableGraph::Vertex* v=optimizer.vertex(id);
      if (!v){
        cerr << "fatal, not found the vertex of id " << id << " in the gaugeList. Aborting";
        return -1;
      } else {
        if (i==0)
          gauge = v;
        cerr << v->id() << " ";
        v->setFixed(1);

//.........这里部分代码省略.........
        numVo = viso.getMotion(stereovoQueue);
        cerr << "Done...There are " << numVo << " vo datas in the queue." << endl << endl;
    }
    else
    {
        cerr << "\033[31mLoading pose file...\033[0m" << endl;
        ss.str("");
        ss << rawFilename << "/CameraTrajectory.txt";
        in.open(ss.str().c_str());

        int numVo = 0;
        Matrix4D posemat;
        for(int i = 0; i < Num; i++)
        {
            for(int j = 0; j < 4; j++)
            {
                for(int k = 0; k < 4; k++)
                    in >> posemat(j, k);
            }
            if(first){
                init_offset.setTranslation(Eigen::Vector2d(init_x, init_y));
                init_offset.setRotation(Eigen::Rotation2Dd::Identity());
//                SE2 x(posemat(2,3),-posemat(0,3),acos(posemat(0,0)));
//                RobotOdom* ro = dynamic_cast<RobotOdom*>(odometryQueue.buffer().begin()->second);
                initialStereoPose = init_offset;
                first = false;
            }
            // save stereo vo as robotOdom node.
            RobotOdom* tempVo = new RobotOdom;
            tempVo->setTimestamp(timeque[i]);
            SE2 x(posemat(2,3),-posemat(0,3),acos(posemat(0,0)));
            tempVo->setOdomPose(init_offset*x);
            stereovoQueue.add(tempVo);
            numVo++;
        }
        in.close();
        cerr << "Done...There are " << numVo << " vo datas in the queue." << endl << endl;
    }

    // adding the measurements
    vector<MotionInformation, Eigen::aligned_allocator<MotionInformation> > motions;
    {
      std::map<double, RobotData*>::const_iterator it = stereovoQueue.buffer().begin();
      std::map<double, RobotData*>::const_iterator prevIt = it++;
      for (; it != stereovoQueue.buffer().end(); ++it) {
        MotionInformation mi;
        RobotOdom* prevVo = dynamic_cast<RobotOdom*>(prevIt->second);
        RobotOdom* curVo = dynamic_cast<RobotOdom*>(it->second);
        mi.stereoMotion = prevVo->odomPose().inverse() * curVo->odomPose();
        // get the motion of the robot in that time interval
        RobotOdom* prevOdom = dynamic_cast<RobotOdom*>(odometryQueue.findClosestData(prevVo->timestamp()));
        RobotOdom* curOdom = dynamic_cast<RobotOdom*>(odometryQueue.findClosestData(curVo->timestamp()));
        mi.odomMotion = prevOdom->odomPose().inverse() * curOdom->odomPose();
        mi.timeInterval = prevOdom->timestamp() - curOdom->timestamp();
        prevIt = it;
        motions.push_back(mi);
      }
    }

    // Construct the graph.
    cerr << "\033[31mConstruct the graph...\033[0m" << endl;
    // Vertex offset
    addVertexSE2(optimizer, initialStereoPose, 0);
    for(int i = 0; i < motions.size(); i++)
    {
        const SE2& odomMotion = motions[i].odomMotion;
        const SE2& stereoMotion = motions[i].stereoMotion;
        // add the edge
        // stereo vo and odom measurement.
        OdomAndStereoMotion  meas;
        meas.StereoMotion = stereoMotion;
        meas.odomMotion = odomMotion;
        addEdgeCalib(optimizer, 0, meas, Eigen::Matrix3d::Identity());
    }
    cerr << "Done..." << endl << endl;

    // if you want check the component of your graph, uncomment the CHECK_GRAPH
#ifdef CHECK_GRAPH
    for(auto it:optimizer.edges())
    {
        VertexSE2* v = static_cast<VertexSE2*>(it->vertex(0));
        cout << v->id() << endl;
    }
#endif

    // optimize.
    optimize(optimizer, 10);
    Eigen::Vector3d result = getEstimation(optimizer);
    cerr << "\033[1;32mCalibrated stereo offset (x, y, theta):\033[0m" << result[0] << " " << result [1] << " " << result[2] << endl << endl;

    // If you want see how's its closed form solution, uncomment the CLOSED_FORM
    if(use_cfs){
        cerr << "\033[31mPerforming the closed form solution...\033[0m" << endl;
        SE2 closedFormStereo;
        ClosedFormCalibration::calibrate(motions, closedFormStereo);
        cerr << "\033[1;32mDone... closed form Calibrated stereo offset (x, y, theta):\033[0m" << closedFormStereo.toVector().transpose() << endl;
    }

    return 0;
}