solidified add and equal
lcarlone
parent
30f304e733
commit
306393a18c
|
|
@ -99,8 +99,9 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<
|
|||
void add(const Point2& measured, const Key& world_P_body_key1,
|
||||
const Key& world_P_body_key2, const double& gamma,
|
||||
const boost::shared_ptr<CALIBRATION>& K, const Pose3 body_P_sensor) {
|
||||
// store measurements in base class (note: we only store the first key there)
|
||||
Base::add(measured, world_P_body_key1);
|
||||
// store measurements in base class (note: we manyally add keys below to make sure they are unique
|
||||
this->measured_.push_back(measured);
|
||||
|
||||
// but we also store the extrinsic calibration keys in the same order
|
||||
world_P_body_key_pairs_.push_back(
|
||||
std::make_pair(world_P_body_key1, world_P_body_key2));
|
||||
|
|
@ -113,6 +114,9 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<
|
|||
== this->keys_.end())
|
||||
this->keys_.push_back(world_P_body_key2); // add only unique keys
|
||||
|
||||
// store interpolation factors
|
||||
gammas_.push_back(gamma);
|
||||
|
||||
// store fixed calibration
|
||||
K_all_.push_back(K);
|
||||
|
||||
|
|
|
|||
|
|
@ -16,7 +16,7 @@
|
|||
* @date July 2021
|
||||
*/
|
||||
|
||||
//#include "gtsam/slam/tests/smartFactorScenarios.h"
|
||||
#include "gtsam/slam/tests/smartFactorScenarios.h"
|
||||
#include <gtsam/slam/ProjectionFactor.h>
|
||||
#include <gtsam/slam/PoseTranslationPrior.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
|
|
@ -44,8 +44,17 @@ using symbol_shorthand::L;
|
|||
static Symbol x1('X', 1);
|
||||
static Symbol x2('X', 2);
|
||||
static Symbol x3('X', 3);
|
||||
static Symbol x4('X', 4);
|
||||
static Pose3 body_P_sensor = Pose3(Rot3::Ypr(-0.1, 0.2, -0.2),
|
||||
Point3(0.1, 0.0, 0.0));
|
||||
|
||||
static Point2 measurement1(323.0, 240.0);
|
||||
static Point2 measurement2(200.0, 220.0);
|
||||
static Point2 measurement3(320.0, 10.0);
|
||||
static double interp_factor = 0.5;
|
||||
static double interp_factor1 = 0.3;
|
||||
static double interp_factor2 = 0.4;
|
||||
static double interp_factor3 = 0.5;
|
||||
|
||||
LevenbergMarquardtParams lmParams;
|
||||
typedef SmartProjectionPoseFactorRollingShutter<Cal3_S2> SmartFactorRS;
|
||||
|
|
@ -55,55 +64,98 @@ TEST( SmartProjectionPoseFactorRollingShutter, Constructor) {
|
|||
SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model));
|
||||
}
|
||||
|
||||
/* ************************************************************************* *
|
||||
/* ************************************************************************* */
|
||||
TEST( SmartProjectionPoseFactorRollingShutter, Constructor2) {
|
||||
using namespace vanillaPose;
|
||||
SmartProjectionParams params;
|
||||
params.setRankTolerance(rankTol);
|
||||
SmartFactor factor1(model, sharedK, params);
|
||||
SmartFactorRS factor1(model, params);
|
||||
}
|
||||
|
||||
/* ************************************************************************* *
|
||||
TEST( SmartProjectionPoseFactorRollingShutter, Constructor3) {
|
||||
/* ************************************************************************* */
|
||||
TEST( SmartProjectionPoseFactorRollingShutter, add) {
|
||||
using namespace vanillaPose;
|
||||
SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK));
|
||||
factor1->add(measurement1, x1);
|
||||
SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model));
|
||||
factor1->add(measurement1, x1, x2, interp_factor, sharedK, body_P_sensor);
|
||||
}
|
||||
|
||||
/* ************************************************************************* *
|
||||
TEST( SmartProjectionPoseFactorRollingShutter, Constructor4) {
|
||||
using namespace vanillaPose;
|
||||
SmartProjectionParams params;
|
||||
params.setRankTolerance(rankTol);
|
||||
SmartFactor factor1(model, sharedK, params);
|
||||
factor1.add(measurement1, x1);
|
||||
}
|
||||
|
||||
/* ************************************************************************* *
|
||||
TEST( SmartProjectionPoseFactorRollingShutter, params) {
|
||||
using namespace vanillaPose;
|
||||
SmartProjectionParams params;
|
||||
double rt = params.getRetriangulationThreshold();
|
||||
EXPECT_DOUBLES_EQUAL(1e-5, rt, 1e-7);
|
||||
params.setRetriangulationThreshold(1e-3);
|
||||
rt = params.getRetriangulationThreshold();
|
||||
EXPECT_DOUBLES_EQUAL(1e-3, rt, 1e-7);
|
||||
}
|
||||
|
||||
/* ************************************************************************* *
|
||||
/* ************************************************************************* */
|
||||
TEST( SmartProjectionPoseFactorRollingShutter, Equals ) {
|
||||
using namespace vanillaPose;
|
||||
SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK));
|
||||
factor1->add(measurement1, x1);
|
||||
|
||||
SmartFactor::shared_ptr factor2(new SmartFactor(model,sharedK));
|
||||
factor2->add(measurement1, x1);
|
||||
// create fake measurements
|
||||
std::vector<Point2> measurements;
|
||||
measurements.push_back(measurement1);
|
||||
measurements.push_back(measurement2);
|
||||
measurements.push_back(measurement3);
|
||||
|
||||
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,x4));
|
||||
|
||||
std::vector<boost::shared_ptr<Cal3_S2>> intrinsicCalibrations;
|
||||
intrinsicCalibrations.push_back(sharedK);
|
||||
intrinsicCalibrations.push_back(sharedK);
|
||||
intrinsicCalibrations.push_back(sharedK);
|
||||
|
||||
std::vector<Pose3> extrinsicCalibrations;
|
||||
extrinsicCalibrations.push_back(body_P_sensor);
|
||||
extrinsicCalibrations.push_back(body_P_sensor);
|
||||
extrinsicCalibrations.push_back(body_P_sensor);
|
||||
|
||||
std::vector<double> interp_factors;
|
||||
interp_factors.push_back(interp_factor1);
|
||||
interp_factors.push_back(interp_factor2);
|
||||
interp_factors.push_back(interp_factor3);
|
||||
|
||||
// create by adding a batch of measurements with a bunch of calibrations
|
||||
SmartFactorRS::shared_ptr factor2(new SmartFactorRS(model));
|
||||
factor2->add(measurements, key_pairs, interp_factors, intrinsicCalibrations, extrinsicCalibrations);
|
||||
|
||||
// create by adding a batch of measurements with a single calibrations
|
||||
SmartFactorRS::shared_ptr factor3(new SmartFactorRS(model));
|
||||
factor3->add(measurements, key_pairs, interp_factors, sharedK, body_P_sensor);
|
||||
|
||||
{ // create equal factors and show equal returns true
|
||||
SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model));
|
||||
factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor);
|
||||
factor1->add(measurement2, x2, x3, interp_factor2, sharedK, body_P_sensor);
|
||||
factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor);
|
||||
|
||||
CHECK(assert_equal(*factor1, *factor2));
|
||||
CHECK(assert_equal(*factor1, *factor3));
|
||||
}
|
||||
{ // create slightly different factors (different keys) and show equal returns false
|
||||
SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model));
|
||||
factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor);
|
||||
factor1->add(measurement2, x2, x2, interp_factor2, sharedK, body_P_sensor); // different!
|
||||
factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor);
|
||||
|
||||
CHECK(!assert_equal(*factor1, *factor2));
|
||||
CHECK(!assert_equal(*factor1, *factor3));
|
||||
}
|
||||
{ // create slightly different factors (different extrinsics) and show equal returns false
|
||||
SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model));
|
||||
factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor);
|
||||
factor1->add(measurement2, x2, x3, interp_factor2, sharedK, body_P_sensor*body_P_sensor); // different!
|
||||
factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor);
|
||||
|
||||
CHECK(!assert_equal(*factor1, *factor2));
|
||||
CHECK(!assert_equal(*factor1, *factor3));
|
||||
}
|
||||
{ // create slightly different factors (different interp factors) and show equal returns false
|
||||
SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model));
|
||||
factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor);
|
||||
factor1->add(measurement2, x2, x3, interp_factor1, sharedK, body_P_sensor); // different!
|
||||
factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor);
|
||||
|
||||
CHECK(!assert_equal(*factor1, *factor2));
|
||||
CHECK(!assert_equal(*factor1, *factor3));
|
||||
}
|
||||
}
|
||||
|
||||
/* *************************************************************************
|
||||
TEST( SmartProjectionPoseFactorRollingShutter, noiseless ) {
|
||||
TEST( SmartProjectionPoseFactorRollingShutter, noiseless ) {
|
||||
|
||||
using namespace vanillaPose;
|
||||
|
||||
|
|
@ -158,10 +210,10 @@ TEST( SmartProjectionPoseFactorRollingShutter, noiseless ) {
|
|||
double actualError3 = b.squaredNorm();
|
||||
EXPECT(assert_equal(expectedE, E, 1e-7));
|
||||
EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-6);
|
||||
}
|
||||
}
|
||||
|
||||
/* *************************************************************************
|
||||
TEST( SmartProjectionPoseFactorRollingShutter, noisy ) {
|
||||
/* *************************************************************************
|
||||
TEST( SmartProjectionPoseFactorRollingShutter, noisy ) {
|
||||
|
||||
using namespace vanillaPose;
|
||||
|
||||
|
|
@ -192,10 +244,10 @@ TEST( SmartProjectionPoseFactorRollingShutter, noisy ) {
|
|||
factor2->add(measurements, views);
|
||||
double actualError2 = factor2->error(values);
|
||||
DOUBLES_EQUAL(actualError1, actualError2, 1e-7);
|
||||
}
|
||||
}
|
||||
|
||||
/* *************************************************************************
|
||||
TEST(SmartProjectionPoseFactorRollingShutter, smartFactorWithSensorBodyTransform) {
|
||||
/* *************************************************************************
|
||||
TEST(SmartProjectionPoseFactorRollingShutter, smartFactorWithSensorBodyTransform) {
|
||||
using namespace vanillaPose;
|
||||
|
||||
// create arbitrary body_T_sensor (transforms from sensor to body)
|
||||
|
|
@ -267,10 +319,10 @@ TEST(SmartProjectionPoseFactorRollingShutter, smartFactorWithSensorBodyTransform
|
|||
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
|
||||
result = optimizer.optimize();
|
||||
EXPECT(assert_equal(wTb3, result.at<Pose3>(x3)));
|
||||
}
|
||||
}
|
||||
|
||||
/* *************************************************************************
|
||||
TEST( SmartProjectionPoseFactorRollingShutter, 3poses_smart_projection_factor ) {
|
||||
/* *************************************************************************
|
||||
TEST( SmartProjectionPoseFactorRollingShutter, 3poses_smart_projection_factor ) {
|
||||
|
||||
using namespace vanillaPose2;
|
||||
Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
|
||||
|
|
@ -328,10 +380,10 @@ TEST( SmartProjectionPoseFactorRollingShutter, 3poses_smart_projection_factor )
|
|||
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
|
||||
result = optimizer.optimize();
|
||||
EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6));
|
||||
}
|
||||
}
|
||||
|
||||
/* *************************************************************************
|
||||
TEST( SmartProjectionPoseFactorRollingShutter, Factors ) {
|
||||
/* *************************************************************************
|
||||
TEST( SmartProjectionPoseFactorRollingShutter, Factors ) {
|
||||
|
||||
using namespace vanillaPose;
|
||||
|
||||
|
|
@ -492,10 +544,10 @@ TEST( SmartProjectionPoseFactorRollingShutter, Factors ) {
|
|||
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 ) {
|
||||
/* *************************************************************************
|
||||
TEST( SmartProjectionPoseFactorRollingShutter, 3poses_iterative_smart_projection_factor ) {
|
||||
|
||||
using namespace vanillaPose;
|
||||
|
||||
|
|
@ -546,66 +598,10 @@ TEST( SmartProjectionPoseFactorRollingShutter, 3poses_iterative_smart_projection
|
|||
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
|
||||
result = optimizer.optimize();
|
||||
EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-7));
|
||||
}
|
||||
}
|
||||
|
||||
/* *************************************************************************
|
||||
TEST( SmartProjectionPoseFactorRollingShutter, jacobianSVD ) {
|
||||
|
||||
using namespace vanillaPose;
|
||||
|
||||
KeyVector views {x1, x2, x3};
|
||||
|
||||
Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
|
||||
|
||||
// Project three landmarks into three cameras
|
||||
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
|
||||
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
|
||||
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
|
||||
|
||||
SmartProjectionParams params;
|
||||
params.setRankTolerance(1.0);
|
||||
params.setLinearizationMode(gtsam::JACOBIAN_SVD);
|
||||
params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY);
|
||||
params.setEnableEPI(false);
|
||||
SmartFactor factor1(model, sharedK, params);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(
|
||||
new SmartFactor(model, sharedK, params));
|
||||
smartFactor1->add(measurements_cam1, views);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(
|
||||
new SmartFactor(model, sharedK, params));
|
||||
smartFactor2->add(measurements_cam2, views);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(
|
||||
new SmartFactor(model, sharedK, params));
|
||||
smartFactor3->add(measurements_cam3, views);
|
||||
|
||||
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, cam1.pose(), noisePrior);
|
||||
graph.addPrior(x2, cam2.pose(), noisePrior);
|
||||
|
||||
// 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, cam1.pose());
|
||||
values.insert(x2, cam2.pose());
|
||||
values.insert(x3, pose_above * noise_pose);
|
||||
|
||||
Values result;
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
|
||||
result = optimizer.optimize();
|
||||
EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6));
|
||||
}
|
||||
|
||||
/* *************************************************************************
|
||||
TEST( SmartProjectionPoseFactorRollingShutter, landmarkDistance ) {
|
||||
/* *************************************************************************
|
||||
TEST( SmartProjectionPoseFactorRollingShutter, landmarkDistance ) {
|
||||
|
||||
using namespace vanillaPose;
|
||||
|
||||
|
|
@ -661,10 +657,10 @@ TEST( SmartProjectionPoseFactorRollingShutter, landmarkDistance ) {
|
|||
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
|
||||
result = optimizer.optimize();
|
||||
EXPECT(assert_equal(values.at<Pose3>(x3), result.at<Pose3>(x3)));
|
||||
}
|
||||
}
|
||||
|
||||
/* *************************************************************************
|
||||
TEST( SmartProjectionPoseFactorRollingShutter, dynamicOutlierRejection ) {
|
||||
/* *************************************************************************
|
||||
TEST( SmartProjectionPoseFactorRollingShutter, dynamicOutlierRejection ) {
|
||||
|
||||
using namespace vanillaPose;
|
||||
|
||||
|
|
@ -727,10 +723,10 @@ TEST( SmartProjectionPoseFactorRollingShutter, dynamicOutlierRejection ) {
|
|||
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
|
||||
result = optimizer.optimize();
|
||||
EXPECT(assert_equal(cam3.pose(), result.at<Pose3>(x3)));
|
||||
}
|
||||
}
|
||||
|
||||
/* *************************************************************************
|
||||
TEST( SmartProjectionPoseFactorRollingShutter, 3poses_projection_factor ) {
|
||||
/* *************************************************************************
|
||||
TEST( SmartProjectionPoseFactorRollingShutter, 3poses_projection_factor ) {
|
||||
|
||||
using namespace vanillaPose2;
|
||||
|
||||
|
|
@ -774,10 +770,10 @@ TEST( SmartProjectionPoseFactorRollingShutter, 3poses_projection_factor ) {
|
|||
DOUBLES_EQUAL(0, graph.error(result), 1e-9);
|
||||
|
||||
EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-7));
|
||||
}
|
||||
}
|
||||
|
||||
/* *************************************************************************
|
||||
TEST( SmartProjectionPoseFactorRollingShutter, CheckHessian) {
|
||||
/* *************************************************************************
|
||||
TEST( SmartProjectionPoseFactorRollingShutter, CheckHessian) {
|
||||
|
||||
KeyVector views {x1, x2, x3};
|
||||
|
||||
|
|
@ -856,10 +852,10 @@ TEST( SmartProjectionPoseFactorRollingShutter, CheckHessian) {
|
|||
|
||||
// Check Hessian
|
||||
EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-6));
|
||||
}
|
||||
}
|
||||
|
||||
/* *************************************************************************
|
||||
TEST( SmartProjectionPoseFactorRollingShutter, Hessian ) {
|
||||
/* *************************************************************************
|
||||
TEST( SmartProjectionPoseFactorRollingShutter, Hessian ) {
|
||||
|
||||
using namespace vanillaPose2;
|
||||
|
||||
|
|
@ -887,9 +883,9 @@ TEST( SmartProjectionPoseFactorRollingShutter, Hessian ) {
|
|||
// compute reprojection errors (sum squared)
|
||||
// compare with factor.info(): the bottom right element is the squared sum of the reprojection errors (normalized by the covariance)
|
||||
// check that it is correctly scaled when using noiseProjection = [1/4 0; 0 1/4]
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
|
|
|
|||
Loading…
Reference in New Issue