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

Split .h/.cpp, use const& (WIP)

release/4.3a0
Toni 2021-01-17 11:08:53 -05:00
parent b384b0cee4
commit 19b7312edb
4 changed files with 159 additions and 93 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
examples/CMakeLists.txt
View File

@ -2,4 +2,4 @@ set (excluded_examples
elaboratePoint2KalmanFilter.cpp
)
gtsamAddExamplesGlob("*.cpp" "${excluded_examples}" "gtsam;${Boost_PROGRAM_OPTIONS_LIBRARY}")
gtsamAddExamplesGlob("*.cpp" "${excluded_examples}" "gtsam;gtsam_unstable;${Boost_PROGRAM_OPTIONS_LIBRARY}")

6
gtsam/slam/SmartFactorBase.h
View File

@ -13,6 +13,7 @@
* @file SmartFactorBase.h
* @brief Base class to create smart factors on poses or cameras
* @author Luca Carlone
* @author Antoni Rosinol
* @author Zsolt Kira
* @author Frank Dellaert
* @author Chris Beall
@ -131,9 +132,10 @@ protected:
/**
* Add a bunch of measurements, together with the camera keys
*/
void add(ZVector& measurements, KeyVector& cameraKeys) {
void add(const ZVector& measurements, const KeyVector& cameraKeys) {
assert(measurements.size() == cameraKeys.size());
for (size_t i = 0; i < measurements.size(); i++) {
this->add(measurements.at(i), cameraKeys.at(i));
this->add(measurements[i], cameraKeys[i]);
}
}

97
gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp Normal file
View File

@ -0,0 +1,97 @@
/* ----------------------------------------------------------------------------
* 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 SmartStereoProjectionPoseFactor.cpp
* @brief Smart stereo factor on poses, assuming camera calibration is fixed
* @author Luca Carlone
* @author Antoni Rosinol
* @author Chris Beall
* @author Zsolt Kira
* @author Frank Dellaert
*/
#include <gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h>
namespace gtsam {
SmartStereoProjectionPoseFactor::SmartStereoProjectionPoseFactor(
const SharedNoiseModel& sharedNoiseModel,
const SmartStereoProjectionParams& params,
const boost::optional<Pose3>& body_P_sensor)
: Base(sharedNoiseModel, params, body_P_sensor) {}
void SmartStereoProjectionPoseFactor::add(
const StereoPoint2& measured, const Key& poseKey,
const boost::shared_ptr<Cal3_S2Stereo>& K) {
Base::add(measured, poseKey);
K_all_.push_back(K);
}
void SmartStereoProjectionPoseFactor::add(
const std::vector<StereoPoint2>& measurements, const KeyVector& poseKeys,
const std::vector<boost::shared_ptr<Cal3_S2Stereo>>& Ks) {
assert(measurements.size() == poseKeys.size());
assert(poseKeys.size() == Ks.size());
Base::add(measurements, poseKeys);
K_all_.insert(K_all_.end(), Ks.begin(), Ks.end());
}
void SmartStereoProjectionPoseFactor::add(
const std::vector<StereoPoint2>& measurements, const KeyVector& poseKeys,
const boost::shared_ptr<Cal3_S2Stereo>& K) {
assert(poseKeys.size() == measurements.size());
for (size_t i = 0; i < measurements.size(); i++) {
Base::add(measurements[i], poseKeys[i]);
K_all_.push_back(K);
}
}
void SmartStereoProjectionPoseFactor::print(
const std::string& s, const KeyFormatter& keyFormatter) const {
std::cout << s << "SmartStereoProjectionPoseFactor, z = \n ";
for (const boost::shared_ptr<Cal3_S2Stereo>& K : K_all_) {
K->print("calibration = ");
}
Base::print("", keyFormatter);
}
bool SmartStereoProjectionPoseFactor::equals(const NonlinearFactor& p,
double tol) const {
const SmartStereoProjectionPoseFactor* e =
dynamic_cast<const SmartStereoProjectionPoseFactor*>(&p);
return e && Base::equals(p, tol);
}
double SmartStereoProjectionPoseFactor::error(const Values& values) const {
if (this->active(values)) {
return this->totalReprojectionError(cameras(values));
} else { // else of active flag
return 0.0;
}
}
SmartStereoProjectionPoseFactor::Base::Cameras
SmartStereoProjectionPoseFactor::cameras(const Values& values) const {
assert(keys_.size() == K_all_.size());
Base::Cameras cameras;
for (size_t i = 0; i < keys_.size(); i++) {
Pose3 pose = values.at<Pose3>(keys_[i]);
if (Base::body_P_sensor_) {
pose = pose.compose(*(Base::body_P_sensor_));
}
cameras.push_back(StereoCamera(pose, K_all_[i]));
}
return cameras;
}
} // \ namespace gtsam

147
gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h
View File

@ -1,6 +1,7 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corpo
* ation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -13,6 +14,7 @@
* @file SmartStereoProjectionPoseFactor.h
* @brief Smart stereo factor on poses, assuming camera calibration is fixed
* @author Luca Carlone
* @author Antoni Rosinol
* @author Chris Beall
* @author Zsolt Kira
* @author Frank Dellaert
@ -28,8 +30,10 @@ namespace gtsam {
* @addtogroup SLAM
*
* If you are using the factor, please cite:
* L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally
* independent sets in factor graphs: a unifying perspective based on smart factors,
* L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating
* conditionally
* independent sets in factor graphs: a unifying perspective based on smart
* factors,
* Int. Conf. on Robotics and Automation (ICRA), 2014.
*
*/
@ -41,14 +45,12 @@ namespace gtsam {
* This factor requires that values contains the involved poses (Pose3).
* @addtogroup SLAM
*/
class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor {
protected:
std::vector<boost::shared_ptr<Cal3_S2Stereo> > K_all_; ///< shared pointer to calibration object (one for each camera)
public:
class SmartStereoProjectionPoseFactor : public SmartStereoProjectionFactor {
protected:
/// shared pointer to calibration object (one for each camera)
std::vector<boost::shared_ptr<Cal3_S2Stereo>> K_all_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// shorthand for base class type
@ -65,129 +67,94 @@ public:
* @param Isotropic measurement noise
* @param params internal parameters of the smart factors
*/
SmartStereoProjectionPoseFactor(const SharedNoiseModel& sharedNoiseModel,
SmartStereoProjectionPoseFactor(
const SharedNoiseModel& sharedNoiseModel,
const SmartStereoProjectionParams& params = SmartStereoProjectionParams(),
const boost::optional<Pose3> body_P_sensor = boost::none) :
Base(sharedNoiseModel, params, body_P_sensor) {
}
const boost::optional<Pose3>& body_P_sensor = boost::none);
/** Virtual destructor */
virtual ~SmartStereoProjectionPoseFactor() {}
virtual ~SmartStereoProjectionPoseFactor() = default;
/**
* add a new measurement and pose key
* @param measured is the 2m dimensional location of the projection of a single landmark in the m view (the measurement)
* @param poseKey is key corresponding to the camera observing the same landmark
* @param measured is the 2m dimensional location of the projection of a
* single landmark in the m view (the measurement)
* @param poseKey is key corresponding to the camera observing the same
* landmark
* @param K is the (fixed) camera calibration
*/
void add(const StereoPoint2 measured, const Key poseKey,
const boost::shared_ptr<Cal3_S2Stereo> K) {
Base::add(measured, poseKey);
K_all_.push_back(K);
}
void add(const StereoPoint2& measured, const Key& poseKey,
const boost::shared_ptr<Cal3_S2Stereo>& K);
/**
* Variant of the previous one in which we include a set of measurements
* @param measurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement)
* @param poseKeys vector of keys corresponding to the camera observing the same landmark
* @param measurements vector of the 2m dimensional location of the projection
* of a single landmark in the m view (the measurement)
* @param poseKeys vector of keys corresponding to the camera observing
* the same landmark
* @param Ks vector of calibration objects
*/
void add(std::vector<StereoPoint2> measurements, KeyVector poseKeys,
std::vector<boost::shared_ptr<Cal3_S2Stereo> > Ks) {
Base::add(measurements, poseKeys);
for (size_t i = 0; i < measurements.size(); i++) {
K_all_.push_back(Ks.at(i));
}
}
void add(const std::vector<StereoPoint2>& measurements,
const KeyVector& poseKeys,
const std::vector<boost::shared_ptr<Cal3_S2Stereo>>& Ks);
/**
* Variant of the previous one in which we include a set of measurements with the same noise and calibration
* @param measurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement)
* @param poseKeys vector of keys corresponding to the camera observing the same landmark
* Variant of the previous one in which we include a set of measurements with
* the same noise and calibration
* @param measurements vector of the 2m dimensional location of the projection
* of a single landmark in the m view (the measurement)
* @param poseKeys vector of keys corresponding to the camera observing the
* same landmark
* @param K the (known) camera calibration (same for all measurements)
*/
void add(std::vector<StereoPoint2> measurements, KeyVector poseKeys,
const boost::shared_ptr<Cal3_S2Stereo> K) {
for (size_t i = 0; i < measurements.size(); i++) {
Base::add(measurements.at(i), poseKeys.at(i));
K_all_.push_back(K);
}
}
void add(const std::vector<StereoPoint2>& measurements,
const KeyVector& poseKeys,
const boost::shared_ptr<Cal3_S2Stereo>& K);
/**
* print
* @param s optional string naming the factor
* @param keyFormatter optional formatter useful for printing Symbols
*/
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override {
std::cout << s << "SmartStereoProjectionPoseFactor, z = \n ";
for(const boost::shared_ptr<Cal3_S2Stereo>& K: K_all_)
K->print("calibration = ");
Base::print("", keyFormatter);
}
*/ void print(
const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
/// equals
bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
const SmartStereoProjectionPoseFactor *e =
dynamic_cast<const SmartStereoProjectionPoseFactor*>(&p);
return e && Base::equals(p, tol);
}
virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const;
/**
* error calculates the error of the factor.
*/
double error(const Values& values) const override {
if (this->active(values)) {
return this->totalReprojectionError(cameras(values));
} else { // else of active flag
return 0.0;
}
}
double error(const Values& values) const override;
/** return the calibration object */
inline const std::vector<boost::shared_ptr<Cal3_S2Stereo> > calibration() const {
inline std::vector<boost::shared_ptr<Cal3_S2Stereo>> calibration() const {
return K_all_;
}
/**
* Collect all cameras involved in this factor
* @param values Values structure which must contain camera poses corresponding
* @param values Values structure which must contain camera poses
* corresponding
* to keys involved in this factor
* @return vector of Values
*/
Base::Cameras cameras(const Values& values) const override {
Base::Cameras cameras;
size_t i=0;
for(const Key& k: this->keys_) {
Pose3 pose = values.at<Pose3>(k);
if (Base::body_P_sensor_)
pose = pose.compose(*(Base::body_P_sensor_));
StereoCamera camera(pose, K_all_[i++]);
cameras.push_back(camera);
}
return cameras;
}
private:
Base::Cameras cameras(const Values& values) const override;
private:
/// Serialization function
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(K_all_);
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar& BOOST_SERIALIZATION_NVP(K_all_);
}
}; // end of class declaration
}; // end of class declaration
/// traits
template<>
struct traits<SmartStereoProjectionPoseFactor> : public Testable<
SmartStereoProjectionPoseFactor> {
};
template <>
struct traits<SmartStereoProjectionPoseFactor>
: public Testable<SmartStereoProjectionPoseFactor> {};
} // \ namespace gtsam
} // \ namespace gtsam