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

Make it work with StereoCamera

release/4.3a0
dellaert 2015-02-19 17:19:59 +01:00
parent 9a5f39b55b
commit bc1da8577f
2 changed files with 46 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
gtsam/geometry/CameraSet.h
View File

@ -20,14 +20,13 @@
#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 <boost/foreach.hpp>
#include <vector> #include <vector>
namespace gtsam { namespace gtsam {
/** /**
* @brief A set of cameras, all with their own calibration * @brief A set of cameras, all with their own calibration
* Assumes that a camera is laid out as 6 Pose3 parameters then calibration
*/ */
template<class CAMERA> template<class CAMERA>
class CameraSet { class CameraSet {
@ -91,13 +90,13 @@ 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, std::vector<Z> project(const Point3& point, boost::optional<Matrix&> F=boost::none,
boost::optional<Matrix&> E, boost::optional<Matrix&> H) const { 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) F->resize(ZDim * nrCameras, 6);
if (E) E->resize(ZDim * nrCameras, 3); if (E) E->resize(ZDim * nrCameras, 3);
if (H) H->resize(ZDim * nrCameras, Dim - 6); 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++) {

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

@ -26,6 +26,7 @@ using namespace std;
using namespace gtsam; using namespace gtsam;
/* ************************************************************************* */ /* ************************************************************************* */
// Cal3Bundler test
#include <gtsam/geometry/PinholeCamera.h> #include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3Bundler.h> #include <gtsam/geometry/Cal3Bundler.h>
TEST(CameraSet, Pinhole) { TEST(CameraSet, Pinhole) {
@ -36,7 +37,14 @@ 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);
// Calculate actual
// Check measurements
Point2 expected;
ZZ z = set.project(p);
CHECK(assert_equal(expected, z[0]));
CHECK(assert_equal(expected, z[1]));
// Calculate expected derivatives using Pinhole
Matrix46 actualF; Matrix46 actualF;
Matrix43 actualE; Matrix43 actualE;
Matrix43 actualH; Matrix43 actualH;
@ -49,11 +57,10 @@ TEST(CameraSet, Pinhole) {
actualF << F1, F1; actualF << F1, F1;
actualH << H1, H1; actualH << H1, H1;
} }
Point2 expected;
// Check computed derivatives
Matrix F, E, H; Matrix F, E, H;
ZZ z = set.project(p, F, E, H); set.project(p, F, E, H);
CHECK(assert_equal(expected, z[0]));
CHECK(assert_equal(expected, z[1]));
CHECK(assert_equal(actualF, F)); CHECK(assert_equal(actualF, F));
CHECK(assert_equal(actualE, E)); CHECK(assert_equal(actualE, E));
CHECK(assert_equal(actualH, H)); CHECK(assert_equal(actualH, H));
@ -62,7 +69,36 @@ TEST(CameraSet, Pinhole) {
/* ************************************************************************* */ /* ************************************************************************* */
#include <gtsam/geometry/StereoCamera.h> #include <gtsam/geometry/StereoCamera.h>
TEST(CameraSet, Stereo) { TEST(CameraSet, Stereo) {
CameraSet<StereoCamera> f; typedef vector<StereoPoint2> ZZ;
CameraSet<StereoCamera> set;
StereoCamera camera;
set.add(camera);
set.add(camera);
Point3 p(0, 0, 1);
EXPECT_LONGS_EQUAL(6,traits<StereoCamera>::dimension);
// Check measurements
StereoPoint2 expected;
ZZ z = set.project(p);
CHECK(assert_equal(expected, z[0]));
CHECK(assert_equal(expected, z[1]));
// Calculate expected derivatives using Pinhole
Matrix46 actualF;
Matrix43 actualE;
{
Matrix36 F1;
Matrix33 E1;
camera.project(p, F1, E1);
actualE << E1, E1;
actualF << F1, F1;
}
// Check computed derivatives
Matrix F, E;
set.project(p, F, E);
CHECK(assert_equal(actualF, F));
CHECK(assert_equal(actualE, E));
} }
/* ************************************************************************* */ /* ************************************************************************* */