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

adjusted rolling shutter as well

release/4.3a0
lcarlone 2021-08-28 01:31:50 -04:00
parent 22f8610472
commit ff33eb614d
7 changed files with 175 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
gtsam/geometry/SphericalCamera.h
View File

@ -30,15 +30,34 @@
namespace gtsam {
class GTSAM_EXPORT EmptyCal {
protected:
Matrix3 K_;
public:
EmptyCal(){}
///< shared pointer to calibration object
EmptyCal()
: K_(Matrix3::Identity()) {
}
/// Default destructor
virtual ~EmptyCal() = default;
using shared_ptr = boost::shared_ptr<EmptyCal>;
void print(const std::string& s) const {
std::cout << "empty calibration: " << s << std::endl;
}
Matrix3 K() const {return K_;}
};
//
//class GTSAM_EXPORT EmptyCal {
// public:
// EmptyCal(){}
// virtual ~EmptyCal() = default;
// using shared_ptr = boost::shared_ptr<EmptyCal>;
// void print(const std::string& s) const {
// std::cout << "empty calibration: " << s << std::endl;
// }
//};
/**
* A spherical camera class that has a Pose3 and measures bearing vectors
* @addtogroup geometry

1
gtsam/slam/SmartProjectionFactor.h
View File

@ -70,6 +70,7 @@ public:
typedef boost::shared_ptr<This> shared_ptr;
/// shorthand for a set of cameras
typedef CAMERA Camera;
typedef CameraSet<CAMERA> Cameras;
/**

1
gtsam/slam/SmartProjectionFactorP.h
View File

@ -60,7 +60,6 @@ class SmartProjectionFactorP : public SmartProjectionFactor<CAMERA> {
static const int ZDim = 2; ///< Measurement dimension (Point2)
protected:
/// vector of keys (one for each observation) with potentially repeated keys
std::vector<Key> nonUniqueKeys_;

1
gtsam/slam/tests/smartFactorScenarios.h
View File

@ -110,6 +110,7 @@ Camera cam2(pose_right, K);
Camera cam3(pose_above, K);
typedef GeneralSFMFactor<Camera, Point3> SFMFactor;
}
/* *************************************************************************/
// Cal3Bundler poses
namespace bundlerPose {

2
gtsam/slam/tests/testSmartProjectionFactorP.cpp
View File

@ -1107,7 +1107,7 @@ TEST( SmartProjectionFactorP, optimization_3poses_sphericalCamera ) {
emptyKs.push_back(emptyK);
SmartProjectionParams params;
params.setRankTolerance(0.01);
params.setRankTolerance(0.1);
SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model,params));
smartFactor1->add(measurements_lmk1, keys, emptyKs);

123
gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h
View File

@ -40,8 +40,16 @@ namespace gtsam {
template<class CAMERA>
class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAMERA> {
public:
private:
typedef SmartProjectionFactor<CAMERA> Base;
typedef SmartProjectionPoseFactorRollingShutter<CAMERA> This;
typedef typename CAMERA::CalibrationType CALIBRATION;
typedef typename CAMERA::Measurement MEASUREMENT;
typedef typename CAMERA::MeasurementVector MEASUREMENTS;
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)
protected:
/// shared pointer to calibration object (one for each observation)
@ -57,22 +65,17 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
std::vector<Pose3> body_P_sensors_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// shorthand for base class type
typedef SmartProjectionFactor<PinholePose<CALIBRATION> > Base;
/// shorthand for this class
typedef SmartProjectionPoseFactorRollingShutter This;
typedef CAMERA Camera;
typedef CameraSet<CAMERA> Cameras;
typedef Eigen::Matrix<double, ZDim, DimBlock> MatrixZD; // F blocks (derivatives wrt block of 2 poses)
typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks; // vector of F blocks
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
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)
typedef Eigen::Matrix<double, ZDim, DimBlock> MatrixZD; // F blocks (derivatives wrt block of 2 poses)
typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks; // vector of F blocks
/// Default constructor, only for serialization
SmartProjectionPoseFactorRollingShutter() {
}
/**
* Constructor
@ -83,6 +86,9 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
const SharedNoiseModel& sharedNoiseModel,
const SmartProjectionParams& params = SmartProjectionParams())
: Base(sharedNoiseModel, params) {
// use only configuration that works with this factor
Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY;
Base::params_.linearizationMode = gtsam::HESSIAN;
}
/** Virtual destructor */
@ -98,7 +104,7 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
* @param K (fixed) camera intrinsic calibration
* @param body_P_sensor (fixed) camera extrinsic calibration
*/
void add(const Point2& measured, const Key& world_P_body_key1,
void add(const MEASUREMENT& measured, const Key& world_P_body_key1,
const Key& world_P_body_key2, const double& alpha,
const boost::shared_ptr<CALIBRATION>& K, const Pose3 body_P_sensor = Pose3::identity()) {
// store measurements in base class
@ -134,7 +140,7 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
* @param Ks vector of (fixed) intrinsic calibration objects
* @param body_P_sensors vector of (fixed) extrinsic calibration objects
*/
void add(const Point2Vector& measurements,
void add(const MEASUREMENTS& measurements,
const std::vector<std::pair<Key, Key>>& world_P_body_key_pairs,
const std::vector<double>& alphas,
const std::vector<boost::shared_ptr<CALIBRATION>>& Ks,
@ -160,7 +166,7 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
* @param K (fixed) camera intrinsic calibration (same for all measurements)
* @param body_P_sensor (fixed) camera extrinsic calibration (same for all measurements)
*/
void add(const Point2Vector& measurements,
void add(const MEASUREMENTS& measurements,
const std::vector<std::pair<Key, Key>>& world_P_body_key_pairs,
const std::vector<double>& alphas,
const boost::shared_ptr<CALIBRATION>& K, const Pose3 body_P_sensor = Pose3::identity()) {
@ -244,6 +250,43 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
&& alphas_ == e->alphas() && keyPairsEqual && extrinsicCalibrationEqual;
}
/**
* Collect all cameras involved in this factor
* @param values Values structure which must contain camera poses
* corresponding to keys involved in this factor
* @return Cameras
*/
typename Base::Cameras cameras(const Values& values) const override {
size_t numViews = this->measured_.size();
assert(numViews == K_all_.size());
assert(numViews == alphas_.size());
assert(numViews == body_P_sensors_.size());
assert(numViews == world_P_body_key_pairs_.size());
typename Base::Cameras cameras;
for (size_t i = 0; i < numViews; i++) { // for each measurement
const Pose3& w_P_body1 = values.at<Pose3>(world_P_body_key_pairs_[i].first);
const Pose3& w_P_body2 = values.at<Pose3>(world_P_body_key_pairs_[i].second);
double interpolationFactor = alphas_[i];
const Pose3& w_P_body = interpolate<Pose3>(w_P_body1, w_P_body2, interpolationFactor);
const Pose3& body_P_cam = body_P_sensors_[i];
const Pose3& w_P_cam = w_P_body.compose(body_P_cam);
cameras.emplace_back(w_P_cam, K_all_[i]);
}
return cameras;
}
/**
* error calculates the error of the factor.
*/
double error(const Values& values) const override {
if (this->active(values)) {
return this->totalReprojectionError(this->cameras(values));
} else { // else of active flag
return 0.0;
}
}
/**
* Compute jacobian F, E and error vector at a given linearization point
* @param values Values structure which must contain camera poses
@ -274,12 +317,11 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
const Pose3& w_P_body = interpolate<Pose3>(w_P_body1, w_P_body2,interpolationFactor, dInterpPose_dPoseBody1, dInterpPose_dPoseBody2);
const Pose3& body_P_cam = body_P_sensors_[i];
const Pose3& w_P_cam = w_P_body.compose(body_P_cam, dPoseCam_dInterpPose);
PinholeCamera<CALIBRATION> camera(w_P_cam, *K_all_[i]);
typename Base::Camera camera(w_P_cam, K_all_[i]);
// get jacobians and error vector for current measurement
Point2 reprojectionError_i = Point2(
camera.project(*this->result_, dProject_dPoseCam, Ei)
- this->measured_.at(i));
Point2 reprojectionError_i = camera.reprojectionError(
*this->result_, this->measured_.at(i), dProject_dPoseCam, Ei);
Eigen::Matrix<double, ZDim, DimBlock> J; // 2 x 12
J.block(0, 0, ZDim, 6) = dProject_dPoseCam * dPoseCam_dInterpPose
* dInterpPose_dPoseBody1; // (2x6) * (6x6) * (6x6)
@ -340,7 +382,7 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
for (size_t i = 0; i < Fs.size(); i++)
Fs[i] = this->noiseModel_->Whiten(Fs[i]);
Matrix3 P = Base::Cameras::PointCov(E, lambda, diagonalDamping);
Matrix3 P = Cameras::PointCov(E, lambda, diagonalDamping);
// Collect all the key pairs: these are the keys that correspond to the blocks in Fs (on which we apply the Schur Complement)
KeyVector nonuniqueKeys;
@ -352,50 +394,13 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
// Build augmented Hessian (with last row/column being the information vector)
// Note: we need to get the augumented hessian wrt the unique keys in key_
SymmetricBlockMatrix augmentedHessianUniqueKeys =
Base::Cameras::SchurComplementAndRearrangeBlocks_3_12_6(
Cameras::SchurComplementAndRearrangeBlocks_3_12_6(
Fs, E, P, b, nonuniqueKeys, this->keys_);
return boost::make_shared < RegularHessianFactor<DimPose>
> (this->keys_, augmentedHessianUniqueKeys);
}
/**
* error calculates the error of the factor.
*/
double error(const Values& values) const override {
if (this->active(values)) {
return this->totalReprojectionError(this->cameras(values));
} else { // else of active flag
return 0.0;
}
}
/**
* Collect all cameras involved in this factor
* @param values Values structure which must contain camera poses
* corresponding to keys involved in this factor
* @return Cameras
*/
typename Base::Cameras cameras(const Values& values) const override {
size_t numViews = this->measured_.size();
assert(numViews == K_all_.size());
assert(numViews == alphas_.size());
assert(numViews == body_P_sensors_.size());
assert(numViews == world_P_body_key_pairs_.size());
typename Base::Cameras cameras;
for (size_t i = 0; i < numViews; i++) { // for each measurement
const Pose3& w_P_body1 = values.at<Pose3>(world_P_body_key_pairs_[i].first);
const Pose3& w_P_body2 = values.at<Pose3>(world_P_body_key_pairs_[i].second);
double interpolationFactor = alphas_[i];
const Pose3& w_P_body = interpolate<Pose3>(w_P_body1, w_P_body2, interpolationFactor);
const Pose3& body_P_cam = body_P_sensors_[i];
const Pose3& w_P_cam = w_P_body.compose(body_P_cam);
cameras.emplace_back(w_P_cam, K_all_[i]);
}
return cameras;
}
/**
* Linearize to Gaussian Factor (possibly adding a damping factor Lambda for LM)
* @param values Values structure which must contain camera poses and extrinsic pose for this factor

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

@ -19,6 +19,7 @@
#include "gtsam/slam/tests/smartFactorScenarios.h"
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/slam/PoseTranslationPrior.h>
#include <gtsam/geometry/SphericalCamera.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h>
#include <gtsam_unstable/slam/ProjectionFactorRollingShutter.h>
@ -72,6 +73,20 @@ Camera cam2(interp_pose2, sharedK);
Camera cam3(interp_pose3, sharedK);
}
/* ************************************************************************* */
// default Cal3_S2 poses with rolling shutter effect
namespace sphericalCameraRS {
typedef SphericalCamera Camera;
typedef SmartProjectionPoseFactorRollingShutter<Camera> SmartFactorRS_spherical;
Pose3 interp_pose1 = interpolate<Pose3>(level_pose,pose_right,interp_factor1);
Pose3 interp_pose2 = interpolate<Pose3>(pose_right,pose_above,interp_factor2);
Pose3 interp_pose3 = interpolate<Pose3>(pose_above,level_pose,interp_factor3);
static EmptyCal::shared_ptr emptyK;
Camera cam1(interp_pose1, emptyK);
Camera cam2(interp_pose2, emptyK);
Camera cam3(interp_pose3, emptyK);
}
LevenbergMarquardtParams lmParams;
typedef SmartProjectionPoseFactorRollingShutter< PinholePose<Cal3_S2> > SmartFactorRS;
@ -1040,6 +1055,79 @@ TEST( SmartProjectionPoseFactorRollingShutter, timing ) {
}
#endif
/* *************************************************************************/
TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_sphericalCameras ) {
using namespace sphericalCameraRS;
std::vector<Unit3> measurements_lmk1, measurements_lmk2, measurements_lmk3;
// Project three landmarks into three cameras
projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark1, measurements_lmk1);
projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark2, measurements_lmk2);
projectToMultipleCameras<Camera>(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);
SmartProjectionParams params;
params.setRankTolerance(0.1);
SmartFactorRS_spherical::shared_ptr smartFactor1(
new SmartFactorRS_spherical(model,params));
smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, emptyK);
SmartFactorRS_spherical::shared_ptr smartFactor2(
new SmartFactorRS_spherical(model,params));
smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, emptyK);
SmartFactorRS_spherical::shared_ptr smartFactor3(
new SmartFactorRS_spherical(model,params));
smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, emptyK);
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-6));
}
/* ************************************************************************* */
int main() {
TestResult tr;