标签：TE match lan conf ram timestamp cond dde ror
在okvis_app_sychronous.cpp内，把IMU和图像数据加入到各自的队列里。由ThreadedKFVio负责队列的各种操作。作者对队列加了特殊功能，保证队列是线程安全的。比如：在push时，当超过最大设定值，可以选择是阻塞还是丢掉最老的数据。在pop时也有互斥锁。
/// \brief Push to the queue if the size is less than max_queue_size, else block.
/// \param[in] value New entry in queue.
/// \param[in] max_queue_size Maximum queue size.
/// \return False if shutdown is requested.
bool PushBlockingIfFull(const QueueType& value, size_t max_queue_size) {
while (!shutdown_) {
pthread_mutex_lock(&mutex_);
size_t size = queue_.size();
if (size >= max_queue_size) {
pthread_cond_wait(&condition_full_, &mutex_);
}
if (size >= max_queue_size) {
pthread_mutex_unlock(&mutex_);
continue;
}
queue_.push(value);
pthread_cond_signal(&condition_empty_); // Signal that data is available.
pthread_mutex_unlock(&mutex_);
return true;
}
return false;
}
/// \brief Push to the queue. If full, drop the oldest entry.
/// \param[in] value New entry in queue.
/// \param[in] max_queue_size Maximum queue size.
/// \return True if oldest was dropped because queue was full.
bool PushNonBlockingDroppingIfFull(const QueueType& value,
size_t max_queue_size) {
pthread_mutex_lock(&mutex_);
bool result = false;
if (queue_.size() >= max_queue_size) {
queue_.pop();
result = true;
}
queue_.push(value);
pthread_cond_signal(&condition_empty_); // Signal that data is available.
pthread_mutex_unlock(&mutex_);
return result;
}
/**
* @brief Get the oldest entry still in the queue. Blocking if queue is empty.
* @param[out] value Oldest entry in queue.
* @return False if shutdown is requested.
*/
bool Pop(QueueType* value) {
return PopBlocking(value);
}
此线程主要是进行IMU积分，获得最新的没有优化过的位姿，以及速度，偏置等信息。每当位姿优化过后，会使repropagationNeeded置为真，则在优化后的参数上进行积分处理。
if (parameters_.publishing.publishImuPropagatedState) {
if (!repropagationNeeded_ && imuMeasurements_.size() > 0) {
start = imuMeasurements_.back().timeStamp;
} else if (repropagationNeeded_) {
std::lock_guard<std::mutex> lastStateLock(lastState_mutex_);
start = lastOptimizedStateTimestamp_;
T_WS_propagated_ = lastOptimized_T_WS_;
speedAndBiases_propagated_ = lastOptimizedSpeedAndBiases_;
repropagationNeeded_ = false;
} else
start = okvis::Time(0, 0);
end = &data.timeStamp;
}
imuMeasurements_.push_back(data);
} // unlock _imuMeasurements_mutex
std::cout<<"IMU loop"<<data.timeStamp<<std::endl;
// notify other threads that imu data with timeStamp is here.
imuFrameSynchronizer_.gotImuData(data.timeStamp);
if (parameters_.publishing.publishImuPropagatedState) {
Eigen::Matrix<double, 15, 15> covariance;
Eigen::Matrix<double, 15, 15> jacobian;
frontend_.propagation(imuMeasurements_, imu_params_, T_WS_propagated_,
speedAndBiases_propagated_, start, *end, &covariance,
&jacobian);
OptimizationResults result;
result.stamp = *end;
result.T_WS = T_WS_propagated_;
result.speedAndBiases = speedAndBiases_propagated_;
result.omega_S = imuMeasurements_.back().measurement.gyroscopes
- speedAndBiases_propagated_.segment<3>(3);
for (size_t i = 0; i < parameters_.nCameraSystem.numCameras(); ++i) {
result.vector_of_T_SCi.push_back(
okvis::kinematics::Transformation(
*parameters_.nCameraSystem.T_SC(i)));
}
result.onlyPublishLandmarks = false;
optimizationResults_.PushNonBlockingDroppingIfFull(result,1);
}每一个相机都会有一个线程处理图像，所以这里需要把两个相机的图像融合到一个数据结构里，也就是multiFrame。
multiFrame = frameSynchronizer_.addNewFrame(frame);此函数内部，不做检测，仅仅是融合左右两个图像到一个multiFrame里。
如果是第一帧图像对，则不进行预积分。这里预积分是为了获得TWS，然后的出相机位姿，因为对特征点进行描述时，需要方向信息。但是目前左右帧都会进行预积分，然后在后端优化时候，estimator类里也会进行预积分。不明白对于同一对图像为什么不能只进行一次预积分。
frontend_.detectAndDescribe(frame->sensorId, multiFrame, T_WC, nullptr);这里没有用什么特殊的方法，都是opencv内的特征提取方法。
for (size_t i = 0; i < numCameras_; ++i) {
featureDetectors_.push_back(
std::shared_ptr<cv::FeatureDetector>(
#ifdef __ARM_NEON__
new cv::GridAdaptedFeatureDetector(
new cv::FastFeatureDetector(briskDetectionThreshold_),
briskDetectionMaximumKeypoints_, 7, 4 ))); // from config file, except the 7x4...
#else
new brisk::ScaleSpaceFeatureDetector<brisk::HarrisScoreCalculator>(
briskDetectionThreshold_, briskDetectionOctaves_,
briskDetectionAbsoluteThreshold_,
briskDetectionMaximumKeypoints_)));
#endif
descriptorExtractors_.push_back(
std::shared_ptr<cv::DescriptorExtractor>(
new brisk::BriskDescriptorExtractor(
briskDescriptionRotationInvariance_,
briskDescriptionScaleInvariance_)));
}在判断左右两幅图像都检测完后，则把数据传给匹配线程。匹配过程略复杂，而且代码使用了很多模板，比较难看懂。在进行图像匹配前，作者先把状态参数，传递给后端，添加各种误差项。这里不太明白为什么要在匹配前进行。
if (estimator_.addStates(frame, imuData, asKeyframe))
{
lastAddedStateTimestamp_ = frame->timestamp();
addStateTimer.stop();
} else {
LOG(ERROR) << "Failed to add state! will drop multiframe.";
addStateTimer.stop();
continue;
}
// -- matching keypoints, initialising landmarks etc.
okvis::kinematics::Transformation T_WS;
estimator_.get_T_WS(frame->id(), T_WS);
matchingTimer.start();
frontend_.dataAssociationAndInitialization(estimator_, T_WS, parameters_, map_, frame, &asKeyframe);
matchingTimer.stop();然后先让current frame 和以前所有的关键帧进行匹配。然后再和lastFrame进行匹配，最后进行左右立体匹配。每种匹配后都会在setBestMatch函数内增加每个点的投影误差。matchingLoop有点像前端和后端的桥梁，在这里准备后端优化所需要的数据。
标签：TE match lan conf ram timestamp cond dde ror
原文地址：https://www.cnblogs.com/easonslam/p/9172644.html
