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

No more use of vector() or default constructor

release/4.3a0
dellaert 2016-02-12 00:06:07 -08:00
parent fe1607c464
commit 1c920967d9
16 changed files with 38 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -167,7 +167,7 @@ Camera camera(Pose3(Rot3().retract(Vector3(0.1, 0.2, 0.3)), Point3(0, 5, 0)),
Cal3Bundler0(1, 0, 0));
Point3 point(10, 0, -5); // negative Z-axis convention of Snavely!
Vector9 P = Camera().localCoordinates(camera);
Vector3 X = point.vector();
Vector3 X = point;
Vector2 expectedMeasurement(1.2431567, 1.2525694);
Matrix E1 = numericalDerivative21<Vector2, Vector9, Vector3>(adapted, P, X);
Matrix E2 = numericalDerivative22<Vector2, Vector9, Vector3>(adapted, P, X);

4
gtsam/nonlinear/tests/testExpression.cpp
View File

@ -81,7 +81,7 @@ double f2(const Point3& p, OptionalJacobian<1, 3> H) {
return 0.0;
}
Vector f3(const Point3& p, OptionalJacobian<Eigen::Dynamic, 3> H) {
return p.vector();
return p;
}
Expression<Point3> p(1);
set<Key> expected = list_of(1);
@ -108,7 +108,7 @@ TEST(Expression, NullaryMethod) {
// Create expression
Expression<Point3> p(67);
Expression<double> norm(p, &Point3::norm);
Expression<double> norm(&gtsam::norm, p);
// Create Values
Values values;

12
gtsam/sam/tests/testRangeFactor.cpp
View File

@ -361,10 +361,10 @@ TEST( RangeFactor, Camera) {
namespace gtsam{
template <>
struct Range<Vector3, Vector3> {
struct Range<Vector4, Vector4> {
typedef double result_type;
double operator()(const Vector3& v1, const Vector3& v2,
OptionalJacobian<1, 3> H1, OptionalJacobian<1, 3> H2) {
double operator()(const Vector4& v1, const Vector4& v2,
OptionalJacobian<1, 4> H1, OptionalJacobian<1, 4> H2) {
return (v2 - v1).norm();
// derivatives not implemented
}
@ -376,11 +376,11 @@ TEST(RangeFactor, NonGTSAM) {
Key poseKey(1);
Key pointKey(2);
double measurement(10.0);
RangeFactor<Vector3> factor(poseKey, pointKey, measurement, model);
RangeFactor<Vector4> factor(poseKey, pointKey, measurement, model);
// Set the linearization point
Vector3 pose(1.0, 2.0, 00);
Vector3 point(-4.0, 11.0, 0);
Vector4 pose(1.0, 2.0, 00, 0);
Vector4 point(-4.0, 11.0, 0, 0);
// The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter / UnitCovariance
Vector expectedError = (Vector(1) << 0.295630141).finished();

4
gtsam/slam/EssentialMatrixFactor.h
View File

@ -139,7 +139,7 @@ public:
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
Base::print(s);
std::cout << " EssentialMatrixFactor2 with measurements\n ("
<< dP1_.vector().transpose() << ")' and (" << pn_.vector().transpose()
<< dP1_.transpose() << ")' and (" << pn_.vector().transpose()
<< ")'" << std::endl;
}
@ -191,7 +191,7 @@ public:
if (Dd) // efficient backwards computation:
// (2*3) * (3*3) * (3*1)
*Dd = -f_ * (Dpn_dP2 * (DP2_point * _1T2.vector()));
*Dd = -f_ * (Dpn_dP2 * (DP2_point * _1T2));
}
Point2 reprojectionError = pn - pn_;

6
gtsam/slam/PoseTranslationPrior.h
View File

@ -70,19 +70,19 @@ public:
(*H).middleCols(transInterval.first, tDim) = R.matrix();
}
return newTrans.vector() - measured_.vector();
return traits<Translation>::Local(measured_, newTrans);
}
/** equals specialized to this factor */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol) && measured_.equals(e->measured_, tol);
return e != NULL && Base::equals(*e, tol) && traits<Translation>::Equals(measured_, e->measured_, tol);
}
/** print contents */
void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
Base::print(s + "PoseTranslationPrior", keyFormatter);
measured_.print("Measured Translation");
traits<Translation>::Print(measured_, "Measured Translation");
}
private:

6
gtsam/slam/RotateFactor.h
View File

@ -44,9 +44,9 @@ public:
virtual void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
Base::print(s);
std::cout << "RotateFactor:" << std::endl;
p_.print("p");
z_.print("z");
std::cout << "RotateFactor:]\n";
std::cout << "p: " << p_.transpose() << std::endl;
std::cout << "z: " << z_.transpose() << std::endl;
}
/// vector of errors returns 2D vector

4
gtsam_unstable/dynamics/FullIMUFactor.h
View File

@ -87,7 +87,7 @@ public:
Vector9 z;
z.head(3).operator=(accel_); // Strange syntax to work around ambiguous operator error with clang
z.segment(3, 3).operator=(gyro_); // Strange syntax to work around ambiguous operator error with clang
z.tail(3).operator=(x2.t().vector()); // Strange syntax to work around ambiguous operator error with clang
z.tail(3).operator=(x2.t()); // Strange syntax to work around ambiguous operator error with clang
if (H1) *H1 = numericalDerivative21<Vector9, PoseRTV, PoseRTV>(
boost::bind(This::predict_proxy, _1, _2, dt_), x1, x2, 1e-5);
if (H2) *H2 = numericalDerivative22<Vector9, PoseRTV, PoseRTV>(
@ -109,7 +109,7 @@ private:
static Vector9 predict_proxy(const PoseRTV& x1, const PoseRTV& x2, double dt) {
Vector9 hx;
hx.head(6).operator=(x1.imuPrediction(x2, dt)); // Strange syntax to work around ambiguous operator error with clang
hx.tail(3).operator=(x1.translationIntegration(x2, dt).vector()); // Strange syntax to work around ambiguous operator error with clang
hx.tail(3).operator=(x1.translationIntegration(x2, dt)); // Strange syntax to work around ambiguous operator error with clang
return hx;
}
};

2
gtsam_unstable/dynamics/VelocityConstraint.h
View File

@ -115,7 +115,7 @@ private:
case dynamics::EULER_END : hx = p1 + Point3(v2 * dt); break;
default: assert(false); break;
}
return p2.vector() - hx.vector();
return p2 - hx;
}
};

3
gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp
View File

@ -89,7 +89,8 @@ Vector newtonEuler(const Vector& Vb, const Vector& Fb, const Matrix& Inertia) {
TEST( DiscreteEulerPoincareHelicopter, evaluateError) {
Vector Fu = computeFu(gamma2, u2);
Vector fGravity_g1 = zero(6); subInsert(fGravity_g1, g1.rotation().unrotate(Point3(0.0, 0.0, -mass*9.81)).vector(), 3); // gravity force in g1 frame
Vector fGravity_g1 = zero(6);
subInsert(fGravity_g1, g1.rotation().unrotate(Point3(0.0, 0.0, -mass*9.81)), 3); // gravity force in g1 frame
Vector Fb = Fu+fGravity_g1;
Vector dV = newtonEuler(V1_g1, Fb, Inertia);

2
gtsam_unstable/geometry/tests/testSimilarity3.cpp
View File

@ -64,7 +64,7 @@ TEST(Similarity3, Constructors) {
Similarity3 sim3_Construct1;
Similarity3 sim3_Construct2(s);
Similarity3 sim3_Construct3(R, P, s);
Similarity3 sim4_Construct4(R.matrix(), P.vector(), s);
Similarity3 sim4_Construct4(R.matrix(), P, s);
}
//******************************************************************************

8
gtsam_unstable/slam/BiasedGPSFactor.h
View File

@ -58,15 +58,15 @@ namespace gtsam {
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "BiasedGPSFactor("
<< keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << ")\n";
measured_.print(" measured: ");
<< keyFormatter(this->key2()) << ")\n"
<< " measured: " << measured_.transpose() << std::endl;
this->noiseModel_->print(" noise model: ");
}
/** equals */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol) && this->measured_.equals(e->measured_, tol);
return e != NULL && Base::equals(*e, tol) && traits<Point3>::Equals(this->measured_, e->measured_, tol);
}
/** implement functions needed to derive from Factor */
@ -82,7 +82,7 @@ namespace gtsam {
H2->resize(3,3); // jacobian wrt bias
(*H2) << Matrix3::Identity();
}
return pose.translation().vector() + bias.vector() - measured_.vector();
return pose.translation() + bias - measured_;
}
/** return the measured */

2
gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h
View File

@ -187,7 +187,7 @@ public:
Vector GyroCorrected(Bias1.correctGyroscope(measurement_gyro_));
body_omega_body = body_R_sensor * GyroCorrected;
Matrix body_omega_body__cross = skewSymmetric(body_omega_body);
body_a_body = body_R_sensor * AccCorrected - body_omega_body__cross * body_omega_body__cross * body_P_sensor_->translation().vector();
body_a_body = body_R_sensor * AccCorrected - body_omega_body__cross * body_omega_body__cross * body_P_sensor_->translation();
} else {
body_a_body = AccCorrected;
}

2
gtsam_unstable/slam/InvDepthFactorVariant2.h
View File

@ -79,7 +79,7 @@ public:
&& Base::equals(p, tol)
&& this->measured_.equals(e->measured_, tol)
&& this->K_->equals(*e->K_, tol)
&& this->referencePoint_.equals(e->referencePoint_, tol);
&& traits<Point3>::Equals(this->referencePoint_, e->referencePoint_, tol);
}
Vector inverseDepthError(const Pose3& pose, const Vector3& landmark) const {

12
gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp
View File

@ -233,7 +233,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1,
///* VADIM - START ************************************************************************* */
//Vector3 predictionRq(const Vector3 angles, const Vector3 q) {
// return (Rot3().RzRyRx(angles) * q).vector();
// return (Rot3().RzRyRx(angles) * q);
//}
//
//TEST (InertialNavFactor_GlobalVelocity, Rotation_Deriv ) {
@ -435,7 +435,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1,
Vector measurement_acc = Vector3(0.2, 0.1, -0.3 + 9.81)
+ omega__cross * omega__cross
* body_P_sensor.rotation().inverse().matrix()
* body_P_sensor.translation().vector(); // Measured in ENU orientation
* body_P_sensor.translation(); // Measured in ENU orientation
InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(
PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc,
@ -474,7 +474,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1,
Vector measurement_acc = Vector3(0.2, 0.1, -0.3 + 9.81)
+ omega__cross * omega__cross
* body_P_sensor.rotation().inverse().matrix()
* body_P_sensor.translation().vector(); // Measured in ENU orientation
* body_P_sensor.translation(); // Measured in ENU orientation
InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(
PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc,
@ -512,7 +512,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1,
Vector measurement_acc = Vector3(0.0, 0.0, 0.0 + 9.81)
+ omega__cross * omega__cross
* body_P_sensor.rotation().inverse().matrix()
* body_P_sensor.translation().vector(); // Measured in ENU orientation
* body_P_sensor.translation(); // Measured in ENU orientation
InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(
PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc,
@ -554,7 +554,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1,
Vector3(-6.763926150509185, 6.501390843381716, +2.300389940090343)
+ omega__cross * omega__cross
* body_P_sensor.rotation().inverse().matrix()
* body_P_sensor.translation().vector(); // Measured in ENU orientation
* body_P_sensor.translation(); // Measured in ENU orientation
InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(
PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc,
@ -605,7 +605,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1,
Vector3(-6.763926150509185, 6.501390843381716, +2.300389940090343)
+ omega__cross * omega__cross
* body_P_sensor.rotation().inverse().matrix()
* body_P_sensor.translation().vector(); // Measured in ENU orientation
* body_P_sensor.translation(); // Measured in ENU orientation
InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> factor(
PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc,

4
tests/simulated3D.h
View File

@ -91,7 +91,7 @@ struct PointPrior3D: public NoiseModelFactor1<Point3> {
*/
Vector evaluateError(const Point3& x, boost::optional<Matrix&> H =
boost::none) const {
return prior(x, H).vector() - measured_.vector();
return prior(x, H) - measured_;
}
};
@ -122,7 +122,7 @@ struct Simulated3DMeasurement: public NoiseModelFactor2<Point3, Point3> {
*/
Vector evaluateError(const Point3& x1, const Point3& x2,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
return mea(x1, x2, H1, H2).vector() - measured_.vector();
return mea(x1, x2, H1, H2) - measured_;
}
};

2
tests/testExpressionFactor.cpp
View File

@ -139,7 +139,7 @@ TEST(ExpressionFactor, Unary) {
typedef Eigen::Matrix<double,9,3> Matrix93;
Vector9 wide(const Point3& p, OptionalJacobian<9,3> H) {
Vector9 v;
v << p.vector(), p.vector(), p.vector();
v << p, p, p;
if (H) *H << eye(3), eye(3), eye(3);
return v;
}