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

yay! error test passes!

release/4.3a0
lcarlone 2021-07-21 15:10:10 -04:00
parent 6f8d639ab8
commit 5d55e153f0
2 changed files with 165 additions and 175 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

71
gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h
View File

@ -238,40 +238,6 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<
&& gammas_ == e->getGammas() && keyPairsEqual && extrinsicCalibrationEqual;
}
/**
* error calculates the error of the factor.
*/
double error(const Values& values) const override {
if (this->active(values)) {
return this->totalReprojectionError(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 {
assert(world_P_body_keys_.size() == K_all_.size());
assert(world_P_body_keys_.size() == body_P_cam_keys_.size());
typename Base::Cameras cameras;
for (size_t i = 0; i < world_P_body_key_pairs_.size(); i++) {
Pose3 w_P_body1 = values.at<Pose3>(world_P_body_key_pairs_[i].first);
Pose3 w_P_body2 = values.at<Pose3>(world_P_body_key_pairs_[i].second);
double interpolationFactor = gammas_[i];
// get interpolated pose:
Pose3 w_P_body = w_P_body1.interpolateRt(w_P_body2, interpolationFactor);
Pose3 body_P_cam = body_P_sensors_[i];
Pose3 w_P_cam = w_P_body.compose(body_P_cam);
cameras.emplace_back(w_P_cam, K_all_[i]);
}
return cameras;
}
/**
* Compute jacobian F, E and error vector at a given linearization point
* @param values Values structure which must contain camera poses
@ -447,6 +413,43 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<
// > ( 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(world_P_body_keys_.size() == K_all_.size());
assert(world_P_body_keys_.size() == body_P_cam_keys_.size());
typename Base::Cameras cameras;
for (size_t i = 0; i < numViews; i++) { // for each measurement
Pose3 w_P_body1 = values.at<Pose3>(world_P_body_key_pairs_[i].first);
Pose3 w_P_body2 = values.at<Pose3>(world_P_body_key_pairs_[i].second);
double interpolationFactor = gammas_[i];
Pose3 w_P_body = interpolate<Pose3>(w_P_body1, w_P_body2, interpolationFactor);
Pose3 body_P_cam = body_P_sensors_[i];
Pose3 w_P_cam = w_P_body.compose(body_P_cam);
std::cout << "id : " << i << std::endl;
w_P_cam.print("w_P_cam\n");
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

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

@ -60,7 +60,6 @@ static double interp_factor3 = 0.5;
// default Cal3_S2 poses with rolling shutter effect
namespace vanillaPoseRS {
typedef PinholePose<Cal3_S2> Camera;
typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h));
Pose3 interp_pose1 = interpolate<Pose3>(level_pose,pose_right,interp_factor1);
Pose3 interp_pose2 = interpolate<Pose3>(pose_right,pose_above,interp_factor2);
@ -170,38 +169,26 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) {
/* *************************************************************************/
TEST( SmartProjectionPoseFactorRollingShutter, noiselessError ) {
std::cout << "============================== " << std::endl;
using namespace vanillaPoseRS;
// // 2 poses such that level_pose_1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
// // can be interpolated with interp_factor1 = 0.2:
// Pose3 level_pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 5));
// Pose3 level_pose2 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0));
// // 2 poses such that pose_right (Second camera 1 meter to the right of first camera)
// // can be interpolated with interp_factor1 = 0.4:
// Pose3 pose_right1 = level_pose * Pose3(Rot3(), Point3(1, 0, 0));
// Pose3 pose_right2 = level_pose * Pose3(Rot3(), Point3(1, 0, 0));
// // 2 poses such that pose_above (Third camera 1 meter above the first camera)
// // can be interpolated with interp_factor1 = 0.5:
// Pose3 pose_above1 = level_pose * Pose3(Rot3(), Point3(0, -1, 0));
// Pose3 pose_above1 = level_pose * Pose3(Rot3(), Point3(0, -1, 0));
//
// // Project two landmarks into two cameras
// Point2 level_uv = level_camera.project(landmark1);
// Point2 level_uv_right = level_camera_right.project(landmark1);
// Pose3 body_P_sensorId = Pose3::identity();
//
// SmartFactor factor(model);
// factor.add(level_uv, x1, x2, interp_factor1, sharedK, body_P_sensorId);
// factor.add(level_uv_right, x2, x3, interp_factor2, sharedK, body_P_sensorId);
//
// Values values; // it's a pose factor, hence these are poses
// values.insert(x1, cam1.pose());
// values.insert(x2, cam2.pose());
//
// double actualError = factor.error(values);
// double expectedError = 0.0;
// EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7);
// Project two landmarks into two cameras
Point2 level_uv = cam1.project(landmark1);
Point2 level_uv_right = cam2.project(landmark1);
Pose3 body_P_sensorId = Pose3::identity();
SmartFactorRS factor(model);
factor.add(level_uv, x1, x2, interp_factor1, sharedK, body_P_sensorId);
factor.add(level_uv_right, x2, x3, interp_factor2, sharedK, body_P_sensorId);
Values values; // it's a pose factor, hence these are poses
values.insert(x1, level_pose);
values.insert(x2, pose_right);
values.insert(x3, pose_above);
double actualError = factor.error(values);
double expectedError = 0.0;
EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7);
}
/* *************************************************************************