lsd:optimizationThreadLoop线程

  while(keepRunning)
  {
    boost::unique_lock<boost::mutex> lock(newConstraintMutex);
    if(!newConstraintAdded)

      //等待新的一致性newConstraintCreatedSignal.timed_wait(lock,boost::posix_time::milliseconds(2000));// slight chance of deadlock otherwise
    newConstraintAdded = false;
    lock.unlock();

    //跑数据集跑完后，做个最终优化
    if(doFinalOptimization)
    {
      printf("doing final optimization iteration!\n");
      optimizationIteration(50, 0.001);
      doFinalOptimization = false;
    }
    //循环优化
    while(optimizationIteration(5, 0.02));
  }

进入optimizationIteration函数：

  // 添加节点和边
  keyFrameGraph->addElementsFromBuffer();
  newConstraintMutex.unlock();

  // Do the optimization. This can take quite some time!优化5次
  int its = keyFrameGraph->optimize(itsPerTry);

  // save the optimization result.
  poseConsistencyMutex.lock_shared();
  keyFrameGraph->keyframesAllMutex.lock_shared();
  float maxChange = 0;
  float sumChange = 0;
  float sum = 0;
  for(size_t i=0;i<keyFrameGraph->keyframesAll.size(); i++)
  {
    // set edge error sum to zero
    keyFrameGraph->keyframesAll[i]->edgeErrorSum = 0;
    keyFrameGraph->keyframesAll[i]->edgesNum = 0;

    if(!keyFrameGraph->keyframesAll[i]->pose->isInGraph) continue;

    // get change from last optimization
    Sim3 a = keyFrameGraph->keyframesAll[i]->pose->graphVertex->estimate();
    Sim3 b = keyFrameGraph->keyframesAll[i]->getScaledCamToWorld();
    Sophus::Vector7f diff = (a*b.inverse()).log().cast<float>();

    for(int j=0;j<7;j++)
    {
      float d = fabsf((float)(diff[j]));
      if(d > maxChange) maxChange = d;
      sumChange += d;
    }
    sum +=7;

    // set change
    keyFrameGraph->keyframesAll[i]->pose->setPoseGraphOptResult(
            keyFrameGraph->keyframesAll[i]->pose->graphVertex->estimate());

    // add error
    for(auto edge : keyFrameGraph->keyframesAll[i]->pose->graphVertex->edges())
    {
      keyFrameGraph->keyframesAll[i]->edgeErrorSum += ((EdgeSim3*)(edge))->chi2();
      keyFrameGraph->keyframesAll[i]->edgesNum++;
    }
  }

  haveUnmergedOptimizationOffset = true;