| 202 | } |
| 203 | |
| 204 | cv::Mat FrameDrawer::DrawRightFrame(float imageScale) |
| 205 | { |
| 206 | cv::Mat im; |
| 207 | vector<cv::KeyPoint> vIniKeys; // Initialization: KeyPoints in reference frame |
| 208 | vector<int> vMatches; // Initialization: correspondeces with reference keypoints |
| 209 | vector<cv::KeyPoint> vCurrentKeys; // KeyPoints in current frame |
| 210 | vector<bool> vbVO, vbMap; // Tracked MapPoints in current frame |
| 211 | int state; // Tracking state |
| 212 | |
| 213 | //Copy variables within scoped mutex |
| 214 | { |
| 215 | unique_lock<mutex> lock(mMutex); |
| 216 | state=mState; |
| 217 | if(mState==Tracking::SYSTEM_NOT_READY) |
| 218 | mState=Tracking::NO_IMAGES_YET; |
| 219 | |
| 220 | mImRight.copyTo(im); |
| 221 | |
| 222 | if(mState==Tracking::NOT_INITIALIZED) |
| 223 | { |
| 224 | vCurrentKeys = mvCurrentKeysRight; |
| 225 | vIniKeys = mvIniKeys; |
| 226 | vMatches = mvIniMatches; |
| 227 | } |
| 228 | else if(mState==Tracking::OK) |
| 229 | { |
| 230 | vCurrentKeys = mvCurrentKeysRight; |
| 231 | vbVO = mvbVO; |
| 232 | vbMap = mvbMap; |
| 233 | } |
| 234 | else if(mState==Tracking::LOST) |
| 235 | { |
| 236 | vCurrentKeys = mvCurrentKeysRight; |
| 237 | } |
| 238 | } // destroy scoped mutex -> release mutex |
| 239 | |
| 240 | if(imageScale != 1.f) |
| 241 | { |
| 242 | int imWidth = im.cols / imageScale; |
| 243 | int imHeight = im.rows / imageScale; |
| 244 | cv::resize(im, im, cv::Size(imWidth, imHeight)); |
| 245 | } |
| 246 | |
| 247 | if(im.channels()<3) //this should be always true |
| 248 | cvtColor(im,im,cv::COLOR_GRAY2BGR); |
| 249 | |
| 250 | //Draw |
| 251 | if(state==Tracking::NOT_INITIALIZED) //INITIALIZING |
| 252 | { |
| 253 | for(unsigned int i=0; i<vMatches.size(); i++) |
| 254 | { |
| 255 | if(vMatches[i]>=0) |
| 256 | { |
| 257 | cv::Point2f pt1,pt2; |
| 258 | if(imageScale != 1.f) |
| 259 | { |
| 260 | pt1 = vIniKeys[i].pt / imageScale; |
| 261 | pt2 = vCurrentKeys[vMatches[i]].pt / imageScale; |