diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 0ad2da7866b87cf2c1117fd6d573f9e8ebd04d5f..b51db6529108ac757397a0a6cef6f0abeaaf0ab3 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -20,6 +20,10 @@ target_link_libraries(gtest_pinhole ${PLUGIN_NAME} ${wolf_LIBRARY}) wolf_add_gtest(gtest_sensor_camera gtest_sensor_camera.cpp) target_link_libraries(gtest_sensor_camera ${PLUGIN_NAME} ${wolf_LIBRARY} ${OpenCV_LIBS} ${vision_utils_LIBRARY}) +# FactorAutodiffTrifocal test +wolf_add_gtest(gtest_factor_autodiff_trifocal gtest_factor_autodiff_trifocal.cpp) +target_link_libraries(gtest_factor_autodiff_trifocal ${PLUGIN_NAME} ${wolf_LIBRARY} ${OpenCV_LIBS} ${vision_utils_LIBRARY}) + # ProcessorFeatureTrifocal test wolf_add_gtest(gtest_processor_tracker_feature_trifocal gtest_processor_tracker_feature_trifocal.cpp) target_link_libraries(gtest_processor_tracker_feature_trifocal ${PLUGIN_NAME} ${wolf_LIBRARY} ${OpenCV_LIBS} ${vision_utils_LIBRARY}) diff --git a/test/gtest_factor_autodiff_trifocal.cpp b/test/gtest_factor_autodiff_trifocal.cpp new file mode 100644 index 0000000000000000000000000000000000000000..31a9c211fff44d5777a386e45744e9a2536ddfff --- /dev/null +++ b/test/gtest_factor_autodiff_trifocal.cpp @@ -0,0 +1,986 @@ +#include "utils_gtest.h" + +#include "core/utils/logging.h" + +#include "core/ceres_wrapper/ceres_manager.h" +#include "vision/processor/processor_tracker_feature_trifocal.h" +#include "vision/capture/capture_image.h" +#include "vision/factor/factor_autodiff_trifocal.h" +#include "vision/internal/config.h" + +using namespace Eigen; +using namespace wolf; + +class FactorAutodiffTrifocalTest : public testing::Test{ + public: + Vector3s pos1, pos2, pos3, pos_cam, point; + Vector3s euler1, euler2, euler3, euler_cam; + Quaternions quat1, quat2, quat3, quat_cam; + Vector4s vquat1, vquat2, vquat3, vquat_cam; // quaternions as vectors + Vector7s pose1, pose2, pose3, pose_cam; + + ProblemPtr problem; + CeresManagerPtr ceres_manager; + + SensorCameraPtr camera; + ProcessorTrackerFeatureTrifocalPtr proc_trifocal; + + SensorBasePtr S; + FrameBasePtr F1, F2, F3; + CaptureImagePtr I1, I2, I3; + FeatureBasePtr f1, f2, f3; + FactorAutodiffTrifocalPtr c123; + + Scalar pixel_noise_std; + + virtual ~FactorAutodiffTrifocalTest() + { + std::cout << "destructor\n"; + } + + virtual void SetUp() override + { + std::string wolf_root = _WOLF_VISION_ROOT_DIR; + + // configuration + /* + * We have three robot poses, in three frames, with cameras C1, C2, C3 + * looking towards the origin of coordinates: + * + * Z + * | + * ________ C3 + * / | / + * --------- /| C2 + * | / | + * |____________/_ | ___ Y + * / | / + * / | / + * --------- C1 |/ + * | / | + * --------- + * / + * Y + * + * Each robot pose is at one axis, facing the origin: + * F1: pos = (1,0,0), ori = (0,0,pi) + * F2: pos = (0,1,0), ori = (0,0,-pi/2) + * F3: pos = (0,0,1), ori = (0,pi/2,pi) + * + * The robot has a camera looking forward + * S: pos = (0,0,0), ori = (-pi/1, 0, -pi/1) + * + * There is a point at the origin + * P: pos = (0,0,0) + * + * The camera is canonical + * K = Id. + * + * Therefore, P projects exactly at the origin on each camera, + * creating three features: + * f1: p1 = (0,0) + * f2: p2 = (0,0) + * f3: p3 = (0,0) + * + * We form a Wolf tree with three frames, three captures, + * three features, and one trifocal factor: + * + * Frame F1, Capture C1, feature f1 + * Frame F2, Capture C2, feature f2 + * Frame F3, Capture C3, feature f3, factor c123 + * + * The three frame poses F1, F2, F3 and the camera pose S + * in the robot frame are variables subject to optimization + * + * We perform a number of tests based on this configuration. + */ + + // all frames look to the origin + pos1 << 1, 0, 0; + pos2 << 0, 1, 0; + pos3 << 0, 0, 1; + euler1 << 0, 0 , M_PI ; + euler2 << 0, 0 , -M_PI_2 ; + euler3 << 0, M_PI_2, M_PI ; + quat1 = e2q(euler1); + quat2 = e2q(euler2); + quat3 = e2q(euler3); + vquat1 = quat1.coeffs(); + vquat2 = quat2.coeffs(); + vquat3 = quat3.coeffs(); + pose1 << pos1, vquat1; + pose2 << pos2, vquat2; + pose3 << pos3, vquat3; + + // camera at the robot origin looking forward + pos_cam << 0, 0, 0; + euler_cam << -M_PI_2, 0, -M_PI_2;// euler_cam *= M_TORAD; + quat_cam = e2q(euler_cam); + vquat_cam = quat_cam.coeffs(); + pose_cam << pos_cam, vquat_cam; + + // Build problem + problem = Problem::create("PO", 3); + ceres::Solver::Options options; + ceres_manager = std::make_shared<CeresManager>(problem, options); + + // Install sensor and processor + S = problem->installSensor("CAMERA", "canonical", pose_cam, wolf_root + "/demos/camera_params_canonical.yaml"); + camera = std::static_pointer_cast<SensorCamera>(S); + + ProcessorParamsTrackerFeatureTrifocalPtr params_tracker_feature_trifocal_trifocal = std::make_shared<ProcessorParamsTrackerFeatureTrifocal>(); + params_tracker_feature_trifocal_trifocal->time_tolerance = 1.0/2; + params_tracker_feature_trifocal_trifocal->max_new_features = 5; + params_tracker_feature_trifocal_trifocal->min_features_for_keyframe = 5; + params_tracker_feature_trifocal_trifocal->yaml_file_params_vision_utils = wolf_root + "/demos/processor_tracker_feature_trifocal_vision_utils.yaml"; + + ProcessorBasePtr proc = problem->installProcessor("TRACKER FEATURE TRIFOCAL", "trifocal", camera, params_tracker_feature_trifocal_trifocal); + proc_trifocal = std::static_pointer_cast<ProcessorTrackerFeatureTrifocal>(proc); + + // Add three viewpoints with frame, capture and feature + pixel_noise_std = 2.0; + Vector2s pix(0,0); + Matrix2s pix_cov(Matrix2s::Identity() * pow(pixel_noise_std, 2)); + + F1 = problem->emplaceFrame(KEY, pose1, 1.0); + I1 = std::static_pointer_cast<CaptureImage>(CaptureBase::emplace<CaptureImage>(F1, 1.0, camera, cv::Mat(2,2,CV_8UC1))); + f1 = FeatureBase::emplace<FeatureBase>(I1, "PIXEL", pix, pix_cov); // pixel at origin + + F2 = problem->emplaceFrame(KEY, pose2, 2.0); + I2 = std::static_pointer_cast<CaptureImage>((CaptureBase::emplace<CaptureImage>(F2, 2.0, camera, cv::Mat(2,2,CV_8UC1)))); + f2 = FeatureBase::emplace<FeatureBase>(I2, "PIXEL", pix, pix_cov); // pixel at origin + + F3 = problem->emplaceFrame(KEY, pose3, 3.0); + I3 = std::static_pointer_cast<CaptureImage>(CaptureBase::emplace<CaptureImage>(F3, 3.0, camera, cv::Mat(2,2,CV_8UC1))); + f3 = FeatureBase::emplace<FeatureBase>(I3, "PIXEL", pix, pix_cov); // pixel at origin + + // trifocal factor + // c123 = std::make_shared<FactorAutodiffTrifocal>(f1, f2, f3, proc_trifocal, false, FAC_ACTIVE); + c123 = std::static_pointer_cast<FactorAutodiffTrifocal>(FactorBase::emplace<FactorAutodiffTrifocal>(f3, f1, f2, f3, proc_trifocal, false, FAC_ACTIVE)); + // f3 ->addFactor (c123); + // f1 ->addConstrainedBy(c123); + // f2 ->addConstrainedBy(c123); + } +}; + +TEST_F(FactorAutodiffTrifocalTest, InfoMatrix) +{ + /** Ground truth covariance. Rationale: + * Due to the orthogonal configuration (see line 40 and onwards), we have: + * Let s = pixel_noise_std. + * Let d = 1 the distance from the cameras to the 3D point + * Let k be a proportionality constant related to the projection and pixellization process + * Let S = k*d*s + * The pixel on camera 1 retroprojects a conic PDF with width S = k*s*d + * The line on camera 2 retroprojects a plane aperture of S = k*s*d + * The product (ie intersection) of cone and plane aperture PDFs is a sphere of radius S + * Projection of the sphere to camera 3 is a circle of S/k/d=s pixels + * This is the projected covariance: s^2 pixel^2 + * The measurement has a std dev of s pixel --> cov is s^2 pixel^2 + * The total cov is s^2 pix^2 + s^2 pix^2 = 2s^2 pix^2 + * The info matrix is 0.5 s^-2 pix^-2 + * The sqrt info matrix is 1/s/sqrt(2) pix^-1 + */ + Matrix3s sqrt_info_gt = Matrix3s::Identity() / pixel_noise_std / sqrt(2.0); + + ASSERT_MATRIX_APPROX(c123->getSqrtInformationUpper(), sqrt_info_gt, 1e-8); + +} + +TEST_F(FactorAutodiffTrifocalTest, expectation) +{ + // Homogeneous transform C2 wrt C1 + Matrix4s _c1Hc2; _c1Hc2 << + 0, 0, -1, 1, + 0, 1, 0, 0, + 1, 0, 0, 1, + 0, 0, 0, 1; + + // rotation and translation + Matrix3s _c1Rc2 = _c1Hc2.block(0,0,3,3); + Vector3s _c1Tc2 = _c1Hc2.block(0,3,3,1); + + // Essential matrix, ground truth (fwd and bkwd) + Matrix3s _c1Ec2 = wolf::skew(_c1Tc2) * _c1Rc2; + Matrix3s _c2Ec1 = _c1Ec2.transpose(); + + // Expected values + vision_utils::TrifocalTensor tensor; + Matrix3s c2Ec1; + c123->expectation(pos1, quat1, pos2, quat2, pos3, quat3, pos_cam, quat_cam, tensor, c2Ec1); + + // check trilinearities + + // Elements computed using the tensor + Matrix3s T0, T1, T2; + tensor.getLayers(T0,T1,T2); + Vector3s _m1 (0,0,1), + _m2 (0,0,1), + _m3 (0,0,1); // ground truth + Matrix3s m1Tt = _m1(0)*T0.transpose() + + _m1(1)*T1.transpose() + + _m1(2)*T2.transpose(); + + // Projective line: l = (nx ny dn), with (nx ny): normal vector; dn: distance to origin times norm(nx,ny) + Vector3s _l2(0,1,0), + _p2(1,0,0), + _p3(0,1,0); // ground truth + Vector3s l2; + l2 = c2Ec1 * _m1; + + // check epipolar lines (check only director vectors for equal direction) + ASSERT_MATRIX_APPROX(l2/l2(1), _l2/_l2(1), 1e-8); + + // check perpendicular lines (check only director vectors for orthogonal direction) + ASSERT_NEAR(l2(0)*_p2(0) + l2(1)*_p2(1), 0, 1e-8); + + // Verify trilinearities + + // Point-line-line + Matrix1s pll = _p3.transpose() * m1Tt * _p2; + ASSERT_TRUE(pll(0)<1e-5); + + // Point-line-point + Vector3s plp = wolf::skew(_m3) * m1Tt * _p2; + ASSERT_MATRIX_APPROX(plp, Vector3s::Zero(), 1e-8); + + // Point-point-line + Vector3s ppl = _p3.transpose() * m1Tt * wolf::skew(_m2); + ASSERT_MATRIX_APPROX(ppl, Vector3s::Zero(), 1e-8); + + // Point-point-point + Matrix3s ppp = wolf::skew(_m3) * m1Tt * wolf::skew(_m2); + ASSERT_MATRIX_APPROX(ppp, Matrix3s::Zero(), 1e-8); + + // check epipolars + ASSERT_MATRIX_APPROX(c2Ec1/c2Ec1(0,1), _c2Ec1/_c2Ec1(0,1), 1e-8); +} + +TEST_F(FactorAutodiffTrifocalTest, residual) +{ + vision_utils::TrifocalTensor tensor; + Matrix3s c2Ec1; + Vector3s residual; + + // Nominal values + c123->expectation(pos1, quat1, pos2, quat2, pos3, quat3, pos_cam, quat_cam, tensor, c2Ec1); + residual = c123->residual(tensor, c2Ec1); + + ASSERT_MATRIX_APPROX(residual, Vector3s::Zero(), 1e-8); +} + +TEST_F(FactorAutodiffTrifocalTest, error_jacobians) +{ + vision_utils::TrifocalTensor tensor; + Matrix3s c2Ec1; + Vector3s residual, residual_pert; + Vector3s pix0, pert, pix_pert; + Scalar epsilon = 1e-8; + + // Nominal values + c123->expectation(pos1, quat1, pos2, quat2, pos3, quat3, pos_cam, quat_cam, tensor, c2Ec1); + residual = c123->residual(tensor, c2Ec1); + + Matrix<Scalar, 3, 3> J_e_m1, J_e_m2, J_e_m3, J_r_m1, J_r_m2, J_r_m3; + c123->error_jacobians(tensor, c2Ec1, J_e_m1, J_e_m2, J_e_m3); + J_r_m1 = c123->getSqrtInformationUpper() * J_e_m1; + J_r_m2 = c123->getSqrtInformationUpper() * J_e_m2; + J_r_m3 = c123->getSqrtInformationUpper() * J_e_m3; + + // numerical jacs + Matrix<Scalar,3,3> Jn_r_m1, Jn_r_m2, Jn_r_m3; + + // jacs wrt m1 + pix0 = c123->getPixelCanonical1(); + for (int i=0; i<3; i++) + { + pert.setZero(); + pert(i) = epsilon; + pix_pert = pix0 + pert; + c123->setPixelCanonical1(pix_pert); // m1 + residual_pert = c123->residual(tensor, c2Ec1); + + Jn_r_m1.col(i) = (residual_pert - residual) / epsilon; + } + c123->setPixelCanonical1(pix0); + + ASSERT_MATRIX_APPROX(J_r_m1, Jn_r_m1, 1e-6); + + // jacs wrt m2 + pix0 = c123->getPixelCanonical2(); + for (int i=0; i<3; i++) + { + pert.setZero(); + pert(i) = epsilon; + pix_pert = pix0 + pert; + c123->setPixelCanonical2(pix_pert); // m2 + residual_pert = c123->residual(tensor, c2Ec1); + + Jn_r_m2.col(i) = (residual_pert - residual) / epsilon; + } + c123->setPixelCanonical2(pix0); + + ASSERT_MATRIX_APPROX(J_r_m2, Jn_r_m2, 1e-6); + + // jacs wrt m3 + pix0 = c123->getPixelCanonical3(); + for (int i=0; i<3; i++) + { + pert.setZero(); + pert(i) = epsilon; + pix_pert = pix0 + pert; + c123->setPixelCanonical3(pix_pert); // m3 + residual_pert = c123->residual(tensor, c2Ec1); + + Jn_r_m3.col(i) = (residual_pert - residual) / epsilon; + } + c123->setPixelCanonical3(pix0); + + ASSERT_MATRIX_APPROX(J_r_m3, Jn_r_m3, 1e-6); + +} + +TEST_F(FactorAutodiffTrifocalTest, operator_parenthesis) +{ + Vector3s res; + + // Factor with exact states should give zero residual + c123->operator ()(pos1.data(), vquat1.data(), + pos2.data(), vquat2.data(), + pos3.data(), vquat3.data(), + pos_cam.data(), vquat_cam.data(), + res.data()); + + ASSERT_MATRIX_APPROX(res, Vector3s::Zero(), 1e-8); +} + +TEST_F(FactorAutodiffTrifocalTest, solve_F1) +{ + F1->setState(pose1); + F2->setState(pose2); + F3->setState(pose3); + S ->getP()->setState(pos_cam); + S ->getO()->setState(vquat_cam); + // Residual with prior + + Vector3s res; + + Eigen::VectorXs F1_p = F1->getP()->getState(); + Eigen::VectorXs F1_o = F1->getO()->getState(); + Eigen::VectorXs F2_p = F2->getP()->getState(); + Eigen::VectorXs F2_o = F2->getO()->getState(); + Eigen::VectorXs F3_p = F3->getP()->getState(); + Eigen::VectorXs F3_o = F3->getO()->getState(); + Eigen::VectorXs S_p = S ->getP()->getState(); + Eigen::VectorXs S_o = S ->getO()->getState(); + + c123->operator ()(F1_p.data(), F1_o.data(), + F2_p.data(), F2_o.data(), + F3_p.data(), F3_o.data(), + S_p. data(), S_o. data(), + res.data()); + + WOLF_DEBUG("Initial state: ", F1->getState().transpose()); + WOLF_DEBUG("residual before perturbing: ", res.transpose()); + ASSERT_MATRIX_APPROX(res, Vector3s::Zero(), 1e-8); + + // Residual with perturbated state + + Vector7s pose_perturbated = F1->getState() + 0.1 * Vector7s::Random(); + pose_perturbated.segment(3,4).normalize(); + F1->setState(pose_perturbated); + + F1_p = F1->getP()->getState(); + F1_o = F1->getO()->getState(); + + c123->operator ()(F1_p.data(), F1_o.data(), + F2_p.data(), F2_o.data(), + F3_p.data(), F3_o.data(), + S_p. data(), S_o. data(), + res.data()); + + WOLF_DEBUG("perturbed state: ", pose_perturbated.transpose()); + WOLF_DEBUG("residual before solve: ", res.transpose()); + + // Residual with solved state + + S ->fixExtrinsics(); + F1->unfix(); + F2->fix(); + F3->fix(); + + std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF); + + F1_p = F1->getP()->getState(); + F1_o = F1->getO()->getState(); + F2_p = F2->getP()->getState(); + F2_o = F2->getO()->getState(); + F3_p = F3->getP()->getState(); + F3_o = F3->getO()->getState(); + S_p = S ->getP()->getState(); + S_o = S ->getO()->getState(); + + c123->operator ()(F1_p.data(), F1_o.data(), + F2_p.data(), F2_o.data(), + F3_p.data(), F3_o.data(), + S_p. data(), S_o. data(), + res.data()); + + WOLF_DEBUG("solved state: ", F1->getState().transpose()); + WOLF_DEBUG("residual after solve: ", res.transpose()); + + WOLF_DEBUG(report, " AND UNION"); + + ASSERT_MATRIX_APPROX(res, Vector3s::Zero(), 1e-8); + +} + +TEST_F(FactorAutodiffTrifocalTest, solve_F2) +{ + F1->setState(pose1); + F2->setState(pose2); + F3->setState(pose3); + S ->getP()->setState(pos_cam); + S ->getO()->setState(vquat_cam); + + // Residual with prior + + Vector3s res; + + Eigen::VectorXs F1_p = F1->getP()->getState(); + Eigen::VectorXs F1_o = F1->getO()->getState(); + Eigen::VectorXs F2_p = F2->getP()->getState(); + Eigen::VectorXs F2_o = F2->getO()->getState(); + Eigen::VectorXs F3_p = F3->getP()->getState(); + Eigen::VectorXs F3_o = F3->getO()->getState(); + Eigen::VectorXs S_p = S ->getP()->getState(); + Eigen::VectorXs S_o = S ->getO()->getState(); + + c123->operator ()(F1_p.data(), F1_o.data(), + F2_p.data(), F2_o.data(), + F3_p.data(), F3_o.data(), + S_p. data(), S_o. data(), + res.data()); + + WOLF_DEBUG("Initial state: ", F2->getState().transpose()); + WOLF_DEBUG("residual before perturbing: ", res.transpose()); + ASSERT_MATRIX_APPROX(res, Vector3s::Zero(), 1e-8); + + // Residual with perturbated state + + Vector7s pose_perturbated = F2->getState() + 0.1 * Vector7s::Random(); + pose_perturbated.segment(3,4).normalize(); + F2->setState(pose_perturbated); + + F2_p = F2->getP()->getState(); + F2_o = F2->getO()->getState(); + + c123->operator ()(F1_p.data(), F1_o.data(), + F2_p.data(), F2_o.data(), + F3_p.data(), F3_o.data(), + S_p. data(), S_o. data(), + res.data()); + + WOLF_DEBUG("perturbed state: ", pose_perturbated.transpose()); + WOLF_DEBUG("residual before solve: ", res.transpose()); + + // Residual with solved state + + S ->fixExtrinsics(); + F1->fix(); + F2->unfix(); + F3->fix(); + + std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF); + + F1_p = F1->getP()->getState(); + F1_o = F1->getO()->getState(); + F2_p = F2->getP()->getState(); + F2_o = F2->getO()->getState(); + F3_p = F3->getP()->getState(); + F3_o = F3->getO()->getState(); + S_p = S ->getP()->getState(); + S_o = S ->getO()->getState(); + + c123->operator ()(F1_p.data(), F1_o.data(), + F2_p.data(), F2_o.data(), + F3_p.data(), F3_o.data(), + S_p. data(), S_o. data(), + res.data()); + + WOLF_DEBUG("solved state: ", F2->getState().transpose()); + WOLF_DEBUG("residual after solve: ", res.transpose()); + + WOLF_DEBUG(report, " AND UNION"); + + ASSERT_MATRIX_APPROX(res, Vector3s::Zero(), 1e-8); + +} + +TEST_F(FactorAutodiffTrifocalTest, solve_F3) +{ + F1->setState(pose1); + F2->setState(pose2); + F3->setState(pose3); + S ->getP()->setState(pos_cam); + S ->getO()->setState(vquat_cam); + + // Residual with prior + + Vector3s res; + + Eigen::VectorXs F1_p = F1->getP()->getState(); + Eigen::VectorXs F1_o = F1->getO()->getState(); + Eigen::VectorXs F2_p = F2->getP()->getState(); + Eigen::VectorXs F2_o = F2->getO()->getState(); + Eigen::VectorXs F3_p = F3->getP()->getState(); + Eigen::VectorXs F3_o = F3->getO()->getState(); + Eigen::VectorXs S_p = S ->getP()->getState(); + Eigen::VectorXs S_o = S ->getO()->getState(); + + c123->operator ()(F1_p.data(), F1_o.data(), + F2_p.data(), F2_o.data(), + F3_p.data(), F3_o.data(), + S_p. data(), S_o. data(), + res.data()); + + WOLF_DEBUG("Initial state: ", F3->getState().transpose()); + WOLF_DEBUG("residual before perturbing: ", res.transpose()); + ASSERT_MATRIX_APPROX(res, Vector3s::Zero(), 1e-8); + + // Residual with perturbated state + + Vector7s pose_perturbated = F3->getState() + 0.1 * Vector7s::Random(); + pose_perturbated.segment(3,4).normalize(); + F3->setState(pose_perturbated); + + F3_p = F3->getP()->getState(); + F3_o = F3->getO()->getState(); + + c123->operator ()(F1_p.data(), F1_o.data(), + F2_p.data(), F2_o.data(), + F3_p.data(), F3_o.data(), + S_p. data(), S_o. data(), + res.data()); + + WOLF_DEBUG("perturbed state: ", pose_perturbated.transpose()); + WOLF_DEBUG("residual before solve: ", res.transpose()); + ASSERT_NEAR(res(2), 0, 1e-8); // Epipolar c2-c1 should be respected when perturbing F3 + + // Residual with solved state + + S ->fixExtrinsics(); + F1->fix(); + F2->fix(); + F3->unfix(); + + std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF); + + F1_p = F1->getP()->getState(); + F1_o = F1->getO()->getState(); + F2_p = F2->getP()->getState(); + F2_o = F2->getO()->getState(); + F3_p = F3->getP()->getState(); + F3_o = F3->getO()->getState(); + S_p = S ->getP()->getState(); + S_o = S ->getO()->getState(); + + c123->operator ()(F1_p.data(), F1_o.data(), + F2_p.data(), F2_o.data(), + F3_p.data(), F3_o.data(), + S_p. data(), S_o. data(), + res.data()); + + WOLF_DEBUG("solved state: ", F3->getState().transpose()); + WOLF_DEBUG("residual after solve: ", res.transpose()); + + WOLF_DEBUG(report, " AND UNION"); + + ASSERT_MATRIX_APPROX(res, Vector3s::Zero(), 1e-8); + +} + +TEST_F(FactorAutodiffTrifocalTest, solve_S) +{ + F1->setState(pose1); + F2->setState(pose2); + F3->setState(pose3); + S ->getP()->setState(pos_cam); + S ->getO()->setState(vquat_cam); + + // Residual with prior + + Vector3s res; + + Eigen::VectorXs F1_p = F1->getP()->getState(); + Eigen::VectorXs F1_o = F1->getO()->getState(); + Eigen::VectorXs F2_p = F2->getP()->getState(); + Eigen::VectorXs F2_o = F2->getO()->getState(); + Eigen::VectorXs F3_p = F3->getP()->getState(); + Eigen::VectorXs F3_o = F3->getO()->getState(); + Eigen::VectorXs S_p = S ->getP()->getState(); + Eigen::VectorXs S_o = S ->getO()->getState(); + + c123->operator ()(F1_p.data(), F1_o.data(), + F2_p.data(), F2_o.data(), + F3_p.data(), F3_o.data(), + S_p. data(), S_o. data(), + res.data()); + + WOLF_DEBUG("Initial state: ", S->getP()->getState().transpose(), " ", S->getO()->getState().transpose()); + WOLF_DEBUG("residual before perturbing: ", res.transpose()); + ASSERT_MATRIX_APPROX(res, Vector3s::Zero(), 1e-8); + + // Residual with perturbated state + + Vector3s pos_perturbated = pos_cam + 0.1 * Vector3s::Random(); + Vector4s ori_perturbated = vquat_cam + 0.1 * Vector4s::Random(); + ori_perturbated.normalize(); + Vector7s pose_perturbated; pose_perturbated << pos_perturbated, ori_perturbated; + S->getP()->setState(pos_perturbated); + S->getO()->setState(ori_perturbated); + + S_p = S->getP()->getState(); + S_o = S->getO()->getState(); + + c123->operator ()(F1_p.data(), F1_o.data(), + F2_p.data(), F2_o.data(), + F3_p.data(), F3_o.data(), + S_p. data(), S_o. data(), + res.data()); + + WOLF_DEBUG("perturbed state: ", pose_perturbated.transpose()); + WOLF_DEBUG("residual before solve: ", res.transpose()); + + // Residual with solved state + + S ->unfixExtrinsics(); + F1->fix(); + F2->fix(); + F3->fix(); + + std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF); + + F1_p = F1->getP()->getState(); + F1_o = F1->getO()->getState(); + F2_p = F2->getP()->getState(); + F2_o = F2->getO()->getState(); + F3_p = F3->getP()->getState(); + F3_o = F3->getO()->getState(); + S_p = S ->getP()->getState(); + S_o = S ->getO()->getState(); + + c123->operator ()(F1_p.data(), F1_o.data(), + F2_p.data(), F2_o.data(), + F3_p.data(), F3_o.data(), + S_p. data(), S_o. data(), + res.data()); + + WOLF_DEBUG("solved state: ", S->getP()->getState().transpose(), " ", S->getO()->getState().transpose()); + WOLF_DEBUG("residual after solve: ", res.transpose()); + + WOLF_DEBUG(report, " AND UNION"); + + ASSERT_MATRIX_APPROX(res, Vector3s::Zero(), 1e-8); + +} + +class FactorAutodiffTrifocalMultiPointTest : public FactorAutodiffTrifocalTest +{ + /* + * In this test class we add 8 more points and perform optimization on the camera frames. + * + * C1 is not optimized as it acts as the reference + * S is not optimized either as observability is compromised + * C2.pos is fixed to set the unobservable scale + * C2.ori and all C3 are optimized + * + */ + + public: + std::vector<FeatureBasePtr> fv1, fv2, fv3; + std::vector<FactorAutodiffTrifocalPtr> cv123; + + virtual void SetUp() override + { + FactorAutodiffTrifocalTest::SetUp(); + + Matrix<Scalar, 2, 9> c1p_can; + c1p_can << + 0, -1/3.00, -1/3.00, 1/3.00, 1/3.00, -1.0000, -1.0000, 1.0000, 1.0000, + 0, 1/3.00, -1/3.00, 1/3.00, -1/3.00, 1.0000, -1.0000, 1.0000, -1.0000; + + Matrix<Scalar, 2, 9> c2p_can; + c2p_can << + 0, 1/3.00, 1/3.00, 1.0000, 1.0000, -1/3.00, -1/3.00, -1.0000, -1.0000, + 0, 1/3.00, -1/3.00, 1.0000, -1.0000, 1/3.00, -1/3.00, 1.0000, -1.0000; + + Matrix<Scalar, 2, 9> c3p_can; + c3p_can << + 0, -1/3.00, -1.0000, 1/3.00, 1.0000, -1/3.00, -1.0000, 1/3.00, 1.0000, + 0, -1/3.00, -1.0000, -1/3.00, -1.0000, 1/3.00, 1.0000, 1/3.00, 1.0000; + + Matrix2s pix_cov; pix_cov.setIdentity(); //pix_cov *= 1e-4; + + // for i==0 we already have them + fv1.push_back(f1); + fv2.push_back(f2); + fv3.push_back(f3); + cv123.push_back(c123); + for (size_t i=1; i<c1p_can.cols() ; i++) + { + // fv1.push_back(std::make_shared<FeatureBase>("PIXEL", c1p_can.col(i), pix_cov)); + auto f = FeatureBase::emplace<FeatureBase>(I1, "PIXEL", c1p_can.col(i), pix_cov); + fv1.push_back(f); + // I1->addFeature(fv1.at(i)); + + // fv2.push_back(std::make_shared<FeatureBase>("PIXEL", c2p_can.col(i), pix_cov)); + auto f2 = FeatureBase::emplace<FeatureBase>(I2, "PIXEL", c2p_can.col(i), pix_cov); + fv2.push_back(f2); + // I2->addFeature(fv2.at(i)); + + // fv3.push_back(std::make_shared<FeatureBase>("PIXEL", c3p_can.col(i), pix_cov)); + auto f3 = FeatureBase::emplace<FeatureBase>(I3, "PIXEL", c3p_can.col(i), pix_cov); + fv3.push_back(f3); + // I3->addFeature(fv3.at(i)); + + auto ff = std::static_pointer_cast<FactorAutodiffTrifocal>(FactorBase::emplace<FactorAutodiffTrifocal>(fv3.at(i), fv1.at(i), fv2.at(i), fv3.at(i), proc_trifocal, false, FAC_ACTIVE)); + cv123.push_back(ff); + // fv3.at(i)->addFactor(cv123.at(i)); + // fv1.at(i)->addConstrainedBy(cv123.at(i)); + // fv2.at(i)->addConstrainedBy(cv123.at(i)); + } + + } + +}; + +TEST_F(FactorAutodiffTrifocalMultiPointTest, solve_multi_point) +{ + /* + * In this test we add 8 more points and perform optimization on the camera frames. + * + * C1 is not optimized as it acts as the reference + * S is not optimized either as observability is compromised + * C2.pos is fixed to set the unobservable scale + * C2.ori and all C3 are optimized + * + */ + + S ->getP()->fix(); // do not calibrate sensor pos + S ->getO()->fix(); // do not calibrate sensor ori + F1->getP()->fix(); // Cam 1 acts as reference + F1->getO()->fix(); // Cam 1 acts as reference + F2->getP()->fix(); // This fixes the scale + F2->getO()->unfix(); // Estimate Cam 2 ori + F3->getP()->unfix(); // Estimate Cam 3 pos + F3->getO()->unfix(); // Estimate Cam 3 ori + + // Perturbate states, keep scale + F1->getP()->setState( pos1 ); + F1->getO()->setState( vquat1 ); + F2->getP()->setState( pos2 ); // this fixes the scale + F2->getO()->setState((vquat2 + 0.2*Vector4s::Random()).normalized()); + F3->getP()->setState( pos3 + 0.2*Vector3s::Random()); + F3->getO()->setState((vquat3 + 0.2*Vector4s::Random()).normalized()); + + std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF); + + // Print results + WOLF_DEBUG("report: ", report); + problem->print(1,0,1,0); + + // Evaluate final states + ASSERT_MATRIX_APPROX(F2->getP()->getState(), pos2 , 1e-10); + ASSERT_MATRIX_APPROX(F2->getO()->getState(), vquat2, 1e-10); + ASSERT_MATRIX_APPROX(F3->getP()->getState(), pos3 , 1e-10); + ASSERT_MATRIX_APPROX(F3->getO()->getState(), vquat3, 1e-10); + + Eigen::VectorXs F1_p = F1->getP()->getState(); + Eigen::VectorXs F1_o = F1->getO()->getState(); + Eigen::VectorXs F2_p = F2->getP()->getState(); + Eigen::VectorXs F2_o = F2->getO()->getState(); + Eigen::VectorXs F3_p = F3->getP()->getState(); + Eigen::VectorXs F3_o = F3->getO()->getState(); + Eigen::VectorXs S_p = S ->getP()->getState(); + Eigen::VectorXs S_o = S ->getO()->getState(); + + // evaluate residuals + Vector3s res; + for (size_t i=0; i<cv123.size(); i++) + { + cv123.at(i)->operator ()(F1_p.data(), F1_o.data(), + F2_p.data(), F2_o.data(), + F3_p.data(), F3_o.data(), + S_p. data(), S_o. data(), + res.data()); + + ASSERT_MATRIX_APPROX(res, Vector3s::Zero(), 1e-10); + } + +} + +TEST_F(FactorAutodiffTrifocalMultiPointTest, solve_multi_point_scale) +{ + /* + * In this test we add 8 more points and perform optimization on the camera frames. + * + * C1 is not optimized as it acts as the reference + * S is not optimized either as observability is compromised + * C2.pos is fixed to set the unobservable scale + * C2.ori and all C3 are optimized + * + */ + + S ->getP()->fix(); // do not calibrate sensor pos + S ->getO()->fix(); // do not calibrate sensor ori + F1->getP()->fix(); // Cam 1 acts as reference + F1->getO()->fix(); // Cam 1 acts as reference + F2->getP()->fix(); // This fixes the scale + F2->getO()->unfix(); // Estimate Cam 2 ori + F3->getP()->unfix(); // Estimate Cam 3 pos + F3->getO()->unfix(); // Estimate Cam 3 ori + + // Perturbate states, change scale + F1->getP()->setState( 2 * pos1 ); + F1->getO()->setState( vquat1 ); + F2->getP()->setState( 2 * pos2 ); + F2->getO()->setState(( vquat2 + 0.2*Vector4s::Random()).normalized()); + F3->getP()->setState( 2 * pos3 + 0.2*Vector3s::Random()); + F3->getO()->setState(( vquat3 + 0.2*Vector4s::Random()).normalized()); + + std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF); + + // Print results + WOLF_DEBUG("report: ", report); + problem->print(1,0,1,0); + + // Evaluate final states + ASSERT_MATRIX_APPROX(F2->getP()->getState(), 2 * pos2, 1e-8); + ASSERT_MATRIX_APPROX(F2->getO()->getState(), vquat2, 1e-8); + ASSERT_MATRIX_APPROX(F3->getP()->getState(), 2 * pos3, 1e-8); + ASSERT_MATRIX_APPROX(F3->getO()->getState(), vquat3, 1e-8); + + Eigen::VectorXs F1_p = F1->getP()->getState(); + Eigen::VectorXs F1_o = F1->getO()->getState(); + Eigen::VectorXs F2_p = F2->getP()->getState(); + Eigen::VectorXs F2_o = F2->getO()->getState(); + Eigen::VectorXs F3_p = F3->getP()->getState(); + Eigen::VectorXs F3_o = F3->getO()->getState(); + Eigen::VectorXs S_p = S ->getP()->getState(); + Eigen::VectorXs S_o = S ->getO()->getState(); + + // evaluate residuals + Vector3s res; + for (size_t i=0; i<cv123.size(); i++) + { + cv123.at(i)->operator ()(F1_p.data(), F1_o.data(), + F2_p.data(), F2_o.data(), + F3_p.data(), F3_o.data(), + S_p. data(), S_o. data(), + res.data()); + + ASSERT_MATRIX_APPROX(res, Vector3s::Zero(), 1e-8); + } +} + +#include "core/factor/factor_autodiff_distance_3D.h" + +TEST_F(FactorAutodiffTrifocalMultiPointTest, solve_multi_point_distance) +{ + /* + * In this test we add 8 more points and perform optimization on the camera frames. + * + * C1 is not optimized as it acts as the reference + * S is not optimized either as observability is compromised + * C2 and C3 are optimized + * The scale is observed through a distance factor + * + */ + + S ->getP()->fix(); // do not calibrate sensor pos + S ->getO()->fix(); // do not calibrate sensor ori + F1->getP()->fix(); // Cam 1 acts as reference + F1->getO()->fix(); // Cam 1 acts as reference + F2->getP()->unfix(); // Estimate Cam 2 pos + F2->getO()->unfix(); // Estimate Cam 2 ori + F3->getP()->unfix(); // Estimate Cam 3 pos + F3->getO()->unfix(); // Estimate Cam 3 ori + + // Perturbate states, change scale + F1->getP()->setState( pos1 ); + F1->getO()->setState( vquat1 ); + F2->getP()->setState( pos2 + 0.2*Vector3s::Random() ); + F2->getO()->setState((vquat2 + 0.2*Vector4s::Random()).normalized()); + F3->getP()->setState( pos3 + 0.2*Vector3s::Random()); + F3->getO()->setState((vquat3 + 0.2*Vector4s::Random()).normalized()); + + // Add a distance factor to fix the scale + Scalar distance = sqrt(2.0); + Scalar distance_std = 0.1; + + auto Cd = CaptureBase::emplace<CaptureBase>(F3, "DISTANCE", F3->getTimeStamp()); + // CaptureBasePtr Cd = std::make_shared<CaptureBase>("DISTANCE", F3->getTimeStamp()); + // F3->addCapture(Cd); + auto fd = FeatureBase::emplace<FeatureBase>(Cd, "DISTANCE", Vector1s(distance), Matrix1s(distance_std * distance_std)); + // FeatureBasePtr fd = std::make_shared<FeatureBase>("DISTANCE", Vector1s(distance), Matrix1s(distance_std * distance_std)); + // Cd->addFeature(fd); + auto cd = FactorBase::emplace<FactorAutodiffDistance3D>(fd, fd, F1, nullptr, false, FAC_ACTIVE); + // FACTORAUTODIFFDISTANCE3DPTR cd = std::make_shared<FactorAutodiffDistance3D>(fd, F1, nullptr, false, FAC_ACTIVE); + // fd->addFactor(cd); + // F1->addConstrainedBy(cd); + + cd->setStatus(FAC_INACTIVE); + std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF); + WOLF_DEBUG("DISTANCE CONSTRAINT INACTIVE: \n", report); + + problem->print(1,0,1,0); + + cd->setStatus(FAC_ACTIVE); + report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF); + + // Print results + WOLF_DEBUG("DISTANCE CONSTRAINT ACTIVE: \n", report); + problem->print(1,0,1,0); + + // Evaluate final states + ASSERT_MATRIX_APPROX(F2->getP()->getState(), pos2 , 1e-8); + ASSERT_MATRIX_APPROX(F2->getO()->getState(), vquat2, 1e-8); + ASSERT_MATRIX_APPROX(F3->getP()->getState(), pos3 , 1e-8); + ASSERT_MATRIX_APPROX(F3->getO()->getState(), vquat3, 1e-8); + + Eigen::VectorXs F1_p = F1->getP()->getState(); + Eigen::VectorXs F1_o = F1->getO()->getState(); + Eigen::VectorXs F2_p = F2->getP()->getState(); + Eigen::VectorXs F2_o = F2->getO()->getState(); + Eigen::VectorXs F3_p = F3->getP()->getState(); + Eigen::VectorXs F3_o = F3->getO()->getState(); + Eigen::VectorXs S_p = S ->getP()->getState(); + Eigen::VectorXs S_o = S ->getO()->getState(); + + // evaluate residuals + Vector3s res; + for (size_t i=0; i<cv123.size(); i++) + { + cv123.at(i)->operator ()(F1_p.data(), F1_o.data(), + F2_p.data(), F2_o.data(), + F3_p.data(), F3_o.data(), + S_p. data(), S_o. data(), + res.data()); + + ASSERT_MATRIX_APPROX(res, Vector3s::Zero(), 1e-8); + } +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + // ::testing::GTEST_FLAG(filter) = "FactorAutodiffTrifocalTest.solve_F1"; + // ::testing::GTEST_FLAG(filter) = "FactorAutodiffTrifocalTest.solve_F2"; + // ::testing::GTEST_FLAG(filter) = "FactorAutodiffTrifocalTest.solve_F3"; + // ::testing::GTEST_FLAG(filter) = "FactorAutodiffTrifocalTest.solve_S"; + // ::testing::GTEST_FLAG(filter) = "FactorAutodiffTrifocalTest.solve_multi_point"; + // ::testing::GTEST_FLAG(filter) = "FactorAutodiffTrifocalMultiPointTest.solve_multi_point_distance"; + return RUN_ALL_TESTS(); +} +