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

Chasing down more vector() and Point2 default constructor uses

release/4.3a0
dellaert 2016-06-06 00:37:49 -07:00
parent bdbbe0203d
commit 41b091e86f
25 changed files with 124 additions and 124 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
gtsam/geometry/CameraSet.h
View File

@ -56,8 +56,7 @@ protected:
// Project and fill error vector
Vector b(ZDim * m);
for (size_t i = 0, row = 0; i < m; i++, row += ZDim) {
Z e = predicted[i] - measured[i];
b.segment<ZDim>(row) = e.vector();
b.segment<ZDim>(row) = traits<Z>::Local(measured[i], predicted[i]);
}
return b;
}

2
gtsam/geometry/Rot3.h
View File

@ -179,7 +179,7 @@ namespace gtsam {
*/
static Rot3 AxisAngle(const Point3& axis, double angle) {
#ifdef GTSAM_USE_QUATERNIONS
return gtsam::Quaternion(Eigen::AngleAxis<double>(angle, axis.vector()));
return gtsam::Quaternion(Eigen::AngleAxis<double>(angle, axis));
#else
return Rot3(SO3::AxisAngle(axis,angle));
#endif

2
gtsam/geometry/tests/testPinholeSet.cpp
View File

@ -84,7 +84,7 @@ TEST(PinholeSet, Pinhole) {
EXPECT(!set.equals(set2));
// Check measurements
Point2 expected;
Point2 expected(0,0);
ZZ z = set.project2(p);
EXPECT(assert_equal(expected, z[0]));
EXPECT(assert_equal(expected, z[1]));

2
gtsam/geometry/triangulation.h
View File

@ -471,7 +471,7 @@ TriangulationResult triangulateSafe(const std::vector<CAMERA>& cameras,
if (params.dynamicOutlierRejectionThreshold > 0) {
const Point2& zi = measured.at(i);
Point2 reprojectionError(camera.project(point) - zi);
totalReprojError += reprojectionError.vector().norm();
totalReprojError += reprojectionError.norm();
}
i += 1;
}

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_.transpose() << ")' and (" << pn_.vector().transpose()
<< dP1_.transpose() << ")' and (" << pn_.transpose()
<< ")'" << std::endl;
}
@ -195,7 +195,7 @@ public:
}
Point2 reprojectionError = pn - pn_;
return f_ * reprojectionError.vector();
return f_ * reprojectionError;
}
};

9
gtsam/slam/GeneralSFMFactor.h
View File

@ -122,8 +122,7 @@ public:
Vector evaluateError(const CAMERA& camera, const LANDMARK& point,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const {
try {
Point2 reprojError(camera.project2(point,H1,H2) - measured_);
return reprojError.vector();
return camera.project2(point,H1,H2) - measured_;
}
catch( CheiralityException& e) {
if (H1) *H1 = JacobianC::Zero();
@ -145,8 +144,7 @@ public:
try {
const CAMERA& camera = values.at<CAMERA>(key1);
const LANDMARK& point = values.at<LANDMARK>(key2);
Point2 reprojError(camera.project2(point, H1, H2) - measured());
b = -reprojError.vector();
b = measured() - camera.project2(point, H1, H2);
} catch (CheiralityException& e) {
H1.setZero();
H2.setZero();
@ -262,8 +260,7 @@ public:
{
try {
Camera camera(pose3,calib);
Point2 reprojError(camera.project(point, H1, H2, H3) - measured_);
return reprojError.vector();
return camera.project(point, H1, H2, H3) - measured_;
}
catch( CheiralityException& e) {
if (H1) *H1 = Matrix::Zero(2, 6);

12
gtsam/slam/ProjectionFactor.h
View File

@ -56,7 +56,9 @@ namespace gtsam {
typedef boost::shared_ptr<This> shared_ptr;
/// Default constructor
GenericProjectionFactor() : throwCheirality_(false), verboseCheirality_(false) {}
GenericProjectionFactor() :
measured_(0, 0), throwCheirality_(false), verboseCheirality_(false) {
}
/**
* Constructor
@ -134,16 +136,14 @@ namespace gtsam {
PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_, H0), *K_);
Point2 reprojectionError(camera.project(point, H1, H2, boost::none) - measured_);
*H1 = *H1 * H0;
return reprojectionError.vector();
return reprojectionError;
} else {
PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_), *K_);
Point2 reprojectionError(camera.project(point, H1, H2, boost::none) - measured_);
return reprojectionError.vector();
return camera.project(point, H1, H2, boost::none) - measured_;
}
} else {
PinholeCamera<CALIBRATION> camera(pose, *K_);
Point2 reprojectionError(camera.project(point, H1, H2, boost::none) - measured_);
return reprojectionError.vector();
return camera.project(point, H1, H2, boost::none) - measured_;
}
} catch( CheiralityException& e) {
if (H1) *H1 = Matrix::Zero(2,6);

4
gtsam/slam/SmartProjectionFactor.h
View File

@ -499,8 +499,8 @@ public:
return Base::totalReprojectionError(cameras, *result_);
else if (params_.degeneracyMode == HANDLE_INFINITY) {
// Otherwise, manage the exceptions with rotation-only factors
const Point2& z0 = this->measured_.at(0);
Unit3 backprojected = cameras.front().backprojectPointAtInfinity(z0);
Unit3 backprojected = cameras.front().backprojectPointAtInfinity(
this->measured_.at(0));
return Base::totalReprojectionError(cameras, backprojected);
} else
// if we don't want to manage the exceptions we discard the factor

3
gtsam/slam/TriangulationFactor.h
View File

@ -119,8 +119,7 @@ public:
Vector evaluateError(const Point3& point, boost::optional<Matrix&> H2 =
boost::none) const {
try {
Measurement error(camera_.project2(point, boost::none, H2) - measured_);
return error.vector();
return traits<Measurement>::Local(measured_, camera_.project2(point, boost::none, H2));
} catch (CheiralityException& e) {
if (H2)
*H2 = Matrix::Zero(Measurement::dimension, 3);

2
gtsam_unstable/geometry/SimWall2D.cpp
View File

@ -73,7 +73,7 @@ bool SimWall2D::intersects(const SimWall2D& B, boost::optional<Point2&> pt) cons
}
// find lower point by y
Point2 low, high;
Point2 low(0,0), high(0,0);
if (Ba.y() > Bb.y()) {
high = Ba;
low = Bb;

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

@ -16,7 +16,7 @@ const double tol=1e-5;
TEST(testPolygon, triangle_basic) {
// create a triangle from points, extract landmarks/walls, check occupancy
Point2 pA, pB(2.0, 0.0), pC(0.0, 1.0);
Point2 pA(0,0), pB(2.0, 0.0), pC(0.0, 1.0);
// construct and extract data
SimPolygon2D actTriangle = SimPolygon2D::createTriangle(pA, pB, pC);

4
gtsam_unstable/geometry/tests/testSimWall2D.cpp
View File

@ -24,7 +24,7 @@ TEST(testSimWall2D2D, construction ) {
/* ************************************************************************* */
TEST(testSimWall2D2D, equals ) {
Point2 p1(1.0, 0.0), p2(1.0, 2.0), p3;
Point2 p1(1.0, 0.0), p2(1.0, 2.0), p3(0,0);
SimWall2D w1(p1, p2), w2(p1, p3);
EXPECT(assert_equal(w1, w1));
EXPECT(assert_inequal(w1, w2));
@ -34,7 +34,7 @@ TEST(testSimWall2D2D, equals ) {
/* ************************************************************************* */
TEST(testSimWall2D2D, intersection1 ) {
SimWall2D w1(2.0, 2.0, 6.0, 2.0), w2(4.0, 4.0, 4.0, 0.0);
Point2 pt;
Point2 pt(0,0);
EXPECT(w1.intersects(w2));
EXPECT(w2.intersects(w1));
w1.intersects(w2, pt);

41
gtsam_unstable/slam/InvDepthFactor3.h
View File

@ -24,17 +24,17 @@ namespace gtsam {
* Ternary factor representing a visual measurement that includes inverse depth
*/
template<class POSE, class LANDMARK, class INVDEPTH>
class InvDepthFactor3: public gtsam::NoiseModelFactor3<POSE, LANDMARK, INVDEPTH> {
class InvDepthFactor3: public NoiseModelFactor3<POSE, LANDMARK, INVDEPTH> {
protected:
// Keep a copy of measurement and calibration for I/O
gtsam::Point2 measured_; ///< 2D measurement
boost::shared_ptr<gtsam::Cal3_S2> K_; ///< shared pointer to calibration object
Point2 measured_; ///< 2D measurement
boost::shared_ptr<Cal3_S2> K_; ///< shared pointer to calibration object
public:
/// shorthand for base class type
typedef gtsam::NoiseModelFactor3<POSE, LANDMARK, INVDEPTH> Base;
typedef NoiseModelFactor3<POSE, LANDMARK, INVDEPTH> Base;
/// shorthand for this class
typedef InvDepthFactor3<POSE, LANDMARK, INVDEPTH> This;
@ -43,7 +43,9 @@ public:
typedef boost::shared_ptr<This> shared_ptr;
/// Default constructor
InvDepthFactor3() : K_(new gtsam::Cal3_S2(444, 555, 666, 777, 888)) {}
InvDepthFactor3() :
measured_(0.0, 0.0), K_(new Cal3_S2(444, 555, 666, 777, 888)) {
}
/**
* Constructor
@ -55,8 +57,8 @@ public:
* @param invDepthKey is the index of inverse depth
* @param K shared pointer to the constant calibration
*/
InvDepthFactor3(const gtsam::Point2& measured, const gtsam::SharedNoiseModel& model,
const gtsam::Key poseKey, gtsam::Key pointKey, gtsam::Key invDepthKey, const Cal3_S2::shared_ptr& K) :
InvDepthFactor3(const Point2& measured, const SharedNoiseModel& model,
const Key poseKey, Key pointKey, Key invDepthKey, const Cal3_S2::shared_ptr& K) :
Base(model, poseKey, pointKey, invDepthKey), measured_(measured), K_(K) {}
/** Virtual destructor */
@ -68,44 +70,43 @@ public:
* @param keyFormatter optional formatter useful for printing Symbols
*/
void print(const std::string& s = "InvDepthFactor3",
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const {
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
Base::print(s, keyFormatter);
measured_.print(s + ".z");
}
/// equals
virtual bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const {
virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
const This *e = dynamic_cast<const This*>(&p);
return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol) && this->K_->equals(*e->K_, tol);
}
/// Evaluate error h(x)-z and optionally derivatives
gtsam::Vector evaluateError(const POSE& pose, const Vector5& point, const INVDEPTH& invDepth,
boost::optional<gtsam::Matrix&> H1=boost::none,
boost::optional<gtsam::Matrix&> H2=boost::none,
boost::optional<gtsam::Matrix&> H3=boost::none) const {
Vector evaluateError(const POSE& pose, const Vector5& point, const INVDEPTH& invDepth,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none,
boost::optional<Matrix&> H3=boost::none) const {
try {
InvDepthCamera3<gtsam::Cal3_S2> camera(pose, K_);
gtsam::Point2 reprojectionError(camera.project(point, invDepth, H1, H2, H3) - measured_);
return reprojectionError.vector();
InvDepthCamera3<Cal3_S2> camera(pose, K_);
return camera.project(point, invDepth, H1, H2, H3) - measured_;
} catch( CheiralityException& e) {
if (H1) *H1 = Matrix::Zero(2,6);
if (H2) *H2 = Matrix::Zero(2,5);
if (H3) *H2 = Matrix::Zero(2,1);
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
" moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
return gtsam::Vector::Ones(2) * 2.0 * K_->fx();
return Vector::Ones(2) * 2.0 * K_->fx();
}
return (gtsam::Vector(1) << 0.0).finished();
return (Vector(1) << 0.0).finished();
}
/** return the measurement */
const gtsam::Point2& imagePoint() const {
const Point2& imagePoint() const {
return measured_;
}
/** return the calibration object */
inline const gtsam::Cal3_S2::shared_ptr calibration() const {
inline const Cal3_S2::shared_ptr calibration() const {
return K_;
}

21
gtsam_unstable/slam/InvDepthFactorVariant1.h
View File

@ -41,7 +41,9 @@ public:
typedef boost::shared_ptr<This> shared_ptr;
/// Default constructor
InvDepthFactorVariant1() : K_(new Cal3_S2(444, 555, 666, 777, 888)) {}
InvDepthFactorVariant1() :
measured_(0.0, 0.0), K_(new Cal3_S2(444, 555, 666, 777, 888)) {
}
/**
* Constructor
@ -64,13 +66,13 @@ public:
* @param keyFormatter optional formatter useful for printing Symbols
*/
void print(const std::string& s = "InvDepthFactorVariant1",
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const {
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
Base::print(s, keyFormatter);
measured_.print(s + ".z");
}
/// equals
virtual bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const {
virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
const This *e = dynamic_cast<const This*>(&p);
return e
&& Base::equals(p, tol)
@ -86,22 +88,21 @@ public:
Point3 world_P_landmark = Point3(x, y, z) + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho);
// Project landmark into Pose2
PinholeCamera<Cal3_S2> camera(pose, *K_);
gtsam::Point2 reprojectionError(camera.project(world_P_landmark) - measured_);
return reprojectionError.vector();
return camera.project(world_P_landmark) - measured_;
} catch( CheiralityException& e) {
std::cout << e.what()
<< ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key2()) << "]"
<< " moved behind camera [" << DefaultKeyFormatter(this->key1()) <<"]"
<< std::endl;
return gtsam::Vector::Ones(2) * 2.0 * K_->fx();
return Vector::Ones(2) * 2.0 * K_->fx();
}
return (gtsam::Vector(1) << 0.0).finished();
return (Vector(1) << 0.0).finished();
}
/// Evaluate error h(x)-z and optionally derivatives
Vector evaluateError(const Pose3& pose, const Vector6& landmark,
boost::optional<gtsam::Matrix&> H1=boost::none,
boost::optional<gtsam::Matrix&> H2=boost::none) const {
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const {
if (H1) {
(*H1) = numericalDerivative11<Vector, Pose3>(
@ -118,7 +119,7 @@ public:
}
/** return the measurement */
const gtsam::Point2& imagePoint() const {
const Point2& imagePoint() const {
return measured_;
}

21
gtsam_unstable/slam/InvDepthFactorVariant2.h
View File

@ -43,7 +43,9 @@ public:
typedef boost::shared_ptr<This> shared_ptr;
/// Default constructor
InvDepthFactorVariant2() : K_(new Cal3_S2(444, 555, 666, 777, 888)) {}
InvDepthFactorVariant2() :
measured_(0.0, 0.0), K_(new Cal3_S2(444, 555, 666, 777, 888)) {
}
/**
* Constructor
@ -67,13 +69,13 @@ public:
* @param keyFormatter optional formatter useful for printing Symbols
*/
void print(const std::string& s = "InvDepthFactorVariant2",
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const {
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
Base::print(s, keyFormatter);
measured_.print(s + ".z");
}
/// equals
virtual bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const {
virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
const This *e = dynamic_cast<const This*>(&p);
return e
&& Base::equals(p, tol)
@ -89,22 +91,21 @@ public:
Point3 world_P_landmark = referencePoint_ + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho);
// Project landmark into Pose2
PinholeCamera<Cal3_S2> camera(pose, *K_);
gtsam::Point2 reprojectionError(camera.project(world_P_landmark) - measured_);
return reprojectionError.vector();
return camera.project(world_P_landmark) - measured_;
} catch( CheiralityException& e) {
std::cout << e.what()
<< ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key2()) << "]"
<< " moved behind camera [" << DefaultKeyFormatter(this->key1()) <<"]"
<< std::endl;
return gtsam::Vector::Ones(2) * 2.0 * K_->fx();
return Vector::Ones(2) * 2.0 * K_->fx();
}
return (gtsam::Vector(1) << 0.0).finished();
return (Vector(1) << 0.0).finished();
}
/// Evaluate error h(x)-z and optionally derivatives
Vector evaluateError(const Pose3& pose, const Vector3& landmark,
boost::optional<gtsam::Matrix&> H1=boost::none,
boost::optional<gtsam::Matrix&> H2=boost::none) const {
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const {
if (H1) {
(*H1) = numericalDerivative11<Vector, Pose3>(
@ -121,7 +122,7 @@ public:
}
/** return the measurement */
const gtsam::Point2& imagePoint() const {
const Point2& imagePoint() const {
return measured_;
}

40
gtsam_unstable/slam/InvDepthFactorVariant3.h
View File

@ -41,7 +41,9 @@ public:
typedef boost::shared_ptr<This> shared_ptr;
/// Default constructor
InvDepthFactorVariant3a() : K_(new Cal3_S2(444, 555, 666, 777, 888)) {}
InvDepthFactorVariant3a() :
measured_(0.0, 0.0), K_(new Cal3_S2(444, 555, 666, 777, 888)) {
}
/**
* Constructor
@ -66,13 +68,13 @@ public:
* @param keyFormatter optional formatter useful for printing Symbols
*/
void print(const std::string& s = "InvDepthFactorVariant3a",
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const {
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
Base::print(s, keyFormatter);
measured_.print(s + ".z");
}
/// equals
virtual bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const {
virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
const This *e = dynamic_cast<const This*>(&p);
return e
&& Base::equals(p, tol)
@ -89,22 +91,21 @@ public:
Point3 world_P_landmark = pose.transform_from(pose_P_landmark);
// Project landmark into Pose2
PinholeCamera<Cal3_S2> camera(pose, *K_);
gtsam::Point2 reprojectionError(camera.project(world_P_landmark) - measured_);
return reprojectionError.vector();
return camera.project(world_P_landmark) - measured_;
} catch( CheiralityException& e) {
std::cout << e.what()
<< ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key1()) << "," << DefaultKeyFormatter(this->key2()) << "]"
<< " moved behind camera [" << DefaultKeyFormatter(this->key1()) << "]"
<< std::endl;
return gtsam::Vector::Ones(2) * 2.0 * K_->fx();
return Vector::Ones(2) * 2.0 * K_->fx();
}
return (Vector(1) << 0.0).finished();
}
/// Evaluate error h(x)-z and optionally derivatives
Vector evaluateError(const Pose3& pose, const Vector3& landmark,
boost::optional<gtsam::Matrix&> H1=boost::none,
boost::optional<gtsam::Matrix&> H2=boost::none) const {
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const {
if(H1) {
(*H1) = numericalDerivative11<Vector,Pose3>(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, _1, landmark), pose);
@ -117,7 +118,7 @@ public:
}
/** return the measurement */
const gtsam::Point2& imagePoint() const {
const Point2& imagePoint() const {
return measured_;
}
@ -160,7 +161,9 @@ public:
typedef boost::shared_ptr<This> shared_ptr;
/// Default constructor
InvDepthFactorVariant3b() : K_(new Cal3_S2(444, 555, 666, 777, 888)) {}
InvDepthFactorVariant3b() :
measured_(0.0, 0.0), K_(new Cal3_S2(444, 555, 666, 777, 888)) {
}
/**
* Constructor
@ -185,13 +188,13 @@ public:
* @param keyFormatter optional formatter useful for printing Symbols
*/
void print(const std::string& s = "InvDepthFactorVariant3",
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const {
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
Base::print(s, keyFormatter);
measured_.print(s + ".z");
}
/// equals
virtual bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const {
virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
const This *e = dynamic_cast<const This*>(&p);
return e
&& Base::equals(p, tol)
@ -208,23 +211,22 @@ public:
Point3 world_P_landmark = pose1.transform_from(pose1_P_landmark);
// Project landmark into Pose2
PinholeCamera<Cal3_S2> camera(pose2, *K_);
gtsam::Point2 reprojectionError(camera.project(world_P_landmark) - measured_);
return reprojectionError.vector();
return camera.project(world_P_landmark) - measured_;
} catch( CheiralityException& e) {
std::cout << e.what()
<< ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key1()) << "," << DefaultKeyFormatter(this->key3()) << "]"
<< " moved behind camera " << DefaultKeyFormatter(this->key2())
<< std::endl;
return gtsam::Vector::Ones(2) * 2.0 * K_->fx();
return Vector::Ones(2) * 2.0 * K_->fx();
}
return (Vector(1) << 0.0).finished();
}
/// Evaluate error h(x)-z and optionally derivatives
Vector evaluateError(const Pose3& pose1, const Pose3& pose2, const Vector3& landmark,
boost::optional<gtsam::Matrix&> H1=boost::none,
boost::optional<gtsam::Matrix&> H2=boost::none,
boost::optional<gtsam::Matrix&> H3=boost::none) const {
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none,
boost::optional<Matrix&> H3=boost::none) const {
if(H1)
(*H1) = numericalDerivative11<Vector,Pose3>(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, _1, pose2, landmark), pose1);
@ -239,7 +241,7 @@ public:
}
/** return the measurement */
const gtsam::Point2& imagePoint() const {
const Point2& imagePoint() const {
return measured_;
}

22
gtsam_unstable/slam/MultiProjectionFactor.h
View File

@ -101,9 +101,9 @@ namespace gtsam {
virtual ~MultiProjectionFactor() {}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
virtual NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<NonlinearFactor>(
NonlinearFactor::shared_ptr(new This(*this))); }
/**
* print
@ -143,20 +143,20 @@ namespace gtsam {
//
// if(body_P_sensor_) {
// if(H1) {
// gtsam::Matrix H0;
// Matrix H0;
// PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_, H0), *K_);
// Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
// *H1 = *H1 * H0;
// return reprojectionError.vector();
// return reprojectionError;
// } else {
// PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_), *K_);
// Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
// return reprojectionError.vector();
// return reprojectionError;
// }
// } else {
// PinholeCamera<CALIBRATION> camera(pose, *K_);
// Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
// return reprojectionError.vector();
// return reprojectionError;
// }
// }
@ -168,20 +168,20 @@ namespace gtsam {
try {
if(body_P_sensor_) {
if(H1) {
gtsam::Matrix H0;
Matrix H0;
PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_, H0), *K_);
Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
*H1 = *H1 * H0;
return reprojectionError.vector();
return reprojectionError;
} else {
PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_), *K_);
Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
return reprojectionError.vector();
return reprojectionError;
}
} else {
PinholeCamera<CALIBRATION> camera(pose, *K_);
Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
return reprojectionError.vector();
return reprojectionError;
}
} catch( CheiralityException& e) {
if (H1) *H1 = Matrix::Zero(2,6);

17
gtsam_unstable/slam/ProjectionFactorPPP.h
View File

@ -54,7 +54,9 @@ namespace gtsam {
typedef boost::shared_ptr<This> shared_ptr;
/// Default constructor
ProjectionFactorPPP() : throwCheirality_(false), verboseCheirality_(false) {}
ProjectionFactorPPP() :
measured_(0.0, 0.0), throwCheirality_(false), verboseCheirality_(false) {
}
/**
* Constructor
@ -94,9 +96,9 @@ namespace gtsam {
virtual ~ProjectionFactorPPP() {}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
virtual NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<NonlinearFactor>(
NonlinearFactor::shared_ptr(new This(*this))); }
/**
* print
@ -125,16 +127,15 @@ namespace gtsam {
boost::optional<Matrix&> H3 = boost::none) const {
try {
if(H1 || H2 || H3) {
gtsam::Matrix H0, H02;
Matrix H0, H02;
PinholeCamera<CALIBRATION> camera(pose.compose(transform, H0, H02), *K_);
Point2 reprojectionError(camera.project(point, H1, H3, boost::none) - measured_);
*H2 = *H1 * H02;
*H1 = *H1 * H0;
return reprojectionError.vector();
return reprojectionError;
} else {
PinholeCamera<CALIBRATION> camera(pose.compose(transform), *K_);
Point2 reprojectionError(camera.project(point, H1, H3, boost::none) - measured_);
return reprojectionError.vector();
return camera.project(point, H1, H3, boost::none) - measured_;
}
} catch( CheiralityException& e) {
if (H1) *H1 = Matrix::Zero(2,6);

17
gtsam_unstable/slam/ProjectionFactorPPPC.h
View File

@ -52,7 +52,9 @@ namespace gtsam {
typedef boost::shared_ptr<This> shared_ptr;
/// Default constructor
ProjectionFactorPPPC() : throwCheirality_(false), verboseCheirality_(false) {}
ProjectionFactorPPPC() :
measured_(0.0, 0.0), throwCheirality_(false), verboseCheirality_(false) {
}
/**
* Constructor
@ -89,9 +91,9 @@ namespace gtsam {
virtual ~ProjectionFactorPPPC() {}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
virtual NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<NonlinearFactor>(
NonlinearFactor::shared_ptr(new This(*this))); }
/**
* print
@ -120,16 +122,15 @@ namespace gtsam {
boost::optional<Matrix&> H4 = boost::none) const {
try {
if(H1 || H2 || H3 || H4) {
gtsam::Matrix H0, H02;
Matrix H0, H02;
PinholeCamera<CALIBRATION> camera(pose.compose(transform, H0, H02), K);
Point2 reprojectionError(camera.project(point, H1, H3, H4) - measured_);
*H2 = *H1 * H02;
*H1 = *H1 * H0;
return reprojectionError.vector();
return reprojectionError;
} else {
PinholeCamera<CALIBRATION> camera(pose.compose(transform), K);
Point2 reprojectionError(camera.project(point, H1, H3, H4) - measured_);
return reprojectionError.vector();
return camera.project(point, H1, H3, H4) - measured_;
}
} catch( CheiralityException& e) {
if (H1) *H1 = Matrix::Zero(2,6);

4
gtsam_unstable/slam/TSAMFactors.h
View File

@ -48,9 +48,7 @@ public:
Vector evaluateError(const Pose2& pose, const Point2& point,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
boost::none) const {
Point2 d = pose.transform_to(point, H1, H2);
Point2 e = d - measured_;
return e.vector();
return pose.transform_to(point, H1, H2) - measured_;
}
};

2
matlab.h
View File

@ -91,7 +91,7 @@ Matrix extractPoint2(const Values& values) {
Values::ConstFiltered<Point2> points = values.filter<Point2>();
Matrix result(points.size(), 2);
for(const auto& key_value: points)
result.row(j++) = key_value.value.vector();
result.row(j++) = key_value.value;
return result;
}

4
tests/smallExample.h
View File

@ -180,7 +180,7 @@ inline boost::shared_ptr<const NonlinearFactorGraph> sharedNonlinearFactorGraph(
new NonlinearFactorGraph);
// prior on x1
Point2 mu;
Point2 mu(0,0);
shared_nlf f1(new simulated2D::Prior(mu, sigma0_1, X(1)));
nlfg->push_back(f1);
@ -597,7 +597,7 @@ inline boost::tuple<GaussianFactorGraph, VectorValues> planarGraph(size_t N) {
VectorValues xtrue;
for (size_t x = 1; x <= N; x++)
for (size_t y = 1; y <= N; y++)
xtrue.insert(key(x, y), Point2((double)x, (double)y).vector());
xtrue.insert(key(x, y), Point2((double)x, (double)y));
// linearize around zero
boost::shared_ptr<GaussianFactorGraph> gfg = nlfg.linearize(zeros);

6
tests/testBoundingConstraint.cpp
View File

@ -181,7 +181,7 @@ TEST( testBoundingConstraint, unary_simple_optimization2) {
/* ************************************************************************* */
TEST( testBoundingConstraint, MaxDistance_basics) {
gtsam::Key key1 = 1, key2 = 2;
Point2 pt1, pt2(1.0, 0.0), pt3(2.0, 0.0), pt4(3.0, 0.0);
Point2 pt1(0,0), pt2(1.0, 0.0), pt3(2.0, 0.0), pt4(3.0, 0.0);
iq2D::PoseMaxDistConstraint rangeBound(key1, key2, 2.0, mu);
EXPECT_DOUBLES_EQUAL(2.0, rangeBound.threshold(), tol);
EXPECT(!rangeBound.isGreaterThan());
@ -220,7 +220,7 @@ TEST( testBoundingConstraint, MaxDistance_basics) {
/* ************************************************************************* */
TEST( testBoundingConstraint, MaxDistance_simple_optimization) {
Point2 pt1, pt2_init(5.0, 0.0), pt2_goal(2.0, 0.0);
Point2 pt1(0,0), pt2_init(5.0, 0.0), pt2_goal(2.0, 0.0);
Symbol x1('x',1), x2('x',2);
NonlinearFactorGraph graph;
@ -246,7 +246,7 @@ TEST( testBoundingConstraint, avoid_demo) {
Symbol x1('x',1), x2('x',2), x3('x',3), l1('l',1);
double radius = 1.0;
Point2 x1_pt, x2_init(2.0, 0.5), x2_goal(2.0, 1.0), x3_pt(4.0, 0.0), l1_pt(2.0, 0.0);
Point2 x1_pt(0,0), x2_init(2.0, 0.5), x2_goal(2.0, 1.0), x3_pt(4.0, 0.0), l1_pt(2.0, 0.0);
Point2 odo(2.0, 0.0);
NonlinearFactorGraph graph;

2
tests/testExtendedKalmanFilter.cpp
View File

@ -400,7 +400,7 @@ TEST( ExtendedKalmanFilter, nonlinear ) {
ExtendedKalmanFilter<Point2> ekf(X(0), x_initial, P_initial);
// Enter Predict-Update Loop
Point2 x_predict, x_update;
Point2 x_predict(0,0), x_update(0,0);
for(unsigned int i = 0; i < 10; ++i){
// Create motion factor
NonlinearMotionModel motionFactor(X(i), X(i+1));

2
tests/testManifold.cpp
View File

@ -166,7 +166,7 @@ template<> struct traits<MyPoint2Pair> : internal::ManifoldTraits<MyPoint2Pair>
TEST(Manifold, ProductManifold) {
BOOST_CONCEPT_ASSERT((IsManifold<MyPoint2Pair>));
MyPoint2Pair pair1;
MyPoint2Pair pair1(Point2(0,0),Point2(0,0));
Vector4 d;
d << 1,2,3,4;
MyPoint2Pair expected(Point2(1,2),Point2(3,4));