12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

finding best way to test RS errors

release/4.3a0
lcarlone 2021-07-21 14:46:52 -04:00
parent 306393a18c
commit 6f8d639ab8
1 changed files with 90 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

134
gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp
View File

@ -56,6 +56,20 @@ static double interp_factor1 = 0.3;
static double interp_factor2 = 0.4; static double interp_factor2 = 0.4;
static double interp_factor3 = 0.5; static double interp_factor3 = 0.5;
/* ************************************************************************* */
// default Cal3_S2 poses with rolling shutter effect
namespace vanillaPoseRS {
typedef PinholePose<Cal3_S2> Camera;
typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h));
Pose3 interp_pose1 = interpolate<Pose3>(level_pose,pose_right,interp_factor1);
Pose3 interp_pose2 = interpolate<Pose3>(pose_right,pose_above,interp_factor2);
Pose3 interp_pose3 = interpolate<Pose3>(pose_above,level_pose,interp_factor3);
Camera cam1(interp_pose1, sharedK);
Camera cam2(interp_pose2, sharedK);
Camera cam3(interp_pose3, sharedK);
}
LevenbergMarquardtParams lmParams; LevenbergMarquardtParams lmParams;
typedef SmartProjectionPoseFactorRollingShutter<Cal3_S2> SmartFactorRS; typedef SmartProjectionPoseFactorRollingShutter<Cal3_S2> SmartFactorRS;
@ -154,63 +168,95 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) {
} }
} }
/* ************************************************************************* /* *************************************************************************/
TEST( SmartProjectionPoseFactorRollingShutter, noiseless ) { TEST( SmartProjectionPoseFactorRollingShutter, noiselessError ) {
using namespace vanillaPose; using namespace vanillaPoseRS;
// Project two landmarks into two cameras // // 2 poses such that level_pose_1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
Point2 level_uv = level_camera.project(landmark1); // // can be interpolated with interp_factor1 = 0.2:
Point2 level_uv_right = level_camera_right.project(landmark1); // Pose3 level_pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 5));
// Pose3 level_pose2 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0));
// // 2 poses such that pose_right (Second camera 1 meter to the right of first camera)
// // can be interpolated with interp_factor1 = 0.4:
// Pose3 pose_right1 = level_pose * Pose3(Rot3(), Point3(1, 0, 0));
// Pose3 pose_right2 = level_pose * Pose3(Rot3(), Point3(1, 0, 0));
// // 2 poses such that pose_above (Third camera 1 meter above the first camera)
// // can be interpolated with interp_factor1 = 0.5:
// Pose3 pose_above1 = level_pose * Pose3(Rot3(), Point3(0, -1, 0));
// Pose3 pose_above1 = level_pose * Pose3(Rot3(), Point3(0, -1, 0));
//
// // Project two landmarks into two cameras
// Point2 level_uv = level_camera.project(landmark1);
// Point2 level_uv_right = level_camera_right.project(landmark1);
// Pose3 body_P_sensorId = Pose3::identity();
//
// SmartFactor factor(model);
// factor.add(level_uv, x1, x2, interp_factor1, sharedK, body_P_sensorId);
// factor.add(level_uv_right, x2, x3, interp_factor2, sharedK, body_P_sensorId);
//
// Values values; // it's a pose factor, hence these are poses
// values.insert(x1, cam1.pose());
// values.insert(x2, cam2.pose());
//
// double actualError = factor.error(values);
// double expectedError = 0.0;
// EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7);
}
SmartFactor factor(model, sharedK); /* *************************************************************************
factor.add(level_uv, x1); TEST( SmartProjectionPoseFactorRollingShutter, Jacobians ) {
factor.add(level_uv_right, x2);
Values values; // it's a pose factor, hence these are poses using namespace vanillaPose;
values.insert(x1, cam1.pose());
values.insert(x2, cam2.pose());
double actualError = factor.error(values); // Project two landmarks into two cameras
double expectedError = 0.0; Point2 level_uv = level_camera.project(landmark1);
EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); Point2 level_uv_right = level_camera_right.project(landmark1);
SmartFactor::Cameras cameras = factor.cameras(values); SmartFactor factor(model, sharedK);
double actualError2 = factor.totalReprojectionError(cameras); factor.add(level_uv, x1);
EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7); factor.add(level_uv_right, x2);
// Calculate expected derivative for point (easiest to check) Values values; // it's a pose factor, hence these are poses
std::function<Vector(Point3)> f = // values.insert(x1, cam1.pose());
std::bind(&SmartFactor::whitenedError<Point3>, factor, cameras, std::placeholders::_1); values.insert(x2, cam2.pose());
// Calculate using computeEP double actualError = factor.error(values);
Matrix actualE; double expectedError = 0.0;
factor.triangulateAndComputeE(actualE, values); EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7);
// get point // Calculate expected derivative for point (easiest to check)
boost::optional<Point3> point = factor.point(); std::function<Vector(Point3)> f = //
CHECK(point); std::bind(&SmartFactor::whitenedError<Point3>, factor, cameras, std::placeholders::_1);
// calculate numerical derivative with triangulated point // Calculate using computeEP
Matrix expectedE = sigma * numericalDerivative11<Vector, Point3>(f, *point); Matrix actualE;
EXPECT(assert_equal(expectedE, actualE, 1e-7)); factor.triangulateAndComputeE(actualE, values);
// Calculate using reprojectionError // get point
SmartFactor::Cameras::FBlocks F; boost::optional<Point3> point = factor.point();
Matrix E; CHECK(point);
Vector actualErrors = factor.unwhitenedError(cameras, *point, F, E);
EXPECT(assert_equal(expectedE, E, 1e-7));
EXPECT(assert_equal(Z_4x1, actualErrors, 1e-7)); // calculate numerical derivative with triangulated point
Matrix expectedE = sigma * numericalDerivative11<Vector, Point3>(f, *point);
EXPECT(assert_equal(expectedE, actualE, 1e-7));
// Calculate using computeJacobians // Calculate using reprojectionError
Vector b; SmartFactor::Cameras::FBlocks F;
SmartFactor::FBlocks Fs; Matrix E;
factor.computeJacobians(Fs, E, b, cameras, *point); Vector actualErrors = factor.unwhitenedError(cameras, *point, F, E);
double actualError3 = b.squaredNorm(); EXPECT(assert_equal(expectedE, E, 1e-7));
EXPECT(assert_equal(expectedE, E, 1e-7));
EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-6); EXPECT(assert_equal(Z_4x1, actualErrors, 1e-7));
}
// Calculate using computeJacobians
Vector b;
SmartFactor::FBlocks Fs;
factor.computeJacobians(Fs, E, b, cameras, *point);
double actualError3 = b.squaredNorm();
EXPECT(assert_equal(expectedE, E, 1e-7));
EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-6);
}
/* ************************************************************************* /* *************************************************************************
TEST( SmartProjectionPoseFactorRollingShutter, noisy ) { TEST( SmartProjectionPoseFactorRollingShutter, noisy ) {