Skip to content
Snippets Groups Projects
Commit 3c51b388 authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Cleanup

parent 8858ee5c
No related branches found
No related tags found
1 merge request!200Vision devel
Hide whitespace changes
Inline Side-by-side
src/test/gtest_constraint_autodiff_trifocal.cpp
+ 52
− 50
View file @ 3c51b388
#include "utils_gtest.h"
#include "wolf.h"
#include "logging.h"
#include "constraints/constraint_autodiff_trifocal.h"
#include "capture_image.h"
#include "processors/processor_tracker_feature_trifocal.h"
#include "ceres_wrapper/ceres_manager.h"
#include "processors/processor_tracker_feature_trifocal.h"
#include "capture_image.h"
#include "constraints/constraint_autodiff_trifocal.h"
using namespace Eigen;
using namespace wolf;
using std::static_pointer_cast;
using namespace Eigen;
class ConstraintAutodiffTrifocalTest : 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;
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;
ProblemPtr problem;
CeresManagerPtr ceres_manager;
SensorCameraPtr camera;
......@@ -95,25 +91,25 @@ class ConstraintAutodiffTrifocalTest : public testing::Test{
pos1 << 1, 0, 0;
pos2 << 0, 1, 0;
pos3 << 0, 0, 1;
euler1 << 0, 0, M_PI; //euler1 *= M_TORAD;
euler2 << 0, 0, -M_PI_2; //euler2 *= M_TORAD;
euler3 << 0, M_PI_2, M_PI ;// euler3 *= M_TORAD;
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();
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;
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;
quat_cam = e2q(euler_cam);
vquat_cam = quat_cam.coeffs();
pose_cam << pos_cam, vquat_cam;
// Build problem
problem = Problem::create("PO 3D");
......@@ -128,13 +124,13 @@ class ConstraintAutodiffTrifocalTest : public testing::Test{
ceres_manager = std::make_shared<CeresManager>(problem, options);
// Install sensor and processor
S = problem->installSensor("CAMERA", "canonical", pose_cam, wolf_root + "/src/examples/camera_params_canonical.yaml");
S = problem->installSensor("CAMERA", "canonical", pose_cam, wolf_root + "/src/examples/camera_params_canonical.yaml");
camera = std::static_pointer_cast<SensorCamera>(S);
ProcessorParamsTrackerFeatureTrifocalPtr params_trifocal = std::make_shared<ProcessorParamsTrackerFeatureTrifocal>();
params_trifocal->time_tolerance = 1.0/2;
params_trifocal->max_new_features = 5;
params_trifocal->min_features_for_keyframe = 5;
params_trifocal->time_tolerance = 1.0/2;
params_trifocal->max_new_features = 5;
params_trifocal->min_features_for_keyframe = 5;
params_trifocal->yaml_file_params_vision_utils = wolf_root + "/src/examples/vision_utils_active_search.yaml";
proc_trifocal = std::make_shared<ProcessorTrackerFeatureTrifocal>(*params_trifocal);
......@@ -146,27 +142,27 @@ class ConstraintAutodiffTrifocalTest : public testing::Test{
F1 = problem->emplaceFrame(KEY_FRAME, pose1, 1.0);
I1 = std::make_shared<CaptureImage>(1.0, camera, cv::Mat(2,2,CV_8UC1));
F1->addCapture(I1);
F1-> addCapture(I1);
f1 = std::make_shared<FeatureBase>("PIXEL", pix, pix_cov); // pixel at origin
I1->addFeature(f1);
I1-> addFeature(f1);
F2 = problem->emplaceFrame(KEY_FRAME, pose2, 2.0);
I2 = std::make_shared<CaptureImage>(2.0, camera, cv::Mat(2,2,CV_8UC1));
F2->addCapture(I2);
F2-> addCapture(I2);
f2 = std::make_shared<FeatureBase>("PIXEL", pix, pix_cov); // pixel at origin
I2->addFeature(f2);
I2-> addFeature(f2);
F3 = problem->emplaceFrame(KEY_FRAME, pose3, 3.0);
I3 = std::make_shared<CaptureImage>(3.0, camera, cv::Mat(2,2,CV_8UC1));
F3->addCapture(I3);
F3-> addCapture(I3);
f3 = std::make_shared<FeatureBase>("PIXEL", pix, pix_cov); // pixel at origin
I3->addFeature(f3);
I3-> addFeature(f3);
// trifocal constraint
c123 = std::make_shared<ConstraintAutodiffTrifocal>(f1, f2, f3, proc_trifocal, false, CTR_ACTIVE);
f3->addConstraint(c123);
f1->addConstrainedBy(c123);
f2->addConstrainedBy(c123);
f3 ->addConstraint (c123);
f1 ->addConstrainedBy(c123);
f2 ->addConstrainedBy(c123);
}
};
......@@ -195,13 +191,19 @@ TEST_F(ConstraintAutodiffTrifocalTest, expectation)
// check trilinearities
// Elements computed using the tensor
Eigen::Matrix3s T0, T1, T2;
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();
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(0,1,0),
_p2(1,0,0),
_p3(0,1,0); // ground truth
Vector3s l2;
l2 = c2Ec1 * _m1;
......@@ -214,19 +216,19 @@ TEST_F(ConstraintAutodiffTrifocalTest, expectation)
// Verify trilinearities
// Point-line-line
Eigen::Matrix1s pll = _p3.transpose() * m1Tt * _p2;
Matrix1s pll = _p3.transpose() * m1Tt * _p2;
ASSERT_TRUE(pll(0)<1e-5);
// Point-line-point
Eigen::Vector3s plp = wolf::skew(_m3) * m1Tt * _p2;
Vector3s plp = wolf::skew(_m3) * m1Tt * _p2;
ASSERT_MATRIX_APPROX(plp, Vector3s::Zero(), 1e-8);
// Point-point-line
Eigen::Vector3s ppl = _p3.transpose() * m1Tt * wolf::skew(_m2);
Vector3s ppl = _p3.transpose() * m1Tt * wolf::skew(_m2);
ASSERT_MATRIX_APPROX(ppl, Vector3s::Zero(), 1e-8);
// Point-point-point
Eigen::Matrix3s ppp = wolf::skew(_m3) * m1Tt * wolf::skew(_m2);
Matrix3s ppp = wolf::skew(_m3) * m1Tt * wolf::skew(_m2);
ASSERT_MATRIX_APPROX(ppp, Matrix3s::Zero(), 1e-8);
// check epipolars
......@@ -260,9 +262,9 @@ TEST_F(ConstraintAutodiffTrifocalTest, error_jacobians)
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;
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;
......@@ -601,17 +603,17 @@ class ConstraintAutodiffTrifocalMultiPointTest : public ConstraintAutodiffTrifoc
{
ConstraintAutodiffTrifocalTest::SetUp();
Eigen::Matrix<Scalar, 2, 9> c1p_can;
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;
Eigen::Matrix<Scalar, 2, 9> c2p_can;
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;
Eigen::Matrix<Scalar, 2, 9> c3p_can;
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;
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment