We read every piece of feedback, and take your input very seriously.
To see all available qualifiers, see our documentation.
There was an error while loading. Please reload this page.
1 parent 9712acd commit 286107bCopy full SHA for 286107b
gtsam/geometry/SO3.cpp
@@ -261,7 +261,6 @@ Matrix3 SO3::Hat(const Vector3& xi) {
261
262
//******************************************************************************
263
template <>
264
-GTSAM_EXPORT
265
Vector3 SO3::Vee(const Matrix3& X) {
266
Vector3 xi;
267
xi(0) = -X(1, 2);
gtsam/geometry/SO4.cpp
@@ -135,27 +135,6 @@ SO4 SO4::Expmap(const Vector6& xi, ChartJacobian H) {
135
}
136
137
138
-template <>
139
140
-SO4::VectorN2 SO4::vec(OptionalJacobian<16, 6> H) const {
141
- const Matrix& Q = matrix_;
142
- if (H) {
143
- H->setZero();
144
- H->block<4, 1>(0, 2) = -Q.col(3);
145
- H->block<4, 1>(0, 4) = -Q.col(2);
146
- H->block<4, 1>(0, 5) = Q.col(1);
147
- H->block<4, 1>(4, 1) = Q.col(3);
148
- H->block<4, 1>(4, 3) = Q.col(2);
149
- H->block<4, 1>(4, 5) = -Q.col(0);
150
- H->block<4, 1>(8, 0) = -Q.col(3);
151
- H->block<4, 1>(8, 3) = -Q.col(1);
152
- H->block<4, 1>(8, 4) = Q.col(0);
153
- H->block<4, 1>(12, 0) = Q.col(2);
154
- H->block<4, 1>(12, 1) = -Q.col(1);
155
- H->block<4, 1>(12, 2) = Q.col(0);
156
- }
157
- return Eigen::Map<const SO4::VectorN2>(Q.data());
158
-}
159
160
///******************************************************************************
161
gtsam/geometry/SO4.h
@@ -74,6 +74,28 @@ GTSAM_EXPORT Matrix3 topLeft(const SO4 &Q, OptionalJacobian<9, 6> H = {});
74
*/
75
GTSAM_EXPORT Matrix43 stiefel(const SO4 &Q, OptionalJacobian<12, 6> H = {});
76
77
+template <>
78
+GTSAM_EXPORT
79
+SO4::VectorN2 SO4::vec(OptionalJacobian<16, 6> H) const {
80
+ const Matrix& Q = matrix_;
81
+ if (H) {
82
+ H->setZero();
83
+ H->block<4, 1>(0, 2) = -Q.col(3);
84
+ H->block<4, 1>(0, 4) = -Q.col(2);
85
+ H->block<4, 1>(0, 5) = Q.col(1);
86
+ H->block<4, 1>(4, 1) = Q.col(3);
87
+ H->block<4, 1>(4, 3) = Q.col(2);
88
+ H->block<4, 1>(4, 5) = -Q.col(0);
89
+ H->block<4, 1>(8, 0) = -Q.col(3);
90
+ H->block<4, 1>(8, 3) = -Q.col(1);
91
+ H->block<4, 1>(8, 4) = Q.col(0);
92
+ H->block<4, 1>(12, 0) = Q.col(2);
93
+ H->block<4, 1>(12, 1) = -Q.col(1);
94
+ H->block<4, 1>(12, 2) = Q.col(0);
95
+ }
96
+ return Eigen::Map<const SO4::VectorN2>(Q.data());
97
+}
98
+
99
#if GTSAM_ENABLE_BOOST_SERIALIZATION
100
template <class Archive>
101
/** Serialization function */
gtsam/linear/NoiseModel.h
@@ -575,6 +575,9 @@ namespace gtsam {
575
* An isotropic noise model created by specifying a precision
576
577
static shared_ptr Precision(size_t dim, double precision, bool smart = true) {
578
+ if (0 == precision) {
579
+ return nullptr;
580
581
return Variance(dim, 1.0/precision, smart);
582
583
0 commit comments