Status KeyframeVisualOdometry::Update(const Image& image) { // Extract keypoints from the frame. std::vector<Keypoint> keypoints; Status status = GetKeypoints(image, &keypoints); if (!status.ok()) return status; // Extract features and descriptors from the keypoints. std::vector<Feature> features; std::vector<Descriptor> descriptors; status = GetDescriptors(image, &keypoints, &features, &descriptors); if (!status.ok()) return status; // Set the annotator's image. if (options_.draw_tracks || options_.draw_features) { annotator_.SetImage(image); } // Initialize the very first view if we don't have one yet. if (current_keyframe_ == kInvalidView) { Landmark::SetRequiredObservations(2); InitializeFirstView(features, descriptors); return Status::Ok(); } // Initialize the second view if we don't have one yet using 2D<-->2D // matching. if (view_indices_.size() == 1) { status = InitializeSecondView(features, descriptors); if (status.ok()) { Landmark::SetRequiredObservations(options_.num_observations_to_triangulate); } return status; } // Create a camera with an unknown pose. Camera camera; camera.SetIntrinsics(intrinsics_); View::Ptr new_view = View::Create(camera); // Is this new image going to be a keyframe? const bool is_keyframe = initialize_new_keyframe_ || NumEstimatedTracks() < options_.min_num_feature_tracks; if (is_keyframe) { initialize_new_keyframe_ = false; } // Update feature tracks and add matched features to the view. status = UpdateFeatureTracks(features, descriptors, new_view->Index(), is_keyframe); if (!status.ok()) { View::DeleteMostRecentView(); return status; } // Compute the new camera pose. status = EstimatePose(new_view->Index()); if (!status.ok()) { View::DeleteMostRecentView(); } else { view_indices_.push_back(new_view->Index()); } if (is_keyframe && options_.perform_bundle_adjustment) { // Bundle adjust views in the sliding window. BundleAdjuster bundle_adjuster; if (!bundle_adjuster.Solve(options_.bundle_adjustment_options, SlidingWindowViewIndices())) return Status::Cancelled("Failed to perform bundle adjustment."); } // Annotate tracks and features. if (options_.draw_features) { annotator_.AnnotateFeatures(features); } if (options_.draw_tracks) { annotator_.AnnotateTracks(tracks_); } if (options_.draw_tracks || options_.draw_features) { annotator_.Draw(); } return status; }
void CvOneWayDescriptor::EstimatePosePCA(CvArr* patch, int& pose_idx, float& distance, CvMat* avg, CvMat* eigenvectors) const { if(avg == 0) { // do not use pca if (!CV_IS_MAT(patch)) { EstimatePose((IplImage*)patch, pose_idx, distance); } else { } return; } CvRect roi; if (!CV_IS_MAT(patch)) { roi = cvGetImageROI((IplImage*)patch); if(roi.width != GetPatchSize().width || roi.height != GetPatchSize().height) { cvResize(patch, m_input_patch); patch = m_input_patch; roi = cvGetImageROI((IplImage*)patch); } } CvMat* pca_coeffs = cvCreateMat(1, m_pca_dim_low, CV_32FC1); if (CV_IS_MAT(patch)) { cvCopy((CvMat*)patch, pca_coeffs); } else { IplImage* patch_32f = cvCreateImage(cvSize(roi.width, roi.height), IPL_DEPTH_32F, 1); float sum = cvSum(patch).val[0]; cvConvertScale(patch, patch_32f, 1.0f/sum); ProjectPCASample(patch_32f, avg, eigenvectors, pca_coeffs); cvReleaseImage(&patch_32f); } distance = 1e10; pose_idx = -1; for(int i = 0; i < m_pose_count; i++) { float dist = cvNorm(m_pca_coeffs[i], pca_coeffs); // float dist = 0; // float data1, data2; // //CvMat* pose_pca_coeffs = m_pca_coeffs[i]; // for (int x=0; x < pca_coeffs->width; x++) // for (int y =0 ; y < pca_coeffs->height; y++) // { // data1 = ((float*)(pca_coeffs->data.ptr + pca_coeffs->step*x))[y]; // data2 = ((float*)(m_pca_coeffs[i]->data.ptr + m_pca_coeffs[i]->step*x))[y]; // dist+=(data1-data2)*(data1-data2); // } ////#if 1 // for (int j = 0; j < m_pca_dim_low; j++) // { // dist += (pose_pca_coeffs->data.fl[j]- pca_coeffs->data.fl[j])*(pose_pca_coeffs->data.fl[j]- pca_coeffs->data.fl[j]); // } //#else // for (int j = 0; j <= m_pca_dim_low - 4; j += 4) // { // dist += (pose_pca_coeffs->data.fl[j]- pca_coeffs->data.fl[j])* // (pose_pca_coeffs->data.fl[j]- pca_coeffs->data.fl[j]); // dist += (pose_pca_coeffs->data.fl[j+1]- pca_coeffs->data.fl[j+1])* // (pose_pca_coeffs->data.fl[j+1]- pca_coeffs->data.fl[j+1]); // dist += (pose_pca_coeffs->data.fl[j+2]- pca_coeffs->data.fl[j+2])* // (pose_pca_coeffs->data.fl[j+2]- pca_coeffs->data.fl[j+2]); // dist += (pose_pca_coeffs->data.fl[j+3]- pca_coeffs->data.fl[j+3])* // (pose_pca_coeffs->data.fl[j+3]- pca_coeffs->data.fl[j+3]); // } //#endif if(dist < distance) { distance = dist; pose_idx = i; } } cvReleaseMat(&pca_coeffs); }