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

equals/print compile

release/4.3a0
dellaert 2015-02-19 18:05:30 +01:00
parent bc1da8577f
commit 0a3272dab6
2 changed files with 35 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
gtsam/geometry/CameraSet.h
View File

@ -20,6 +20,7 @@
#include <gtsam/geometry/Point3.h> #include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/CalibratedCamera.h> // for Cheirality exception #include <gtsam/geometry/CalibratedCamera.h> // for Cheirality exception
#include <gtsam/base/Testable.h>
#include <vector> #include <vector>
namespace gtsam { namespace gtsam {
@ -53,17 +54,16 @@ public:
CameraSet() { CameraSet() {
} }
/** Virtual destructor */ /// Add a new camera
virtual ~CameraSet() {
}
/**
* Add a new camera
*/
void add(const CAMERA& camera) { void add(const CAMERA& camera) {
cameras_.push_back(camera); cameras_.push_back(camera);
} }
/// Retrieve ith camera
const CAMERA& operator[](size_t i) const {
return cameras_[i];
}
/** /**
* print * print
* @param s optional string naming the factor * @param s optional string naming the factor
@ -72,7 +72,7 @@ public:
void print(const std::string& s = "") const { void print(const std::string& s = "") const {
std::cout << s << "CameraSet, cameras = \n"; std::cout << s << "CameraSet, cameras = \n";
for (size_t k = 0; k < cameras_.size(); ++k) for (size_t k = 0; k < cameras_.size(); ++k)
cameras_[k]->print(); cameras_[k].print();
} }
/// equals /// equals
@ -90,13 +90,17 @@ public:
* project, with derivatives in this, point, and calibration * project, with derivatives in this, point, and calibration
* throws CheiralityException * throws CheiralityException
*/ */
std::vector<Z> project(const Point3& point, boost::optional<Matrix&> F=boost::none, std::vector<Z> project(const Point3& point, boost::optional<Matrix&> F =
boost::optional<Matrix&> E=boost::none, boost::optional<Matrix&> H=boost::none) const { boost::none, boost::optional<Matrix&> E = boost::none,
boost::optional<Matrix&> H = boost::none) const {
size_t nrCameras = cameras_.size(); size_t nrCameras = cameras_.size();
if (F) F->resize(ZDim * nrCameras, 6); if (F)
if (E) E->resize(ZDim * nrCameras, 3); F->resize(ZDim * nrCameras, 6);
if (H && Dim>6) H->resize(ZDim * nrCameras, Dim - 6); if (E)
E->resize(ZDim * nrCameras, 3);
if (H && Dim > 6)
H->resize(ZDim * nrCameras, Dim - 6);
std::vector<Z> z(nrCameras); std::vector<Z> z(nrCameras);
for (size_t i = 0; i < cameras_.size(); i++) { for (size_t i = 0; i < cameras_.size(); i++) {
@ -104,9 +108,12 @@ public:
Eigen::Matrix<double, ZDim, 3> Ei; Eigen::Matrix<double, ZDim, 3> Ei;
Eigen::Matrix<double, ZDim, Dim - 6> Hi; Eigen::Matrix<double, ZDim, Dim - 6> Hi;
z[i] = cameras_[i].project(point, F ? &Fi : 0, E ? &Ei : 0, H ? &Hi : 0); z[i] = cameras_[i].project(point, F ? &Fi : 0, E ? &Ei : 0, H ? &Hi : 0);
if (F) F->block<ZDim, 6>(ZDim * i, 0) = Fi; if (F)
if (E) E->block<ZDim, 3>(ZDim * i, 0) = Ei; F->block<ZDim, 6>(ZDim * i, 0) = Fi;
if (H) H->block<ZDim, Dim - 6>(ZDim * i, 0) = Hi; if (E)
E->block<ZDim, 3>(ZDim * i, 0) = Ei;
if (H)
H->block<ZDim, Dim - 6>(ZDim * i, 0) = Hi;
} }
return z; return z;
} }
@ -124,4 +131,12 @@ private:
template<class CAMERA> template<class CAMERA>
const int CameraSet<CAMERA>::ZDim; const int CameraSet<CAMERA>::ZDim;
template<class CAMERA>
struct traits<CameraSet<CAMERA> > : public Testable<CameraSet<CAMERA> > {
};
template<class CAMERA>
struct traits<const CameraSet<CAMERA> > : public Testable<CameraSet<CAMERA> > {
};
} // \ namespace gtsam } // \ namespace gtsam

4
gtsam/geometry/tests/testCameraSet.cpp
View File

@ -37,6 +37,10 @@ TEST(CameraSet, Pinhole) {
set.add(camera); set.add(camera);
set.add(camera); set.add(camera);
Point3 p(0, 0, 1); Point3 p(0, 0, 1);
CHECK(assert_equal(set,set));
CameraSet<Camera> set2 = set;
set2.add(camera);
CHECK(!assert_equal(set,set2));
// Check measurements // Check measurements
Point2 expected; Point2 expected;