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

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

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