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

got to the final monster. Now I need to implement createHessian

release/4.3a0
lcarlone 2021-07-21 22:46:42 -04:00
parent e6ff03f73e
commit d4b88ba59a
2 changed files with 56 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h
View File

@ -98,7 +98,7 @@ PinholePose<CALIBRATION> > {
*/ */
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 = Pose3::identity()) {
// store measurements in base class (note: we manyally add keys below to make sure they are unique // store measurements in base class (note: we manyally add keys below to make sure they are unique
this->measured_.push_back(measured); this->measured_.push_back(measured);
@ -131,7 +131,7 @@ PinholePose<CALIBRATION> > {
* @param world_P_body_key_pairs vector of (1 for each view) containing the pair of poses from which each view can be interpolated * @param world_P_body_key_pairs vector of (1 for each view) containing the pair of poses from which each view can be interpolated
* @param Ks vector of intrinsic calibration objects * @param Ks vector of intrinsic calibration objects
*/ */
void add(const std::vector<Point2>& measurements, void add(const Point2Vector& measurements,
const std::vector<std::pair<Key, Key>>& world_P_body_key_pairs, const std::vector<std::pair<Key, Key>>& world_P_body_key_pairs,
const std::vector<double>& gammas, const std::vector<double>& gammas,
const std::vector<boost::shared_ptr<CALIBRATION>>& Ks, const std::vector<boost::shared_ptr<CALIBRATION>>& Ks,
@ -154,10 +154,10 @@ PinholePose<CALIBRATION> > {
* @param world_P_body_key_pairs vector of (1 for each view) containing the pair of poses from which each view can be interpolated * @param world_P_body_key_pairs vector of (1 for each view) containing the pair of poses from which each view can be interpolated
* @param K the (known) camera calibration (same for all measurements) * @param K the (known) camera calibration (same for all measurements)
*/ */
void add(const std::vector<Point2>& measurements, void add(const Point2Vector& measurements,
const std::vector<std::pair<Key, Key>>& world_P_body_key_pairs, const std::vector<std::pair<Key, Key>>& world_P_body_key_pairs,
const std::vector<double>& gammas, const std::vector<double>& gammas,
const boost::shared_ptr<CALIBRATION>& K, const Pose3 body_P_sensor) { const boost::shared_ptr<CALIBRATION>& K, const Pose3 body_P_sensor = Pose3::identity()) {
assert(world_P_body_key_pairs.size() == measurements.size()); assert(world_P_body_key_pairs.size() == measurements.size());
assert(world_P_body_key_pairs.size() == gammas.size()); assert(world_P_body_key_pairs.size() == gammas.size());
for (size_t i = 0; i < measurements.size(); i++) { for (size_t i = 0; i < measurements.size(); i++) {

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

@ -98,7 +98,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) {
using namespace vanillaPose; using namespace vanillaPose;
// create fake measurements // create fake measurements
std::vector<Point2> measurements; Point2Vector measurements;
measurements.push_back(measurement1); measurements.push_back(measurement1);
measurements.push_back(measurement2); measurements.push_back(measurement2);
measurements.push_back(measurement3); measurements.push_back(measurement3);
@ -170,7 +170,6 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) {
} }
static const int DimBlock = 12; ///< size of the variable stacking 2 poses from which the observation pose is interpolated static const int DimBlock = 12; ///< size of the variable stacking 2 poses from which the observation pose is interpolated
static const int DimPose = 6; ///< Pose3 dimension
static const int ZDim = 2; ///< Measurement dimension (Point2) static const int ZDim = 2; ///< Measurement dimension (Point2)
typedef Eigen::Matrix<double, ZDim, DimBlock> MatrixZD; // F blocks (derivatives wrt camera) typedef Eigen::Matrix<double, ZDim, DimBlock> MatrixZD; // F blocks (derivatives wrt camera)
typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks; // vector of F blocks typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks; // vector of F blocks
@ -298,68 +297,69 @@ TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) {
EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7);
} }
/* ************************************************************************* /* *************************************************************************/
TEST( SmartProjectionPoseFactorRollingShutter, 3poses_smart_projection_factor ) { TEST( SmartProjectionPoseFactorRollingShutter, 3poses_smart_projection_factor ) {
std::cout << "===================" << std::endl;
using namespace vanillaPose2; using namespace vanillaPoseRS;
Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
// Project three landmarks into three cameras // Project three landmarks into three cameras
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
KeyVector views; // create inputs
views.push_back(x1); std::vector<std::pair<Key,Key>> key_pairs;
views.push_back(x2); key_pairs.push_back(std::make_pair(x1,x2));
views.push_back(x3); key_pairs.push_back(std::make_pair(x2,x3));
key_pairs.push_back(std::make_pair(x3,x1));
SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK2)); std::vector<double> interp_factors;
smartFactor1->add(measurements_cam1, views); interp_factors.push_back(interp_factor1);
interp_factors.push_back(interp_factor2);
interp_factors.push_back(interp_factor3);
SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedK2)); SmartFactorRS smartFactor1(model);
smartFactor2->add(measurements_cam2, views); smartFactor1.add(measurements_cam1, key_pairs, interp_factors, sharedK);
SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedK2)); SmartFactorRS smartFactor2(model);
smartFactor3->add(measurements_cam3, views); smartFactor2.add(measurements_cam2, key_pairs, interp_factors, sharedK);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); SmartFactorRS smartFactor3(model);
smartFactor3.add(measurements_cam3, key_pairs, interp_factors, sharedK);
NonlinearFactorGraph graph; const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.addPrior(x1, cam1.pose(), noisePrior);
graph.addPrior(x2, cam2.pose(), noisePrior);
Values groundTruth; NonlinearFactorGraph graph;
groundTruth.insert(x1, cam1.pose()); graph.push_back(smartFactor1);
groundTruth.insert(x2, cam2.pose()); graph.push_back(smartFactor2);
groundTruth.insert(x3, cam3.pose()); graph.push_back(smartFactor3);
DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); graph.addPrior(x1, level_pose, noisePrior);
graph.addPrior(x2, pose_right, 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 Values groundTruth;
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), groundTruth.insert(x1, level_pose);
Point3(0.1, 0.1, 0.1)); // smaller noise groundTruth.insert(x2, pose_right);
Values values; groundTruth.insert(x3, pose_above);
values.insert(x1, cam1.pose()); DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);
values.insert(x2, cam2.pose());
// initialize third pose with some noise, we expect it to move back to original pose_above
values.insert(x3, pose_above * noise_pose);
EXPECT(
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; // 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
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
result = optimizer.optimize(); Point3(0.1, 0.1, 0.1)); // smaller noise
EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6)); 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);
/* ************************************************************************* Values result;
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; using namespace vanillaPose;