diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index ba06e1d85..0cf771844 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -52,7 +52,9 @@ class GTSAM_EXPORT Point3 : public Vector3 { #ifndef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// Default constructor now creates *uninitialized* point !!!! - Point3() {} + Point3() { + throw std::runtime_error("Default constructor called!"); + } #endif @@ -106,9 +108,14 @@ class GTSAM_EXPORT Point3 : public Vector3 { /// @name Standard Interface /// @{ - /** return vectorized form (column-wise)*/ + /// return as Vector3 const Vector3& vector() const { return *this; } + /// return as transposed vector + Eigen::DenseBase::ConstTransposeReturnType transpose() const { + return this->Vector3::transpose(); + } + /// @} /// Output stream operator diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index c79fb5823..d037f7535 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -779,7 +779,7 @@ bool writeBAL(const string& filename, SfM_data &data) { os << endl; for (size_t j = 0; j < data.number_tracks(); j++) { // for each 3D point j - SfM_Track track = data.tracks[j]; + const SfM_Track& track = data.tracks[j]; for (size_t k = 0; k < track.number_measurements(); k++) { // for each observation of the 3D point j size_t i = track.measurements[k].first; // camera id @@ -808,7 +808,9 @@ bool writeBAL(const string& filename, SfM_data &data) { Cal3Bundler cameraCalibration = data.cameras[i].calibration(); Pose3 poseOpenGL = gtsam2openGL(poseGTSAM); os << Rot3::Logmap(poseOpenGL.rotation()) << endl; - os << poseOpenGL.translation() << endl; + os << poseOpenGL.translation().x() << endl; + os << poseOpenGL.translation().y() << endl; + os << poseOpenGL.translation().z() << endl; os << cameraCalibration.fx() << endl; os << cameraCalibration.k1() << endl; os << cameraCalibration.k2() << endl; diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp index b3953dd23..b6fc61411 100644 --- a/gtsam_unstable/dynamics/PoseRTV.cpp +++ b/gtsam_unstable/dynamics/PoseRTV.cpp @@ -37,7 +37,7 @@ PoseRTV::PoseRTV(const Vector& rtv) : Vector PoseRTV::vector() const { Vector rtv(9); rtv.head(3) = rotation().xyz(); - rtv.segment(3,3) = translation().vector(); + rtv.segment(3,3) = translation(); rtv.tail(3) = velocity(); return rtv; } @@ -52,7 +52,7 @@ bool PoseRTV::equals(const PoseRTV& other, double tol) const { void PoseRTV::print(const string& s) const { cout << s << ":" << endl; gtsam::print((Vector)R().xyz(), " R:rpy"); - t().print(" T"); + cout << " T" << t().transpose() << endl; gtsam::print((Vector)velocity(), " V"); } @@ -103,7 +103,7 @@ PoseRTV PoseRTV::flyingDynamics( Rot3 yaw_correction_bn = Rot3::Yaw(yaw2); Point3 forward(forward_accel, 0.0, 0.0); Vector Acc_n = - yaw_correction_bn.rotate(forward).vector() // applies locally forward force in the global frame + yaw_correction_bn.rotate(forward) // applies locally forward force in the global frame - drag * (Vector(3) << v1.x(), v1.y(), 0.0).finished() // drag term dependent on v1 + delta(3, 2, loss_lift - lift_control); // falling due to lift lost from pitch @@ -172,7 +172,7 @@ double PoseRTV::range(const PoseRTV& other, const Point3 t1 = pose().translation(H1 ? &D_t1_pose : 0); const Point3 t2 = other.pose().translation(H2 ? &D_t2_other : 0); Matrix13 D_d_t1, D_d_t2; - double d = t1.distance(t2, H1 ? &D_d_t1 : 0, H2 ? &D_d_t2 : 0); + double d = distance(t1, t2, H1 ? &D_d_t1 : 0, H2 ? &D_d_t2 : 0); if (H1) *H1 << D_d_t1 * D_t1_pose, 0,0,0; if (H2) *H2 << D_d_t2 * D_t2_other, 0,0,0; return d; @@ -188,7 +188,7 @@ PoseRTV PoseRTV::transformed_from(const Pose3& trans, ChartJacobian Dglobal, // Note that we rotate the velocity Matrix3 D_newvel_R, D_newvel_v; - Velocity3 newvel = trans.rotation().rotate(Point3(velocity()), D_newvel_R, D_newvel_v).vector(); + Velocity3 newvel = trans.rotation().rotate(Point3(velocity()), D_newvel_R, D_newvel_v); if (Dglobal) { Dglobal->setZero(); diff --git a/gtsam_unstable/dynamics/PoseRTV.h b/gtsam_unstable/dynamics/PoseRTV.h index b59ea4614..bf92ab9c4 100644 --- a/gtsam_unstable/dynamics/PoseRTV.h +++ b/gtsam_unstable/dynamics/PoseRTV.h @@ -66,7 +66,7 @@ public: // Access to vector for ease of use with Matlab // and avoidance of Point3 Vector vector() const; - Vector translationVec() const { return pose().translation().vector(); } + Vector translationVec() const { return pose().translation(); } const Velocity3& velocityVec() const { return velocity(); } // testable @@ -126,7 +126,7 @@ public: /// @return a vector for Matlab compatibility inline Vector translationIntegrationVec(const PoseRTV& x2, double dt) const { - return translationIntegration(x2, dt).vector(); + return translationIntegration(x2, dt); } /** diff --git a/gtsam_unstable/geometry/BearingS2.cpp b/gtsam_unstable/geometry/BearingS2.cpp index 71278789c..9dfed612c 100644 --- a/gtsam_unstable/geometry/BearingS2.cpp +++ b/gtsam_unstable/geometry/BearingS2.cpp @@ -34,7 +34,7 @@ BearingS2 BearingS2::fromDownwardsObservation(const Pose3& A, const Point3& B) { 0.,1.,0., -1.,0.,0.).finished(); // p_rel_c = Cbc*Cnb*(PosObj - Pos); - Vector p_rel_c = Cbc*Cnb*(B.vector() - A.translation().vector()); + Vector p_rel_c = Cbc*Cnb*(B - A.translation()); // FIXME: the matlab code checks for p_rel_c(0) greater than @@ -50,7 +50,7 @@ BearingS2 BearingS2::fromForwardObservation(const Pose3& A, const Point3& B) { // Cnb = DCMnb(Att); Matrix Cnb = A.rotation().matrix().transpose(); - Vector p_rel_c = Cnb*(B.vector() - A.translation().vector()); + Vector p_rel_c = Cnb*(B - A.translation()); // FIXME: the matlab code checks for p_rel_c(0) greater than diff --git a/gtsam_unstable/geometry/Event.h b/gtsam_unstable/geometry/Event.h index 77b02d9fb..615b1d467 100644 --- a/gtsam_unstable/geometry/Event.h +++ b/gtsam_unstable/geometry/Event.h @@ -60,14 +60,13 @@ public: /** print with optional string */ void print(const std::string& s = "") const { - std::cout << s << "time = " << time_; - location_.print("; location"); + std::cout << s << "time = " << time_ << "location = " << location_.transpose(); } /** equals with an tolerance */ bool equals(const Event& other, double tol = 1e-9) const { return std::abs(time_ - other.time_) < tol - && location_.equals(other.location_, tol); + && traits::Equals(location_, other.location_, tol); } /// Updates a with tangent space delta @@ -86,7 +85,7 @@ public: OptionalJacobian<1, 3> H2 = boost::none) const { static const double Speed = 330; Matrix13 D1, D2; - double distance = location_.distance(microphone, D1, D2); + double distance = gtsam::distance(location_, microphone, D1, D2); if (H1) // derivative of toa with respect to event *H1 << 1.0, D1 / Speed; diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index 516e1fb5b..9768f4fa4 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -43,7 +43,7 @@ Similarity3::Similarity3(const Matrix4& T) : } bool Similarity3::equals(const Similarity3& other, double tol) const { - return R_.equals(other.R_, tol) && t_.equals(other.t_, tol) + return R_.equals(other.R_, tol) && traits::Equals(t_, other.t_, tol) && s_ < (other.s_ + tol) && s_ > (other.s_ - tol); } @@ -55,8 +55,7 @@ void Similarity3::print(const std::string& s) const { std::cout << std::endl; std::cout << s; rotation().print("R:\n"); - translation().print("t: "); - std::cout << "s: " << scale() << std::endl; + std::cout << "t: " << translation().transpose() << "s: " << scale() << std::endl; } Similarity3 Similarity3::identity() { @@ -79,7 +78,7 @@ Point3 Similarity3::transform_from(const Point3& p, // // For this derivative, see LieGroups.pdf const Matrix3 sR = s_ * R_.matrix(); const Matrix3 DR = sR * skewSymmetric(-p.x(), -p.y(), -p.z()); - *H1 << DR, sR, sR * p.vector(); + *H1 << DR, sR, sR * p; } if (H2) *H2 = s_ * R_.matrix(); // just 3*3 sub-block of matrix() @@ -103,7 +102,7 @@ Matrix4 Similarity3::wedge(const Vector7& xi) { Matrix7 Similarity3::AdjointMap() const { // http://www.ethaneade.org/latex2html/lie/node30.html const Matrix3 R = R_.matrix(); - const Vector3 t = t_.vector(); + const Vector3 t = t_; const Matrix3 A = s_ * skewSymmetric(t) * R; Matrix7 adj; adj << R, Z_3x3, Matrix31::Zero(), // 3*7 @@ -153,7 +152,7 @@ Vector7 Similarity3::Logmap(const Similarity3& T, OptionalJacobian<7, 7> Hm) { const Vector3 w = Rot3::Logmap(T.R_); const double lambda = log(T.s_); Vector7 result; - result << w, GetV(w, lambda).inverse() * T.t_.vector(), lambda; + result << w, GetV(w, lambda).inverse() * T.t_, lambda; if (Hm) { throw std::runtime_error("Similarity3::Logmap: derivative not implemented"); } @@ -173,13 +172,13 @@ Similarity3 Similarity3::Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm) { std::ostream &operator<<(std::ostream &os, const Similarity3& p) { os << "[" << p.rotation().xyz().transpose() << " " - << p.translation().vector().transpose() << " " << p.scale() << "]\';"; + << p.translation().transpose() << " " << p.scale() << "]\';"; return os; } const Matrix4 Similarity3::matrix() const { Matrix4 T; - T.topRows<3>() << R_.matrix(), t_.vector(); + T.topRows<3>() << R_.matrix(), t_; T.bottomRows<1>() << 0, 0, 0, 1.0 / s_; return T; } diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 6651c005f..7cbeaae65 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -1,12 +1,12 @@ /* ---------------------------------------------------------------------------- - + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - + * See LICENSE for the license information - + * -------------------------------------------------------------------------- */ /** @@ -253,7 +253,7 @@ public: reprojections.push_back(cameras[i].backproject(measured_[i])); } - Point3 pw_sum; + Point3 pw_sum(0,0,0); BOOST_FOREACH(const Point3& pw, reprojections) { pw_sum = pw_sum + pw; } diff --git a/wrap/Class.cpp b/wrap/Class.cpp index e62e31bc1..d7a3b6ee4 100644 --- a/wrap/Class.cpp +++ b/wrap/Class.cpp @@ -567,7 +567,7 @@ void Class::deserialization_fragments(FileWriter& proxyFile, // string serialized = unwrap< string >(in[0]); // istringstream in_archive_stream(serialized); // boost::archive::text_iarchive in_archive(in_archive_stream); - // Shared output(new Point3()); + // Shared output(new Point3(0,0,0)); // in_archive >> *output; // out[0] = wrap_shared_ptr(output,"Point3", false); //} diff --git a/wrap/tests/expected/geometry_wrapper.cpp b/wrap/tests/expected/geometry_wrapper.cpp index 226030a0d..6646394f6 100644 --- a/wrap/tests/expected/geometry_wrapper.cpp +++ b/wrap/tests/expected/geometry_wrapper.cpp @@ -326,7 +326,7 @@ void gtsamPoint3_string_deserialize_19(int nargout, mxArray *out[], int nargin, string serialized = unwrap< string >(in[0]); istringstream in_archive_stream(serialized); boost::archive::text_iarchive in_archive(in_archive_stream); - Shared output(new gtsam::Point3()); + Shared output(new gtsam::Point3(0,0,0)); in_archive >> *output; out[0] = wrap_shared_ptr(output,"gtsam.Point3", false); }