| 51 | #include "glog/logging.h" |
| 52 | |
| 53 | TEST(okvisTestSuite, Map) { |
| 54 | // initialize random number generator |
| 55 | // srand((unsigned int) time(0)); // disabled: make unit tests deterministic... |
| 56 | |
| 57 | OKVIS_DEFINE_EXCEPTION(Exception, std::runtime_error); |
| 58 | |
| 59 | // set up a random geometry |
| 60 | std::cout << "set up a random geometry... " << std::flush; |
| 61 | okvis::kinematics::Transformation T_WS; // world to sensor |
| 62 | T_WS.setRandom(10.0, M_PI); |
| 63 | okvis::kinematics::Transformation T_disturb; |
| 64 | T_disturb.setRandom(1, 0.01); |
| 65 | okvis::kinematics::Transformation T_WS_init = T_WS * T_disturb; // world to sensor |
| 66 | okvis::kinematics::Transformation T_SC; // sensor to camera |
| 67 | T_SC.setRandom(0.2, M_PI); |
| 68 | std::shared_ptr<okvis::ceres::PoseParameterBlock> poseParameterBlock_ptr( |
| 69 | new okvis::ceres::PoseParameterBlock(T_WS_init, 1, okvis::Time(0))); |
| 70 | std::shared_ptr<okvis::ceres::PoseParameterBlock> extrinsicsParameterBlock_ptr( |
| 71 | new okvis::ceres::PoseParameterBlock(T_SC, 2, okvis::Time(0))); |
| 72 | |
| 73 | // use the custom graph/map datastructure now: |
| 74 | okvis::ceres::Map map; |
| 75 | map.addParameterBlock(poseParameterBlock_ptr, okvis::ceres::Map::Pose6d); |
| 76 | map.addParameterBlock(extrinsicsParameterBlock_ptr, okvis::ceres::Map::Pose6d); |
| 77 | map.setParameterBlockConstant(extrinsicsParameterBlock_ptr); // do not optimize this... |
| 78 | std::cout << " [ OK ] " << std::endl; |
| 79 | |
| 80 | // set up a random camera geometry |
| 81 | std::cout << "set up a random camera geometry... " << std::flush; |
| 82 | typedef okvis::cameras::PinholeCamera<okvis::cameras::EquidistantDistortion> DistortedPinholeCameraGeometry; |
| 83 | std::shared_ptr<const DistortedPinholeCameraGeometry> cameraGeometry = |
| 84 | std::static_pointer_cast<const DistortedPinholeCameraGeometry>( |
| 85 | DistortedPinholeCameraGeometry::createTestObject()); |
| 86 | std::cout << " [ OK ] " << std::endl; |
| 87 | |
| 88 | // get some random points and build error terms |
| 89 | const size_t N = 1000; |
| 90 | |
| 91 | std::cout << "create N=" << N << " visible points and add respective reprojection error terms... " << std::flush; |
| 92 | ::ceres::CauchyLoss loss(1); |
| 93 | for (size_t i = 0; i < N; ++i) { |
| 94 | Eigen::Vector4d point = cameraGeometry->createRandomVisibleHomogeneousPoint(static_cast<double>(i % 10) * 3 + 2.0); |
| 95 | std::shared_ptr<okvis::ceres::HomogeneousPointParameterBlock> homogeneousPointParameterBlock_ptr( |
| 96 | new okvis::ceres::HomogeneousPointParameterBlock(T_WS * T_SC * point, i + 3)); |
| 97 | map.addParameterBlock(homogeneousPointParameterBlock_ptr, okvis::ceres::Map::HomogeneousPoint); |
| 98 | map.setParameterBlockConstant(homogeneousPointParameterBlock_ptr); // no point optimization |
| 99 | |
| 100 | // get a randomized projection |
| 101 | Eigen::Vector2d kp; |
| 102 | cameraGeometry->projectHomogeneous(point, &kp); |
| 103 | kp += Eigen::Vector2d::Random(); |
| 104 | |
| 105 | // Set up the only cost function (also known as residual). |
| 106 | Eigen::Matrix2d information = Eigen::Matrix2d::Identity(); |
| 107 | std::shared_ptr<::ceres::CostFunction> cost_function( |
| 108 | new okvis::ceres::ReprojectionError<DistortedPinholeCameraGeometry>(cameraGeometry, 1, kp, information)); |
| 109 | |
| 110 | ::ceres::ResidualBlockId id = map.addResidualBlock( |
nothing calls this directly
no test coverage detected