diff --git a/src/test/gtest_constraint_autodiff_trifocal.cpp b/src/test/gtest_constraint_autodiff_trifocal.cpp
index 76c53c106e353f6cd0cee63b7de461d015f8b325..f4c8bc89905b7263105ab75e7197f3989dd00096 100644
--- a/src/test/gtest_constraint_autodiff_trifocal.cpp
+++ b/src/test/gtest_constraint_autodiff_trifocal.cpp
@@ -1,28 +1,24 @@
#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;