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

solidified add and equal

release/4.3a0
lcarlone 2021-07-21 14:30:55 -04:00
parent 30f304e733
commit 306393a18c
2 changed files with 832 additions and 832 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h
View File

@ -99,8 +99,9 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<
void add(const Point2& measured, const Key& world_P_body_key1, void add(const Point2& measured, const Key& world_P_body_key1,
const Key& world_P_body_key2, const double& gamma, const Key& world_P_body_key2, const double& gamma,
const boost::shared_ptr<CALIBRATION>& K, const Pose3 body_P_sensor) { const boost::shared_ptr<CALIBRATION>& K, const Pose3 body_P_sensor) {
// store measurements in base class (note: we only store the first key there) // store measurements in base class (note: we manyally add keys below to make sure they are unique
Base::add(measured, world_P_body_key1); this->measured_.push_back(measured);
// but we also store the extrinsic calibration keys in the same order // but we also store the extrinsic calibration keys in the same order
world_P_body_key_pairs_.push_back( world_P_body_key_pairs_.push_back(
std::make_pair(world_P_body_key1, world_P_body_key2)); std::make_pair(world_P_body_key1, world_P_body_key2));
@ -113,6 +114,9 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<
== this->keys_.end()) == this->keys_.end())
this->keys_.push_back(world_P_body_key2); // add only unique keys this->keys_.push_back(world_P_body_key2); // add only unique keys
// store interpolation factors
gammas_.push_back(gamma);
// store fixed calibration // store fixed calibration
K_all_.push_back(K); K_all_.push_back(K);

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

@ -16,7 +16,7 @@
* @date July 2021 * @date July 2021
*/ */
//#include "gtsam/slam/tests/smartFactorScenarios.h" #include "gtsam/slam/tests/smartFactorScenarios.h"
#include <gtsam/slam/ProjectionFactor.h> #include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/slam/PoseTranslationPrior.h> #include <gtsam/slam/PoseTranslationPrior.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
@ -44,8 +44,17 @@ using symbol_shorthand::L;
static Symbol x1('X', 1); static Symbol x1('X', 1);
static Symbol x2('X', 2); static Symbol x2('X', 2);
static Symbol x3('X', 3); 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 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; LevenbergMarquardtParams lmParams;
typedef SmartProjectionPoseFactorRollingShutter<Cal3_S2> SmartFactorRS; typedef SmartProjectionPoseFactorRollingShutter<Cal3_S2> SmartFactorRS;
@ -55,51 +64,94 @@ TEST( SmartProjectionPoseFactorRollingShutter, Constructor) {
SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model));
} }
/* ************************************************************************* * /* ************************************************************************* */
TEST( SmartProjectionPoseFactorRollingShutter, Constructor2) { TEST( SmartProjectionPoseFactorRollingShutter, Constructor2) {
using namespace vanillaPose;
SmartProjectionParams params; SmartProjectionParams params;
params.setRankTolerance(rankTol); params.setRankTolerance(rankTol);
SmartFactor factor1(model, sharedK, params); SmartFactorRS factor1(model, params);
} }
/* ************************************************************************* * /* ************************************************************************* */
TEST( SmartProjectionPoseFactorRollingShutter, Constructor3) { TEST( SmartProjectionPoseFactorRollingShutter, add) {
using namespace vanillaPose; using namespace vanillaPose;
SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK)); SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model));
factor1->add(measurement1, x1); 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 ) { TEST( SmartProjectionPoseFactorRollingShutter, Equals ) {
using namespace vanillaPose; using namespace vanillaPose;
SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK));
factor1->add(measurement1, x1);
SmartFactor::shared_ptr factor2(new SmartFactor(model,sharedK)); // create fake measurements
factor2->add(measurement1, x1); 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, *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));
}
} }
/* ************************************************************************* /* *************************************************************************
@ -548,62 +600,6 @@ TEST( SmartProjectionPoseFactorRollingShutter, 3poses_iterative_smart_projection
EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-7)); 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 ) {