Merge branch 'develop' into wrap/pybind-stl
commit
f58ee917f0
|
@ -21,24 +21,15 @@ jobs:
|
|||
# Github Actions requires a single row to be added to the build matrix.
|
||||
# See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
|
||||
name: [
|
||||
#TODO This build fails, need to understand why.
|
||||
# windows-2016-cl,
|
||||
windows-2019-cl,
|
||||
]
|
||||
|
||||
build_type: [
|
||||
Debug,
|
||||
#TODO(Varun) The release build takes over 2.5 hours, need to figure out why.
|
||||
# Release
|
||||
Release
|
||||
]
|
||||
build_unstable: [ON]
|
||||
include:
|
||||
#TODO This build fails, need to understand why.
|
||||
# - name: windows-2016-cl
|
||||
# os: windows-2016
|
||||
# compiler: cl
|
||||
# platform: 32
|
||||
|
||||
- name: windows-2019-cl
|
||||
os: windows-2019
|
||||
compiler: cl
|
||||
|
@ -125,4 +116,3 @@ jobs:
|
|||
|
||||
# Run GTSAM_UNSTABLE tests
|
||||
#cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.base_unstable
|
||||
|
||||
|
|
|
@ -258,10 +258,19 @@ public:
|
|||
inline const Rot2& r() const { return r_; }
|
||||
|
||||
/// translation
|
||||
inline const Point2& translation() const { return t_; }
|
||||
inline const Point2& translation(OptionalJacobian<2, 3> Hself={}) const {
|
||||
if (Hself) {
|
||||
*Hself = Matrix::Zero(2, 3);
|
||||
(*Hself).block<2, 2>(0, 0) = rotation().matrix();
|
||||
}
|
||||
return t_;
|
||||
}
|
||||
|
||||
/// rotation
|
||||
inline const Rot2& rotation() const { return r_; }
|
||||
inline const Rot2& rotation(OptionalJacobian<1, 3> Hself={}) const {
|
||||
if (Hself) *Hself << 0, 0, 1;
|
||||
return r_;
|
||||
}
|
||||
|
||||
//// return transformation matrix
|
||||
GTSAM_EXPORT Matrix3 matrix() const;
|
||||
|
|
|
@ -434,7 +434,9 @@ class Pose2 {
|
|||
gtsam::Rot2 bearing(const gtsam::Point2& point) const;
|
||||
double range(const gtsam::Point2& point) const;
|
||||
gtsam::Point2 translation() const;
|
||||
gtsam::Point2 translation(Eigen::Ref<Eigen::MatrixXd> Hself) const;
|
||||
gtsam::Rot2 rotation() const;
|
||||
gtsam::Rot2 rotation(Eigen::Ref<Eigen::MatrixXd> Hself) const;
|
||||
Matrix matrix() const;
|
||||
|
||||
// enabling serialization functionality
|
||||
|
|
|
@ -474,6 +474,33 @@ TEST( Pose2, compose_matrix )
|
|||
EXPECT(assert_equal(gM1*_1M2,matrix(gT1.compose(_1T2)))); // RIGHT DOES NOT
|
||||
}
|
||||
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Pose2, translation ) {
|
||||
Pose2 pose(3.5, -8.2, 4.2);
|
||||
|
||||
Matrix actualH;
|
||||
EXPECT(assert_equal((Vector2() << 3.5, -8.2).finished(), pose.translation(actualH), 1e-8));
|
||||
|
||||
std::function<Point2(const Pose2&)> f = [](const Pose2& T) { return T.translation(); };
|
||||
Matrix numericalH = numericalDerivative11<Point2, Pose2>(f, pose);
|
||||
EXPECT(assert_equal(numericalH, actualH, 1e-6));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Pose2, rotation ) {
|
||||
Pose2 pose(3.5, -8.2, 4.2);
|
||||
|
||||
Matrix actualH(4, 3);
|
||||
EXPECT(assert_equal(Rot2(4.2), pose.rotation(actualH), 1e-8));
|
||||
|
||||
std::function<Rot2(const Pose2&)> f = [](const Pose2& T) { return T.rotation(); };
|
||||
Matrix numericalH = numericalDerivative11<Rot2, Pose2>(f, pose);
|
||||
EXPECT(assert_equal(numericalH, actualH, 1e-6));
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Pose2, between )
|
||||
{
|
||||
|
|
|
@ -85,7 +85,8 @@ Vector4 triangulateHomogeneousDLT(
|
|||
|
||||
Point3 triangulateLOST(const std::vector<Pose3>& poses,
|
||||
const Point3Vector& calibratedMeasurements,
|
||||
const SharedIsotropic& measurementNoise) {
|
||||
const SharedIsotropic& measurementNoise,
|
||||
double rank_tol) {
|
||||
size_t m = calibratedMeasurements.size();
|
||||
assert(m == poses.size());
|
||||
|
||||
|
@ -96,17 +97,38 @@ Point3 triangulateLOST(const std::vector<Pose3>& poses,
|
|||
for (size_t i = 0; i < m; i++) {
|
||||
const Pose3& wTi = poses[i];
|
||||
// TODO(akshay-krishnan): are there better ways to select j?
|
||||
const int j = (i + 1) % m;
|
||||
int j = (i + 1) % m;
|
||||
const Pose3& wTj = poses[j];
|
||||
|
||||
const Point3 d_ij = wTj.translation() - wTi.translation();
|
||||
Point3 d_ij = wTj.translation() - wTi.translation();
|
||||
Point3 wZi = wTi.rotation().rotate(calibratedMeasurements[i]);
|
||||
Point3 wZj = wTj.rotation().rotate(calibratedMeasurements[j]);
|
||||
double num_i = wZi.cross(wZj).norm();
|
||||
double den_i = d_ij.cross(wZj).norm();
|
||||
|
||||
const Point3 wZi = wTi.rotation().rotate(calibratedMeasurements[i]);
|
||||
const Point3 wZj = wTj.rotation().rotate(calibratedMeasurements[j]);
|
||||
// Handle q_i = 0 (or NaN), which arises if the measurement vectors, wZi and
|
||||
// wZj, coincide (or the baseline vector coincides with the jth measurement
|
||||
// vector).
|
||||
if (num_i == 0 || den_i == 0) {
|
||||
bool success = false;
|
||||
for (size_t k = 2; k < m; k++) {
|
||||
j = (i + k) % m;
|
||||
const Pose3& wTj = poses[j];
|
||||
|
||||
d_ij = wTj.translation() - wTi.translation();
|
||||
wZj = wTj.rotation().rotate(calibratedMeasurements[j]);
|
||||
num_i = wZi.cross(wZj).norm();
|
||||
den_i = d_ij.cross(wZj).norm();
|
||||
if (num_i > 0 && den_i > 0) {
|
||||
success = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!success) throw(TriangulationUnderconstrainedException());
|
||||
}
|
||||
|
||||
// Note: Setting q_i = 1.0 gives same results as DLT.
|
||||
const double q_i = wZi.cross(wZj).norm() /
|
||||
(measurementNoise->sigma() * d_ij.cross(wZj).norm());
|
||||
const double q_i = num_i / (measurementNoise->sigma() * den_i);
|
||||
|
||||
const Matrix23 coefficientMat =
|
||||
q_i * skewSymmetric(calibratedMeasurements[i]).topLeftCorner(2, 3) *
|
||||
|
@ -115,7 +137,13 @@ Point3 triangulateLOST(const std::vector<Pose3>& poses,
|
|||
A.block<2, 3>(2 * i, 0) << coefficientMat;
|
||||
b.block<2, 1>(2 * i, 0) << coefficientMat * wTi.translation();
|
||||
}
|
||||
return A.colPivHouseholderQr().solve(b);
|
||||
|
||||
Eigen::ColPivHouseholderQR<Matrix> A_Qr = A.colPivHouseholderQr();
|
||||
A_Qr.setThreshold(rank_tol);
|
||||
|
||||
if (A_Qr.rank() < 3) throw(TriangulationUnderconstrainedException());
|
||||
|
||||
return A_Qr.solve(b);
|
||||
}
|
||||
|
||||
Point3 triangulateDLT(
|
||||
|
|
|
@ -110,7 +110,8 @@ GTSAM_EXPORT Point3 triangulateDLT(
|
|||
*/
|
||||
GTSAM_EXPORT Point3 triangulateLOST(const std::vector<Pose3>& poses,
|
||||
const Point3Vector& calibratedMeasurements,
|
||||
const SharedIsotropic& measurementNoise);
|
||||
const SharedIsotropic& measurementNoise,
|
||||
double rank_tol = 1e-9);
|
||||
|
||||
/**
|
||||
* Create a factor graph with projection factors from poses and one calibration
|
||||
|
@ -439,7 +440,8 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
|
|||
auto calibratedMeasurements =
|
||||
calibrateMeasurementsShared<CALIBRATION>(*sharedCal, measurements);
|
||||
|
||||
point = triangulateLOST(poses, calibratedMeasurements, measurementNoise);
|
||||
point = triangulateLOST(poses, calibratedMeasurements, measurementNoise,
|
||||
rank_tol);
|
||||
} else {
|
||||
// construct projection matrices from poses & calibration
|
||||
auto projection_matrices = projectionMatricesFromPoses(poses, sharedCal);
|
||||
|
@ -512,7 +514,8 @@ Point3 triangulatePoint3(const CameraSet<CAMERA>& cameras,
|
|||
auto calibratedMeasurements =
|
||||
calibrateMeasurements<CAMERA>(cameras, measurements);
|
||||
|
||||
point = triangulateLOST(poses, calibratedMeasurements, measurementNoise);
|
||||
point = triangulateLOST(poses, calibratedMeasurements, measurementNoise,
|
||||
rank_tol);
|
||||
} else {
|
||||
// construct projection matrices from poses & calibration
|
||||
auto projection_matrices = projectionMatricesFromCameras(cameras);
|
||||
|
@ -750,4 +753,3 @@ using CameraSetCal3Fisheye = CameraSet<PinholeCamera<Cal3Fisheye>>;
|
|||
using CameraSetCal3Unified = CameraSet<PinholeCamera<Cal3Unified>>;
|
||||
using CameraSetSpherical = CameraSet<SphericalCamera>;
|
||||
} // \namespace gtsam
|
||||
|
||||
|
|
Loading…
Reference in New Issue