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

Merge branch 'develop' into feature/rot-print

release/4.3a0
Varun Agrawal 2020-07-23 20:05:25 -05:00
parent 61b1a9e88d 905be415fa
commit 71c5567f17
4 changed files with 14 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
CMakeLists.txt
View File

@ -10,7 +10,7 @@ endif()
# Set the version number for the library # Set the version number for the library
set (GTSAM_VERSION_MAJOR 4) set (GTSAM_VERSION_MAJOR 4)
set (GTSAM_VERSION_MINOR 0) set (GTSAM_VERSION_MINOR 0)
set (GTSAM_VERSION_PATCH 2) set (GTSAM_VERSION_PATCH 3)
math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}") math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}")
set (GTSAM_VERSION_STRING "${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}") set (GTSAM_VERSION_STRING "${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}")
@ -68,8 +68,8 @@ if(GTSAM_UNSTABLE_AVAILABLE)
endif() endif()
option(BUILD_SHARED_LIBS "Build shared gtsam library, instead of static" ON) option(BUILD_SHARED_LIBS "Build shared gtsam library, instead of static" ON)
option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF) option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF)
option(GTSAM_POSE3_EXPMAP "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." OFF) option(GTSAM_POSE3_EXPMAP "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." ON)
option(GTSAM_ROT3_EXPMAP "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." OFF) option(GTSAM_ROT3_EXPMAP "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." ON)
option(GTSAM_ENABLE_CONSISTENCY_CHECKS "Enable/Disable expensive consistency checks" OFF) option(GTSAM_ENABLE_CONSISTENCY_CHECKS "Enable/Disable expensive consistency checks" OFF)
option(GTSAM_WITH_TBB "Use Intel Threaded Building Blocks (TBB) if available" ON) option(GTSAM_WITH_TBB "Use Intel Threaded Building Blocks (TBB) if available" ON)
option(GTSAM_WITH_EIGEN_MKL "Eigen will use Intel MKL if available" OFF) option(GTSAM_WITH_EIGEN_MKL "Eigen will use Intel MKL if available" OFF)

2
appveyor.yml
View File

@ -1,5 +1,5 @@
# version format # version format
version: 4.0.2-{branch}-build{build} version: 4.0.3-{branch}-build{build}
os: Visual Studio 2019 os: Visual Studio 2019

4
gtsam/navigation/TangentPreintegration.cpp
View File

@ -68,7 +68,7 @@ Vector9 TangentPreintegration::UpdatePreintegrated(const Vector3& a_body,
Matrix3 w_tangent_H_theta, invH; Matrix3 w_tangent_H_theta, invH;
const Vector3 w_tangent = // angular velocity mapped back to tangent space const Vector3 w_tangent = // angular velocity mapped back to tangent space
local.applyInvDexp(w_body, A ? &w_tangent_H_theta : 0, C ? &invH : 0); local.applyInvDexp(w_body, A ? &w_tangent_H_theta : 0, C ? &invH : 0);
const Rot3 R(local.expmap()); const Rot3 R(local.expmap()); // nRb: rotation of body in nav frame
const Vector3 a_nav = R * a_body; const Vector3 a_nav = R * a_body;
const double dt22 = 0.5 * dt * dt; const double dt22 = 0.5 * dt * dt;
@ -110,7 +110,7 @@ void TangentPreintegration::update(const Vector3& measuredAcc,
Vector3 acc = biasHat_.correctAccelerometer(measuredAcc); Vector3 acc = biasHat_.correctAccelerometer(measuredAcc);
Vector3 omega = biasHat_.correctGyroscope(measuredOmega); Vector3 omega = biasHat_.correctGyroscope(measuredOmega);
// Possibly correct for sensor pose // Possibly correct for sensor pose by converting to body frame
Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega; Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega;
if (p().body_P_sensor) if (p().body_P_sensor)
boost::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega, boost::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega,

8
gtsam/nonlinear/tests/testAdaptAutoDiff.cpp
View File

@ -168,7 +168,11 @@ Camera camera(Pose3(Rot3().retract(Vector3(0.1, 0.2, 0.3)), Point3(0, 5, 0)),
Point3 point(10, 0, -5); // negative Z-axis convention of Snavely! Point3 point(10, 0, -5); // negative Z-axis convention of Snavely!
Vector9 P = Camera().localCoordinates(camera); Vector9 P = Camera().localCoordinates(camera);
Vector3 X = point; Vector3 X = point;
#ifdef GTSAM_POSE3_EXPMAP
Vector2 expectedMeasurement(1.3124675, 1.2057287);
#else
Vector2 expectedMeasurement(1.2431567, 1.2525694); Vector2 expectedMeasurement(1.2431567, 1.2525694);
#endif
Matrix E1 = numericalDerivative21<Vector2, Vector9, Vector3>(adapted, P, X); Matrix E1 = numericalDerivative21<Vector2, Vector9, Vector3>(adapted, P, X);
Matrix E2 = numericalDerivative22<Vector2, Vector9, Vector3>(adapted, P, X); Matrix E2 = numericalDerivative22<Vector2, Vector9, Vector3>(adapted, P, X);
} }
@ -177,7 +181,11 @@ Matrix E2 = numericalDerivative22<Vector2, Vector9, Vector3>(adapted, P, X);
// Check that Local worked as expected // Check that Local worked as expected
TEST(AdaptAutoDiff, Local) { TEST(AdaptAutoDiff, Local) {
using namespace example; using namespace example;
#ifdef GTSAM_POSE3_EXPMAP
Vector9 expectedP = (Vector9() << 0.1, 0.2, 0.3, 0.7583528428, 4.9582357859, -0.224941471539, 1, 0, 0).finished();
#else
Vector9 expectedP = (Vector9() << 0.1, 0.2, 0.3, 0, 5, 0, 1, 0, 0).finished(); Vector9 expectedP = (Vector9() << 0.1, 0.2, 0.3, 0, 5, 0, 1, 0, 0).finished();
#endif
EXPECT(equal_with_abs_tol(expectedP, P)); EXPECT(equal_with_abs_tol(expectedP, P));
Vector3 expectedX(10, 0, -5); // negative Z-axis convention of Snavely! Vector3 expectedX(10, 0, -5); // negative Z-axis convention of Snavely!
EXPECT(equal_with_abs_tol(expectedX, X)); EXPECT(equal_with_abs_tol(expectedX, X));