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

275
gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h
View File

@ -39,7 +39,7 @@ namespace gtsam {
*/
template<class CALIBRATION>
class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<
PinholePose<CALIBRATION> > {
PinholePose<CALIBRATION> > {
protected:
/// shared pointer to calibration object (one for each observation)
@ -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
@ -344,107 +310,144 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<
throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: "
"measured_.size() inconsistent with input");
// // triangulate 3D point at given linearization point
// triangulateSafe(cameras(values));
//
// if (!this->result_) { // failed: return "empty/zero" Hessian
// for (Matrix& m : Gs)
// m = Matrix::Zero(DimPose, DimPose);
// for (Vector& v : gs)
// v = Vector::Zero(DimPose);
// return boost::make_shared < RegularHessianFactor<DimPose>
// > ( this->keys_, Gs, gs, 0.0);
// }
//
// // compute Jacobian given triangulated 3D Point
// FBlocks Fs;
// Matrix F, E;
// Vector b;
// computeJacobiansWithTriangulatedPoint(Fs, E, b, values);
//
// // Whiten using noise model
// this->noiseModel_->WhitenSystem(E, b);
// for (size_t i = 0; i < Fs.size(); i++)
// Fs[i] = this->noiseModel_->Whiten(Fs[i]);
//
// // build augmented Hessian (with last row/column being the information vector)
// Matrix3 P;
// This::Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping);
//
// // marginalize point: note - we reuse the standard SchurComplement function
// SymmetricBlockMatrix augmentedHessian = This::Cameras::SchurComplement<2,DimBlock>(Fs, E, P, b);
// // triangulate 3D point at given linearization point
// triangulateSafe(cameras(values));
//
// if (!this->result_) { // failed: return "empty/zero" Hessian
// for (Matrix& m : Gs)
// m = Matrix::Zero(DimPose, DimPose);
// for (Vector& v : gs)
// v = Vector::Zero(DimPose);
// return boost::make_shared < RegularHessianFactor<DimPose>
// > ( this->keys_, Gs, gs, 0.0);
// }
//
// // compute Jacobian given triangulated 3D Point
// FBlocks Fs;
// Matrix F, E;
// Vector b;
// computeJacobiansWithTriangulatedPoint(Fs, E, b, values);
//
// // Whiten using noise model
// this->noiseModel_->WhitenSystem(E, b);
// for (size_t i = 0; i < Fs.size(); i++)
// Fs[i] = this->noiseModel_->Whiten(Fs[i]);
//
// // build augmented Hessian (with last row/column being the information vector)
// Matrix3 P;
// This::Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping);
//
// // marginalize point: note - we reuse the standard SchurComplement function
// SymmetricBlockMatrix augmentedHessian = This::Cameras::SchurComplement<2,DimBlock>(Fs, E, P, b);
// // now pack into an Hessian factor
// std::vector<DenseIndex> dims(nrUniqueKeys + 1); // this also includes the b term
// std::fill(dims.begin(), dims.end() - 1, 6);
// dims.back() = 1;
// SymmetricBlockMatrix augmentedHessianUniqueKeys;
//
// // here we have to deal with the fact that some cameras may share the same extrinsic key
// if (nrUniqueKeys == nrNonuniqueKeys) { // if there is 1 calibration key per camera
// augmentedHessianUniqueKeys = SymmetricBlockMatrix(
// dims, Matrix(augmentedHessian.selfadjointView()));
// } else { // if multiple cameras share a calibration we have to rearrange
// // the results of the Schur complement matrix
// std::vector<DenseIndex> nonuniqueDims(nrNonuniqueKeys + 1); // this also includes the b term
// std::fill(nonuniqueDims.begin(), nonuniqueDims.end() - 1, 6);
// nonuniqueDims.back() = 1;
// augmentedHessian = SymmetricBlockMatrix(
// nonuniqueDims, Matrix(augmentedHessian.selfadjointView()));
//
// // these are the keys that correspond to the blocks in augmentedHessian (output of SchurComplement)
// KeyVector nonuniqueKeys;
// for (size_t i = 0; i < world_P_body_key_pairs_.size(); i++) {
// nonuniqueKeys.push_back(world_P_body_key_pairs_.at(i));
// nonuniqueKeys.push_back(body_P_cam_ this->keys_.at(i));
// }
//
// // get map from key to location in the new augmented Hessian matrix (the one including only unique keys)
// std::map<Key, size_t> keyToSlotMap;
// for (size_t k = 0; k < nrUniqueKeys; k++) {
// keyToSlotMap[ this->keys_[k]] = k;
// }
//
// // initialize matrix to zero
// augmentedHessianUniqueKeys = SymmetricBlockMatrix(
// dims, Matrix::Zero(6 * nrUniqueKeys + 1, 6 * nrUniqueKeys + 1));
//
// // add contributions for each key: note this loops over the hessian with nonUnique keys (augmentedHessian)
// // and populates an Hessian that only includes the unique keys (that is what we want to return)
// for (size_t i = 0; i < nrNonuniqueKeys; i++) { // rows
// Key key_i = nonuniqueKeys.at(i);
//
// // update information vector
// augmentedHessianUniqueKeys.updateOffDiagonalBlock(
// keyToSlotMap[key_i], nrUniqueKeys,
// augmentedHessian.aboveDiagonalBlock(i, nrNonuniqueKeys));
//
// // update blocks
// for (size_t j = i; j < nrNonuniqueKeys; j++) { // cols
// Key key_j = nonuniqueKeys.at(j);
// if (i == j) {
// augmentedHessianUniqueKeys.updateDiagonalBlock(
// keyToSlotMap[key_i], augmentedHessian.diagonalBlock(i));
// } else { // (i < j)
// if (keyToSlotMap[key_i] != keyToSlotMap[key_j]) {
// augmentedHessianUniqueKeys.updateOffDiagonalBlock(
// keyToSlotMap[key_i], keyToSlotMap[key_j],
// augmentedHessian.aboveDiagonalBlock(i, j));
// } else {
// augmentedHessianUniqueKeys.updateDiagonalBlock(
// keyToSlotMap[key_i],
// augmentedHessian.aboveDiagonalBlock(i, j)
// + augmentedHessian.aboveDiagonalBlock(i, j).transpose());
// }
// }
// }
// }
// // update bottom right element of the matrix
// augmentedHessianUniqueKeys.updateDiagonalBlock(
// nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys));
// }
// return boost::make_shared < RegularHessianFactor<DimPose>
// > ( this->keys_, augmentedHessianUniqueKeys);
// // now pack into an Hessian factor
// std::vector<DenseIndex> dims(nrUniqueKeys + 1); // this also includes the b term
// std::fill(dims.begin(), dims.end() - 1, 6);
// dims.back() = 1;
// SymmetricBlockMatrix augmentedHessianUniqueKeys;
//
// // here we have to deal with the fact that some cameras may share the same extrinsic key
// if (nrUniqueKeys == nrNonuniqueKeys) { // if there is 1 calibration key per camera
// augmentedHessianUniqueKeys = SymmetricBlockMatrix(
// dims, Matrix(augmentedHessian.selfadjointView()));
// } else { // if multiple cameras share a calibration we have to rearrange
// // the results of the Schur complement matrix
// std::vector<DenseIndex> nonuniqueDims(nrNonuniqueKeys + 1); // this also includes the b term
// std::fill(nonuniqueDims.begin(), nonuniqueDims.end() - 1, 6);
// nonuniqueDims.back() = 1;
// augmentedHessian = SymmetricBlockMatrix(
// nonuniqueDims, Matrix(augmentedHessian.selfadjointView()));
//
// // these are the keys that correspond to the blocks in augmentedHessian (output of SchurComplement)
// KeyVector nonuniqueKeys;
// for (size_t i = 0; i < world_P_body_key_pairs_.size(); i++) {
// nonuniqueKeys.push_back(world_P_body_key_pairs_.at(i));
// nonuniqueKeys.push_back(body_P_cam_ this->keys_.at(i));
// }
//
// // get map from key to location in the new augmented Hessian matrix (the one including only unique keys)
// std::map<Key, size_t> keyToSlotMap;
// for (size_t k = 0; k < nrUniqueKeys; k++) {
// keyToSlotMap[ this->keys_[k]] = k;
// }
//
// // initialize matrix to zero
// augmentedHessianUniqueKeys = SymmetricBlockMatrix(
// dims, Matrix::Zero(6 * nrUniqueKeys + 1, 6 * nrUniqueKeys + 1));
//
// // add contributions for each key: note this loops over the hessian with nonUnique keys (augmentedHessian)
// // and populates an Hessian that only includes the unique keys (that is what we want to return)
// for (size_t i = 0; i < nrNonuniqueKeys; i++) { // rows
// Key key_i = nonuniqueKeys.at(i);
//
// // update information vector
// augmentedHessianUniqueKeys.updateOffDiagonalBlock(
// keyToSlotMap[key_i], nrUniqueKeys,
// augmentedHessian.aboveDiagonalBlock(i, nrNonuniqueKeys));
//
// // update blocks
// for (size_t j = i; j < nrNonuniqueKeys; j++) { // cols
// Key key_j = nonuniqueKeys.at(j);
// if (i == j) {
// augmentedHessianUniqueKeys.updateDiagonalBlock(
// keyToSlotMap[key_i], augmentedHessian.diagonalBlock(i));
// } else { // (i < j)
// if (keyToSlotMap[key_i] != keyToSlotMap[key_j]) {
// augmentedHessianUniqueKeys.updateOffDiagonalBlock(
// keyToSlotMap[key_i], keyToSlotMap[key_j],
// augmentedHessian.aboveDiagonalBlock(i, j));
// } else {
// augmentedHessianUniqueKeys.updateDiagonalBlock(
// keyToSlotMap[key_i],
// augmentedHessian.aboveDiagonalBlock(i, j)
// + augmentedHessian.aboveDiagonalBlock(i, j).transpose());
// }
// }
// }
// }
// // update bottom right element of the matrix
// augmentedHessianUniqueKeys.updateDiagonalBlock(
// nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys));
// }
// 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(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;
}
/**
@ -485,7 +488,7 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<
/// traits
template<class CALIBRATION>
struct traits<SmartProjectionPoseFactorRollingShutter<CALIBRATION> > :
public Testable<SmartProjectionPoseFactorRollingShutter<CALIBRATION> > {
public Testable<SmartProjectionPoseFactorRollingShutter<CALIBRATION> > {
};
} // namespace gtsam

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);
}
/* *************************************************************************