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;