diff --git a/.cproject b/.cproject index 0bef605c5..0a3e306f4 100644 --- a/.cproject +++ b/.cproject @@ -1011,6 +1011,14 @@ true true + + make + -j4 + testCameraSet.run + true + true + true + make -j2 diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h new file mode 100644 index 000000000..6d64ca045 --- /dev/null +++ b/gtsam/geometry/CameraSet.h @@ -0,0 +1,126 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file CameraSet.h + * @brief Base class to create smart factors on poses or cameras + * @author Frank Dellaert + * @date Feb 19, 2015 + */ + +#pragma once + +#include +#include // for Cheirality exception +#include + +#include + +namespace gtsam { + +/** + * @brief A set of cameras, all with their own calibration + */ +template +class CameraSet { + +private: + + std::vector cameras_; + + /** + * 2D measurement and noise model for each of the m views + * The order is kept the same as the keys that we use to create the factor. + */ + typedef typename CAMERA::Measurement Z; + + static const int ZDim = traits::dimension; ///< Measurement dimension + static const int Dim = traits::dimension; ///< Camera dimension + + /// shorthand for this class + typedef CameraSet This; + +public: + + /// Default Constructor + CameraSet() { + } + + /** Virtual destructor */ + virtual ~CameraSet() { + } + + /** + * Add a new camera + */ + void add(const CAMERA& camera) { + cameras_.push_back(camera); + } + + /** + * print + * @param s optional string naming the factor + * @param keyFormatter optional formatter useful for printing Symbols + */ + 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(); + } + + /// equals + virtual bool equals(const CameraSet& p, double tol = 1e-9) const { + bool camerasAreEqual = true; + for (size_t i = 0; i < cameras_.size(); i++) { + if (cameras_.at(i).equals(p.cameras_.at(i), tol) == false) + camerasAreEqual = false; + break; + } + return camerasAreEqual; + } + + /** + * project, with derivatives in this, point, and calibration + * throws CheiralityException + */ + std::vector project(const Point3& point, boost::optional F, + boost::optional E, boost::optional H) const { + + size_t nrCameras = cameras_.size(); + if (F) F->resize(ZDim * nrCameras, 6); + if (E) E->resize(ZDim * nrCameras, 3); + if (H) H->resize(ZDim * nrCameras, Dim - 6); + std::vector z(nrCameras); + + for (size_t i = 0; i < cameras_.size(); i++) { + Matrix Fi(ZDim, 6), Ei(ZDim, 3), Hi(ZDim, Dim - 6); + z[i] = cameras_[i].project(point, F ? &Fi : 0, E ? &Ei : 0, H ? &Hi : 0); + if (F) F->block(ZDim * i, 0) = Fi; + if (E) E->block(ZDim * i, 0) = Ei; + if (H) H->block(ZDim * i, 0) = Hi; + } + return z; + } + +private: + + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & cameras_; + } +}; + +template +const int CameraSet::ZDim; + +} // \ namespace gtsam diff --git a/gtsam/geometry/tests/testCameraSet.cpp b/gtsam/geometry/tests/testCameraSet.cpp new file mode 100644 index 000000000..730d7fa36 --- /dev/null +++ b/gtsam/geometry/tests/testCameraSet.cpp @@ -0,0 +1,51 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file testCameraSet.cpp + * @brief Unit tests for testCameraSet Class + * @author Frank Dellaert + * @date Feb 19, 2015 + */ + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +#include +#include +class PinholeSet: public CameraSet > { +}; + +TEST(CameraSet, Pinhole) { + PinholeSet f; +} + +/* ************************************************************************* */ +#include +class StereoSet: public CameraSet { +}; + +TEST(CameraSet, Stereo) { + StereoSet f; +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ +