Merge branch 'feature/rollingShutterSmartFactors' into feature/cameraTemplateForAllSmartFactors

release/4.3a0
lcarlone 2021-08-26 11:30:59 -04:00
commit 8af633a991
2 changed files with 218 additions and 9 deletions

View File

@ -37,9 +37,11 @@ namespace gtsam {
* from which the pixel observation pose can be interpolated.
* @addtogroup SLAM
*/
template<class CALIBRATION>
class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<
PinholePose<CALIBRATION> > {
template<class CAMERA>
class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAMERA> {
public:
typedef typename CAMERA::CalibrationType CALIBRATION;
protected:
/// shared pointer to calibration object (one for each observation)
@ -213,8 +215,8 @@ PinholePose<CALIBRATION> > {
/// equals
bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
const SmartProjectionPoseFactorRollingShutter<CALIBRATION>* e =
dynamic_cast<const SmartProjectionPoseFactorRollingShutter<CALIBRATION>*>(&p);
const SmartProjectionPoseFactorRollingShutter<CAMERA>* e =
dynamic_cast<const SmartProjectionPoseFactorRollingShutter<CAMERA>*>(&p);
double keyPairsEqual = true;
if(this->world_P_body_key_pairs_.size() == e->world_P_body_key_pairs().size()){
@ -430,9 +432,9 @@ PinholePose<CALIBRATION> > {
// end of class declaration
/// traits
template<class CALIBRATION>
struct traits<SmartProjectionPoseFactorRollingShutter<CALIBRATION> > :
public Testable<SmartProjectionPoseFactorRollingShutter<CALIBRATION> > {
template<class CAMERA>
struct traits<SmartProjectionPoseFactorRollingShutter<CAMERA> > :
public Testable<SmartProjectionPoseFactorRollingShutter<CAMERA> > {
};
} // namespace gtsam

View File

@ -73,7 +73,7 @@ Camera cam3(interp_pose3, sharedK);
}
LevenbergMarquardtParams lmParams;
typedef SmartProjectionPoseFactorRollingShutter<Cal3_S2> SmartFactorRS;
typedef SmartProjectionPoseFactorRollingShutter< PinholePose<Cal3_S2> > SmartFactorRS;
/* ************************************************************************* */
TEST( SmartProjectionPoseFactorRollingShutter, Constructor) {
@ -770,6 +770,213 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli
EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7);
}
/* *************************************************************************/
TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRollingShutter_measurementsFromSamePose) {
// in this test we make sure the fact works even if we have multiple pixel measurements of the same landmark
// at a single pose, a setup that occurs in multi-camera systems
using namespace vanillaPoseRS;
Point2Vector measurements_lmk1;
// Project three landmarks into three cameras
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1);
// create redundant measurements:
Camera::MeasurementVector measurements_lmk1_redundant = measurements_lmk1;
measurements_lmk1_redundant.push_back(measurements_lmk1.at(0)); // we readd the first measurement
// create inputs
std::vector<std::pair<Key, Key>> key_pairs;
key_pairs.push_back(std::make_pair(x1, x2));
key_pairs.push_back(std::make_pair(x2, x3));
key_pairs.push_back(std::make_pair(x3, x1));
key_pairs.push_back(std::make_pair(x1, x2));
std::vector<double> interp_factors;
interp_factors.push_back(interp_factor1);
interp_factors.push_back(interp_factor2);
interp_factors.push_back(interp_factor3);
interp_factors.push_back(interp_factor1);
SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model));
smartFactor1->add(measurements_lmk1_redundant, key_pairs, interp_factors, sharedK);
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise
Values values;
values.insert(x1, level_pose);
values.insert(x2, pose_right);
// initialize third pose with some noise to get a nontrivial linearization point
values.insert(x3, pose_above * noise_pose);
EXPECT( // check that the pose is actually noisy
assert_equal( Pose3( Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), Point3(0.1, -0.1, 1.9)), values.at<Pose3>(x3)));
// linearization point for the poses
Pose3 pose1 = level_pose;
Pose3 pose2 = pose_right;
Pose3 pose3 = pose_above * noise_pose;
// ==== check Hessian of smartFactor1 =====
// -- compute actual Hessian
boost::shared_ptr<GaussianFactor> linearfactor1 = smartFactor1->linearize(
values);
Matrix actualHessian = linearfactor1->information();
// -- compute expected Hessian from manual Schur complement from Jacobians
// linearization point for the 3D point
smartFactor1->triangulateSafe(smartFactor1->cameras(values));
TriangulationResult point = smartFactor1->point();
EXPECT(point.valid()); // check triangulated point is valid
// Use standard ProjectionFactorRollingShutter factor to calculate the Jacobians
Matrix F = Matrix::Zero(2 * 4, 6 * 3);
Matrix E = Matrix::Zero(2 * 4, 3);
Vector b = Vector::Zero(8);
// create projection factors rolling shutter
ProjectionFactorRollingShutter factor11(measurements_lmk1_redundant[0], interp_factor1,
model, x1, x2, l0, sharedK);
Matrix H1Actual, H2Actual, H3Actual;
// note: b is minus the reprojection error, cf the smart factor jacobian computation
b.segment<2>(0) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, H2Actual, H3Actual);
F.block<2, 6>(0, 0) = H1Actual;
F.block<2, 6>(0, 6) = H2Actual;
E.block<2, 3>(0, 0) = H3Actual;
ProjectionFactorRollingShutter factor12(measurements_lmk1_redundant[1], interp_factor2,
model, x2, x3, l0, sharedK);
b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual, H2Actual, H3Actual);
F.block<2, 6>(2, 6) = H1Actual;
F.block<2, 6>(2, 12) = H2Actual;
E.block<2, 3>(2, 0) = H3Actual;
ProjectionFactorRollingShutter factor13(measurements_lmk1_redundant[2], interp_factor3,
model, x3, x1, l0, sharedK);
b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual, H2Actual, H3Actual);
F.block<2, 6>(4, 12) = H1Actual;
F.block<2, 6>(4, 0) = H2Actual;
E.block<2, 3>(4, 0) = H3Actual;
ProjectionFactorRollingShutter factor14(measurements_lmk1_redundant[3], interp_factor1,
model, x1, x2, l0, sharedK);
b.segment<2>(6) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, H2Actual, H3Actual);
F.block<2, 6>(6, 0) = H1Actual;
F.block<2, 6>(6, 6) = H2Actual;
E.block<2, 3>(6, 0) = H3Actual;
// whiten
F = (1/sigma) * F;
E = (1/sigma) * E;
b = (1/sigma) * b;
//* G = F' * F - F' * E * P * E' * F
Matrix P = (E.transpose() * E).inverse();
Matrix expectedHessian = F.transpose() * F
- (F.transpose() * E * P * E.transpose() * F);
EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6));
// ==== check Information vector of smartFactor1 =====
GaussianFactorGraph gfg;
gfg.add(linearfactor1);
Matrix actualHessian_v2 = gfg.hessian().first;
EXPECT(assert_equal(actualHessian_v2, actualHessian, 1e-6)); // sanity check on hessian
// -- compute actual information vector
Vector actualInfoVector = gfg.hessian().second;
// -- compute expected information vector from manual Schur complement from Jacobians
//* g = F' * (b - E * P * E' * b)
Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b);
EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6));
// ==== check error of smartFactor1 (again) =====
NonlinearFactorGraph nfg_projFactorsRS;
nfg_projFactorsRS.add(factor11);
nfg_projFactorsRS.add(factor12);
nfg_projFactorsRS.add(factor13);
nfg_projFactorsRS.add(factor14);
values.insert(l0, *point);
double actualError = smartFactor1->error(values);
double expectedError = nfg_projFactorsRS.error(values);
EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7);
}
/* *************************************************************************/
TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_measurementsFromSamePose ) {
using namespace vanillaPoseRS;
Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3;
// Project three landmarks into three cameras
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1);
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2);
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3);
// create inputs
std::vector<std::pair<Key,Key>> key_pairs;
key_pairs.push_back(std::make_pair(x1,x2));
key_pairs.push_back(std::make_pair(x2,x3));
key_pairs.push_back(std::make_pair(x3,x1));
std::vector<double> interp_factors;
interp_factors.push_back(interp_factor1);
interp_factors.push_back(interp_factor2);
interp_factors.push_back(interp_factor3);
// For first factor, we create redundant measurement (taken by the same keys as factor 1, to
// make sure the redundancy in the keys does not create problems)
Camera::MeasurementVector& measurements_lmk1_redundant = measurements_lmk1;
measurements_lmk1_redundant.push_back(measurements_lmk1.at(0)); // we readd the first measurement
std::vector<std::pair<Key,Key>> key_pairs_redundant = key_pairs;
key_pairs_redundant.push_back(key_pairs.at(0)); // we readd the first pair of keys
std::vector<double> interp_factors_redundant = interp_factors;
interp_factors_redundant.push_back(interp_factors.at(0));// we readd the first interp factor
SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model));
smartFactor1->add(measurements_lmk1_redundant, key_pairs_redundant, interp_factors_redundant, sharedK);
SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model));
smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, sharedK);
SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model));
smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, sharedK);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
NonlinearFactorGraph graph;
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.addPrior(x1, level_pose, noisePrior);
graph.addPrior(x2, pose_right, noisePrior);
Values groundTruth;
groundTruth.insert(x1, level_pose);
groundTruth.insert(x2, pose_right);
groundTruth.insert(x3, pose_above);
DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise
Values values;
values.insert(x1, level_pose);
values.insert(x2, pose_right);
// initialize third pose with some noise, we expect it to move back to original pose_above
values.insert(x3, pose_above * noise_pose);
EXPECT( // check that the pose is actually noisy
assert_equal(
Pose3(
Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598,
-0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
Point3(0.1, -0.1, 1.9)), values.at<Pose3>(x3)));
Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
result = optimizer.optimize();
EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-5));
}
#ifndef DISABLE_TIMING
#include <gtsam/base/timing.h>
// -Total: 0 CPU (0 times, 0 wall, 0.04 children, min: 0 max: 0)