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

all set here!

release/4.3a0
lcarlone 2021-10-05 23:29:20 -04:00
parent f0745a9c28
commit 737dcf65e4
2 changed files with 94 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h
View File

@ -49,9 +49,6 @@ class SmartProjectionPoseFactorRollingShutter
typedef typename CAMERA::CalibrationType CALIBRATION;
protected:
/// shared pointer to calibration object (one for each observation)
std::vector<boost::shared_ptr<CALIBRATION>> K_all_;
/// The keys of the pose of the body (with respect to an external world
/// frame): two consecutive poses for each observation
std::vector<std::pair<Key, Key>> world_P_body_key_pairs_;
@ -60,10 +57,10 @@ class SmartProjectionPoseFactorRollingShutter
/// pair of consecutive poses
std::vector<double> alphas_;
/// one or more cameras in the rig (fixed poses wrt body + fixed intrinsics)
/// one or more cameras taking observations (fixed poses wrt body + fixed intrinsics)
typename Base::Cameras cameraRig_;
/// vector of camera Ids (one for each observation, in the same order), identifying which camera in the rig took the measurement
/// vector of camera Ids (one for each observation, in the same order), identifying which camera took the measurement
FastVector<size_t> cameraIds_;
public:
@ -87,6 +84,7 @@ class SmartProjectionPoseFactorRollingShutter
/**
* Constructor
* @param Isotropic measurement noise
* @param cameraRig set of cameras (fixed poses wrt body and intrinsics) taking the measurements
* @param params internal parameters of the smart factors
*/
SmartProjectionPoseFactorRollingShutter(
@ -102,6 +100,7 @@ class SmartProjectionPoseFactorRollingShutter
/**
* Constructor
* @param Isotropic measurement noise
* @param camera single camera (fixed poses wrt body and intrinsics)
* @param params internal parameters of the smart factors
*/
SmartProjectionPoseFactorRollingShutter(
@ -114,13 +113,11 @@ class SmartProjectionPoseFactorRollingShutter
cameraRig_.push_back(camera);
}
/** Virtual destructor */
~SmartProjectionPoseFactorRollingShutter() override = default;
/**
* add a new measurement, with 2 pose keys, interpolation factor, camera
* (intrinsic and extrinsic) calibration, and observed pixel.
* add a new measurement, with 2 pose keys, interpolation factor, and cameraId
* @param measured 2-dimensional location of the projection of a single
* landmark in a single view (the measurement), interpolated from the 2 poses
* @param world_P_body_key1 key corresponding to the first body poses (time <=
@ -129,8 +126,7 @@ class SmartProjectionPoseFactorRollingShutter
* >= time pixel is acquired)
* @param alpha interpolation factor in [0,1], such that if alpha = 0 the
* interpolated pose is the same as world_P_body_key1
* @param K (fixed) camera intrinsic calibration
* @param body_P_sensor (fixed) camera extrinsic calibration
* @param cameraId ID of the camera taking the measurement (default 0)
*/
void add(const Point2& measured, const Key& world_P_body_key1,
const Key& world_P_body_key2, const double& alpha,
@ -168,8 +164,7 @@ class SmartProjectionPoseFactorRollingShutter
* for the i0-th measurement can be interpolated
* @param alphas vector of interpolation params (in [0,1]), one for each
* measurement (in the same order)
* @param Ks vector of (fixed) intrinsic calibration objects
* @param body_P_sensors vector of (fixed) extrinsic calibration objects
* @param cameraIds IDs of the cameras taking each measurement (same order as the measurements)
*/
void add(const Point2Vector& measurements,
const std::vector<std::pair<Key, Key>>& world_P_body_key_pairs,
@ -223,7 +218,7 @@ class SmartProjectionPoseFactorRollingShutter
const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
std::cout << s << "SmartProjectionPoseFactorRollingShutter: \n ";
for (size_t i = 0; i < K_all_.size(); i++) {
for (size_t i = 0; i < cameraIds_.size(); i++) {
std::cout << "-- Measurement nr " << i << std::endl;
std::cout << " pose1 key: "
<< keyFormatter(world_P_body_key_pairs_[i].first) << std::endl;
@ -409,14 +404,8 @@ class SmartProjectionPoseFactorRollingShutter
* @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
for (size_t i = 0; i < this->measured_.size(); i++) { // for each measurement
const Pose3& w_P_body1 =
values.at<Pose3>(world_P_body_key_pairs_[i].first);
const Pose3& w_P_body2 =

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

@ -496,6 +496,91 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam) {
EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6));
}
/* *************************************************************************/
TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam2) {
using namespace vanillaPoseRS;
Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3;
// create arbitrary body_T_sensor (transforms from sensor to body)
Pose3 body_T_sensor1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2),
Point3(1, 1, 1));
Pose3 body_T_sensor2 = Pose3(Rot3::Ypr(-M_PI / 5, 0., -M_PI / 2),
Point3(0, 0, 1));
Pose3 body_T_sensor3 = Pose3::identity();
Camera camera1(interp_pose1*body_T_sensor1, sharedK);
Camera camera2(interp_pose2*body_T_sensor2, sharedK);
Camera camera3(interp_pose3*body_T_sensor3, sharedK);
// Project three landmarks into three cameras
projectToMultipleCameras(camera1, camera2, camera3, landmark1, measurements_lmk1);
projectToMultipleCameras(camera1, camera2, camera3, landmark2, measurements_lmk2);
projectToMultipleCameras(camera1, camera2, camera3, 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);
Cameras cameraRig;
cameraRig.push_back(Camera(body_T_sensor1, sharedK));
cameraRig.push_back(Camera(body_T_sensor2, sharedK));
cameraRig.push_back(Camera(body_T_sensor3, sharedK));
SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, cameraRig));
smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, {0,1,2});
SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, cameraRig));
smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, {0,1,2});
SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, cameraRig));
smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, {0,1,2});
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); // pose above is the pose of the camera
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-4));
}
/* *************************************************************************/
TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) {
// here we replicate a test in SmartProjectionPoseFactor by setting