fixed another test, few more to go
lcarlone
parent
1c3ff0580b
commit
934413522d
|
|
@ -52,7 +52,7 @@ PinholePose<CALIBRATION> > {
|
|||
std::vector<std::pair<Key, Key>> world_P_body_key_pairs_;
|
||||
|
||||
/// interpolation factor (one for each observation) to interpolate between pair of consecutive poses
|
||||
std::vector<double> gammas_;
|
||||
std::vector<double> interp_param_;
|
||||
|
||||
/// Pose of the camera in the body frame
|
||||
std::vector<Pose3> body_P_sensors_; ///< Pose of the camera in the body frame
|
||||
|
|
@ -117,7 +117,7 @@ PinholePose<CALIBRATION> > {
|
|||
this->keys_.push_back(world_P_body_key2); // add only unique keys
|
||||
|
||||
// store interpolation factors
|
||||
gammas_.push_back(gamma);
|
||||
interp_param_.push_back(gamma);
|
||||
|
||||
// store fixed calibration
|
||||
K_all_.push_back(K);
|
||||
|
|
@ -180,7 +180,7 @@ PinholePose<CALIBRATION> > {
|
|||
|
||||
/// return the interpolation factors gammas
|
||||
const std::vector<double> getGammas() const {
|
||||
return gammas_;
|
||||
return interp_param_;
|
||||
}
|
||||
|
||||
/// return the extrinsic camera calibration body_P_sensors
|
||||
|
|
@ -202,7 +202,7 @@ PinholePose<CALIBRATION> > {
|
|||
<< keyFormatter(world_P_body_key_pairs_[i].first) << std::endl;
|
||||
std::cout << " pose2 key: "
|
||||
<< keyFormatter(world_P_body_key_pairs_[i].second) << std::endl;
|
||||
std::cout << " gamma: " << gammas_[i] << std::endl;
|
||||
std::cout << " gamma: " << interp_param_[i] << std::endl;
|
||||
body_P_sensors_[i].print("extrinsic calibration:\n");
|
||||
K_all_[i]->print("intrinsic calibration = ");
|
||||
}
|
||||
|
|
@ -237,7 +237,7 @@ PinholePose<CALIBRATION> > {
|
|||
}else{ extrinsicCalibrationEqual = false; }
|
||||
|
||||
return e && Base::equals(p, tol) && K_all_ == e->calibration()
|
||||
&& gammas_ == e->getGammas() && keyPairsEqual && extrinsicCalibrationEqual;
|
||||
&& interp_param_ == e->getGammas() && keyPairsEqual && extrinsicCalibrationEqual;
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -264,7 +264,7 @@ PinholePose<CALIBRATION> > {
|
|||
for (size_t i = 0; i < numViews; i++) { // for each camera/measurement
|
||||
Pose3 w_P_body1 = values.at<Pose3>(world_P_body_key_pairs_[i].first);
|
||||
Pose3 w_P_body2 = values.at<Pose3>(world_P_body_key_pairs_[i].second);
|
||||
double interpolationFactor = gammas_[i];
|
||||
double interpolationFactor = interp_param_[i];
|
||||
// get interpolated pose:
|
||||
Pose3 w_P_body = interpolate<Pose3>(w_P_body1, w_P_body2,interpolationFactor, dInterpPose_dPoseBody1, dInterpPose_dPoseBody2);
|
||||
Pose3 body_P_cam = body_P_sensors_[i];
|
||||
|
|
@ -322,7 +322,7 @@ PinholePose<CALIBRATION> > {
|
|||
|
||||
// compute Jacobian given triangulated 3D Point
|
||||
FBlocks Fs;
|
||||
Matrix F, E;
|
||||
Matrix E;
|
||||
Vector b;
|
||||
this->computeJacobiansWithTriangulatedPoint(Fs, E, b, values);
|
||||
|
||||
|
|
@ -369,7 +369,7 @@ PinholePose<CALIBRATION> > {
|
|||
typename Base::Cameras cameras(const Values& values) const override {
|
||||
size_t numViews = this->measured_.size();
|
||||
assert(numViews == K_all_.size());
|
||||
assert(numViews == gammas_.size());
|
||||
assert(numViews == interp_param_.size());
|
||||
assert(numViews == body_P_sensors_.size());
|
||||
assert(numViews == world_P_body_key_pairs_.size());
|
||||
|
||||
|
|
@ -377,7 +377,7 @@ PinholePose<CALIBRATION> > {
|
|||
for (size_t i = 0; i < numViews; i++) { // for each measurement
|
||||
Pose3 w_P_body1 = values.at<Pose3>(world_P_body_key_pairs_[i].first);
|
||||
Pose3 w_P_body2 = values.at<Pose3>(world_P_body_key_pairs_[i].second);
|
||||
double interpolationFactor = gammas_[i];
|
||||
double interpolationFactor = interp_param_[i];
|
||||
Pose3 w_P_body = interpolate<Pose3>(w_P_body1, w_P_body2, interpolationFactor);
|
||||
Pose3 body_P_cam = body_P_sensors_[i];
|
||||
Pose3 w_P_cam = w_P_body.compose(body_P_cam);
|
||||
|
|
|
|||
|
|
@ -298,8 +298,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) {
|
|||
}
|
||||
|
||||
/* *************************************************************************/
|
||||
TEST( SmartProjectionPoseFactorRollingShutter, 3poses_smart_projection_factor ) {
|
||||
std::cout << "===================" << std::endl;
|
||||
TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses ) {
|
||||
|
||||
using namespace vanillaPoseRS;
|
||||
Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
|
||||
|
|
@ -365,173 +364,101 @@ TEST( SmartProjectionPoseFactorRollingShutter, 3poses_smart_projection_factor )
|
|||
EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6));
|
||||
}
|
||||
|
||||
/* *************************************************************************
|
||||
TEST( SmartProjectionPoseFactorRollingShutter, Factors ) {
|
||||
/* *************************************************************************/
|
||||
TEST( SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses ) {
|
||||
// here we replicate a test in SmartProjectionPoseFactor by setting interpolation
|
||||
// factors to 0 and 1 (such that the rollingShutter measurements falls back to standard pixel measurements)
|
||||
// Note: this is a quite extreme test since in typical camera you would not have more than
|
||||
// 1 measurement per landmark at each interpolated pose
|
||||
using namespace vanillaPose;
|
||||
|
||||
using namespace vanillaPose;
|
||||
// Default cameras for simple derivatives
|
||||
static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0));
|
||||
|
||||
// Default cameras for simple derivatives
|
||||
Rot3 R;
|
||||
static Cal3_S2::shared_ptr sharedK(new Cal3_S2(100, 100, 0, 0, 0));
|
||||
Camera cam1(Pose3(R, Point3(0, 0, 0)), sharedK), cam2(
|
||||
Pose3(R, Point3(1, 0, 0)), sharedK);
|
||||
Rot3 R = Rot3::identity();
|
||||
Pose3 pose1 = Pose3(R, Point3(0, 0, 0));
|
||||
Pose3 pose2 = Pose3(R, Point3(1, 0, 0));
|
||||
Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple);
|
||||
Pose3 body_P_sensorId = Pose3::identity();
|
||||
|
||||
// one landmarks 1m in front of camera
|
||||
Point3 landmark1(0, 0, 10);
|
||||
// one landmarks 1m in front of camera
|
||||
Point3 landmark1(0, 0, 10);
|
||||
|
||||
Point2Vector measurements_cam1;
|
||||
Point2Vector measurements_cam1;
|
||||
|
||||
// Project 2 landmarks into 2 cameras
|
||||
measurements_cam1.push_back(cam1.project(landmark1));
|
||||
measurements_cam1.push_back(cam2.project(landmark1));
|
||||
// Project 2 landmarks into 2 cameras
|
||||
measurements_cam1.push_back(cam1.project(landmark1));
|
||||
measurements_cam1.push_back(cam2.project(landmark1));
|
||||
|
||||
// Create smart factors
|
||||
KeyVector views {x1, x2};
|
||||
SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model));
|
||||
double interp_factor = 0; // equivalent to measurement taken at pose 1
|
||||
smartFactor1->add(measurements_cam1[0], x1, x2, interp_factor, sharedKSimple,
|
||||
body_P_sensorId);
|
||||
interp_factor = 1; // equivalent to measurement taken at pose 2
|
||||
smartFactor1->add(measurements_cam1[1], x1, x2, interp_factor, sharedKSimple,
|
||||
body_P_sensorId);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1 = boost::make_shared<SmartFactor>(model, sharedK);
|
||||
smartFactor1->add(measurements_cam1, views);
|
||||
SmartFactor::Cameras cameras;
|
||||
cameras.push_back(cam1);
|
||||
cameras.push_back(cam2);
|
||||
|
||||
SmartFactor::Cameras cameras;
|
||||
cameras.push_back(cam1);
|
||||
cameras.push_back(cam2);
|
||||
// Make sure triangulation works
|
||||
CHECK(smartFactor1->triangulateSafe(cameras));
|
||||
CHECK(!smartFactor1->isDegenerate());
|
||||
CHECK(!smartFactor1->isPointBehindCamera());
|
||||
boost::optional<Point3> p = smartFactor1->point();
|
||||
CHECK(p);
|
||||
EXPECT(assert_equal(landmark1, *p));
|
||||
|
||||
// Make sure triangulation works
|
||||
CHECK(smartFactor1->triangulateSafe(cameras));
|
||||
CHECK(!smartFactor1->isDegenerate());
|
||||
CHECK(!smartFactor1->isPointBehindCamera());
|
||||
boost::optional<Point3> p = smartFactor1->point();
|
||||
CHECK(p);
|
||||
EXPECT(assert_equal(landmark1, *p));
|
||||
VectorValues zeroDelta;
|
||||
Vector6 delta;
|
||||
delta.setZero();
|
||||
zeroDelta.insert(x1, delta);
|
||||
zeroDelta.insert(x2, delta);
|
||||
|
||||
VectorValues zeroDelta;
|
||||
Vector6 delta;
|
||||
delta.setZero();
|
||||
zeroDelta.insert(x1, delta);
|
||||
zeroDelta.insert(x2, delta);
|
||||
VectorValues perturbedDelta;
|
||||
delta.setOnes();
|
||||
perturbedDelta.insert(x1, delta);
|
||||
perturbedDelta.insert(x2, delta);
|
||||
double expectedError = 2500;
|
||||
|
||||
VectorValues perturbedDelta;
|
||||
delta.setOnes();
|
||||
perturbedDelta.insert(x1, delta);
|
||||
perturbedDelta.insert(x2, delta);
|
||||
double expectedError = 2500;
|
||||
// After eliminating the point, A1 and A2 contain 2-rank information on cameras:
|
||||
Matrix16 A1, A2;
|
||||
A1 << -10, 0, 0, 0, 1, 0;
|
||||
A2 << 10, 0, 1, 0, -1, 0;
|
||||
A1 *= 10. / sigma;
|
||||
A2 *= 10. / sigma;
|
||||
Matrix expectedInformation; // filled below
|
||||
|
||||
// After eliminating the point, A1 and A2 contain 2-rank information on cameras:
|
||||
Matrix16 A1, A2;
|
||||
A1 << -10, 0, 0, 0, 1, 0;
|
||||
A2 << 10, 0, 1, 0, -1, 0;
|
||||
A1 *= 10. / sigma;
|
||||
A2 *= 10. / sigma;
|
||||
Matrix expectedInformation; // filled below
|
||||
{
|
||||
// createHessianFactor
|
||||
Matrix66 G11 = 0.5 * A1.transpose() * A1;
|
||||
Matrix66 G12 = 0.5 * A1.transpose() * A2;
|
||||
Matrix66 G22 = 0.5 * A2.transpose() * A2;
|
||||
// createHessianFactor
|
||||
Matrix66 G11 = 0.5 * A1.transpose() * A1;
|
||||
Matrix66 G12 = 0.5 * A1.transpose() * A2;
|
||||
Matrix66 G22 = 0.5 * A2.transpose() * A2;
|
||||
|
||||
Vector6 g1;
|
||||
g1.setZero();
|
||||
Vector6 g2;
|
||||
g2.setZero();
|
||||
Vector6 g1;
|
||||
g1.setZero();
|
||||
Vector6 g2;
|
||||
g2.setZero();
|
||||
|
||||
double f = 0;
|
||||
double f = 0;
|
||||
|
||||
RegularHessianFactor<6> expected(x1, x2, G11, G12, g1, G22, g2, f);
|
||||
expectedInformation = expected.information();
|
||||
RegularHessianFactor<6> expected(x1, x2, G11, G12, g1, G22, g2, f);
|
||||
expectedInformation = expected.information();
|
||||
|
||||
boost::shared_ptr<RegularHessianFactor<6> > actual =
|
||||
smartFactor1->createHessianFactor(cameras, 0.0);
|
||||
EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6));
|
||||
EXPECT(assert_equal(expected, *actual, 1e-6));
|
||||
EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6);
|
||||
EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6);
|
||||
}
|
||||
|
||||
{
|
||||
Matrix26 F1;
|
||||
F1.setZero();
|
||||
F1(0, 1) = -100;
|
||||
F1(0, 3) = -10;
|
||||
F1(1, 0) = 100;
|
||||
F1(1, 4) = -10;
|
||||
Matrix26 F2;
|
||||
F2.setZero();
|
||||
F2(0, 1) = -101;
|
||||
F2(0, 3) = -10;
|
||||
F2(0, 5) = -1;
|
||||
F2(1, 0) = 100;
|
||||
F2(1, 2) = 10;
|
||||
F2(1, 4) = -10;
|
||||
Matrix E(4, 3);
|
||||
E.setZero();
|
||||
E(0, 0) = 10;
|
||||
E(1, 1) = 10;
|
||||
E(2, 0) = 10;
|
||||
E(2, 2) = 1;
|
||||
E(3, 1) = 10;
|
||||
SmartFactor::FBlocks Fs = list_of<Matrix>(F1)(F2);
|
||||
Vector b(4);
|
||||
b.setZero();
|
||||
|
||||
// Create smart factors
|
||||
KeyVector keys;
|
||||
keys.push_back(x1);
|
||||
keys.push_back(x2);
|
||||
|
||||
// createJacobianQFactor
|
||||
SharedIsotropic n = noiseModel::Isotropic::Sigma(4, sigma);
|
||||
Matrix3 P = (E.transpose() * E).inverse();
|
||||
JacobianFactorQ<6, 2> expectedQ(keys, Fs, E, P, b, n);
|
||||
EXPECT(assert_equal(expectedInformation, expectedQ.information(), 1e-6));
|
||||
|
||||
boost::shared_ptr<JacobianFactorQ<6, 2> > actualQ =
|
||||
smartFactor1->createJacobianQFactor(cameras, 0.0);
|
||||
CHECK(actualQ);
|
||||
EXPECT(assert_equal(expectedInformation, actualQ->information(), 1e-6));
|
||||
EXPECT(assert_equal(expectedQ, *actualQ));
|
||||
EXPECT_DOUBLES_EQUAL(0, actualQ->error(zeroDelta), 1e-6);
|
||||
EXPECT_DOUBLES_EQUAL(expectedError, actualQ->error(perturbedDelta), 1e-6);
|
||||
|
||||
// Whiten for RegularImplicitSchurFactor (does not have noise model)
|
||||
model->WhitenSystem(E, b);
|
||||
Matrix3 whiteP = (E.transpose() * E).inverse();
|
||||
Fs[0] = model->Whiten(Fs[0]);
|
||||
Fs[1] = model->Whiten(Fs[1]);
|
||||
|
||||
// createRegularImplicitSchurFactor
|
||||
RegularImplicitSchurFactor<Camera> expected(keys, Fs, E, whiteP, b);
|
||||
|
||||
boost::shared_ptr<RegularImplicitSchurFactor<Camera> > actual =
|
||||
smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0);
|
||||
CHECK(actual);
|
||||
EXPECT(assert_equal(expectedInformation, expected.information(), 1e-6));
|
||||
EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6));
|
||||
EXPECT(assert_equal(expected, *actual));
|
||||
EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6);
|
||||
EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6);
|
||||
}
|
||||
|
||||
{
|
||||
// createJacobianSVDFactor
|
||||
Vector1 b;
|
||||
b.setZero();
|
||||
double s = sigma * sin(M_PI_4);
|
||||
SharedIsotropic n = noiseModel::Isotropic::Sigma(4 - 3, sigma);
|
||||
JacobianFactor expected(x1, s * A1, x2, s * A2, b, n);
|
||||
EXPECT(assert_equal(expectedInformation, expected.information(), 1e-6));
|
||||
|
||||
boost::shared_ptr<JacobianFactor> actual =
|
||||
smartFactor1->createJacobianSVDFactor(cameras, 0.0);
|
||||
CHECK(actual);
|
||||
EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6));
|
||||
EXPECT(assert_equal(expected, *actual));
|
||||
EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6);
|
||||
EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6);
|
||||
}
|
||||
}
|
||||
Values values;
|
||||
values.insert(x1, pose1);
|
||||
values.insert(x2, pose2);
|
||||
boost::shared_ptr < RegularHessianFactor<6> > actual = smartFactor1
|
||||
->createHessianFactor(values);
|
||||
EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6));
|
||||
EXPECT(assert_equal(expected, *actual, 1e-6));
|
||||
EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6);
|
||||
EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6);
|
||||
}
|
||||
|
||||
/* *************************************************************************
|
||||
TEST( SmartProjectionPoseFactorRollingShutter, 3poses_iterative_smart_projection_factor ) {
|
||||
|
||||
std::cout << "===================" << std::endl;
|
||||
using namespace vanillaPose;
|
||||
|
||||
KeyVector views {x1, x2, x3};
|
||||
|
|
|
|||
Loading…
Reference in New Issue