/ helper function
| 609 | /* ************************************************************************* */ |
| 610 | // helper function |
| 611 | GaussianFactorGraph::shared_ptr createOmegaBarTest() { |
| 612 | // create 3 stamped pose |
| 613 | Pose3 pose0 = Pose3(Rot3::Ypr(0.2, 0.4, 0.5), Point3(0, 0, 1)); |
| 614 | StampedPose spose0 = StampedPose(pose0, 0); |
| 615 | StampedPose spose1 = StampedPose( |
| 616 | pose0.compose(Pose3(Rot3::Ypr(0.02, 0.04, 0.05), Point3(0.2, 0, 0))), |
| 617 | 0.5); |
| 618 | |
| 619 | // create featureSelectionData |
| 620 | FeatureSelectorData featureSelectionData; |
| 621 | featureSelectionData.posesAtFutureKeyframes.push_back(spose0); |
| 622 | featureSelectionData.posesAtFutureKeyframes.push_back(spose1); |
| 623 | featureSelectionData.currentNavStateCovariance = |
| 624 | 0.0001 * Matrix::Identity(15, 15); |
| 625 | featureSelectionData.left_undistRectCameraMatrix = K; |
| 626 | featureSelectionData.right_undistRectCameraMatrix = K; |
| 627 | Camera cam(spose0.pose, K); |
| 628 | Point3 pworld_l = |
| 629 | cam.backproject(Point2(320, 200), 2.0); // backprojected 2 meters away |
| 630 | featureSelectionData.keypoints_3d.push_back( |
| 631 | pose0.transformTo(pworld_l)); // convert to local frame |
| 632 | featureSelectionData.keypointLife.push_back(3); |
| 633 | |
| 634 | // instantiate selector |
| 635 | VioBackEndParams vp = VioBackEndParams(); |
| 636 | vp.smartNoiseSigma_ = 1000; |
| 637 | FeatureSelector f(trackerParams, vp); |
| 638 | Cameras left_cameras, right_cameras; |
| 639 | tie(left_cameras, right_cameras) = f.getCameras(featureSelectionData); |
| 640 | |
| 641 | // create OmegaBar |
| 642 | return f.createOmegaBar(featureSelectionData, left_cameras, right_cameras); |
| 643 | } |
| 644 | /* ************************************************************************* */ |
| 645 | TEST(FeatureSelector, DISABLED_createOmegaBar) { |
| 646 | // get gaussian factor graph by calling createOmegaBar (done inside function |
no test coverage detected