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

made templated and cleaned up some more

release/4.3a0
Chris Beall 2010-11-15 16:43:23 +00:00
parent 651db9240b
commit 6c41184d19
1 changed files with 70 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
gtsam/slam/StereoFactor.h
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */
/**
* @file StereoFactor.h
* @file GenericStereoFactor.h
* @brief A non-linear factor for stereo measurements
* @author Alireza Fathi
* @author Chris Beall
@ -26,7 +26,8 @@ namespace gtsam {
using namespace gtsam;
using namespace gtsam::visualSLAM;
class StereoFactor: public NonlinearFactor2<Values, PoseKey, PointKey>, Testable<StereoFactor> {
template<class VALUES=Values, class KEY1=PoseKey, class KEY2=PointKey>
class GenericStereoFactor: public NonlinearFactor2<VALUES, KEY1, KEY2> {
private:
// Keep a copy of measurement and calibration for I/O
@ -37,15 +38,15 @@ private:
public:
// shorthand for base class type
typedef NonlinearFactor2<Values, PoseKey, PointKey> Base;
typedef NonlinearFactor2<VALUES, KEY1, KEY2> Base;
// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<StereoFactor> shared_ptr;
typedef boost::shared_ptr<GenericStereoFactor> shared_ptr;
/**
* Default constructor
*/
StereoFactor() : K_(new Cal3_S2(444, 555, 666, 777, 888)) {}
GenericStereoFactor() : K_(new Cal3_S2(444, 555, 666, 777, 888)) {}
/**
* Constructor
@ -55,31 +56,33 @@ public:
* @param landmarkNumber is the index of the landmark
* @param K the constant calibration
*/
StereoFactor(const StereoPoint2& z, const SharedGaussian& model, PoseKey j_pose, PointKey j_landmark, const shared_ptrK& K, double baseline) :
Base(model, j_pose, j_landmark), z_(z), K_(K), baseline_(baseline) {}
GenericStereoFactor(const StereoPoint2& z, const SharedGaussian& model, KEY1 j_pose,
KEY2 j_landmark, const shared_ptrK& K, double baseline) :
Base(model, j_pose, j_landmark), z_(z), K_(K), baseline_(baseline) {
}
/**
* print
* @param s optional string naming the factor
*/
void print(const std::string& s) const {
printf("%s %s %s\n", s.c_str(), ((string) key1_).c_str(),
((string) key2_).c_str());
z_.print(s+".z");
Base::print(s);
z_.print(s + ".z");
}
/**
* equals
*/
bool equals(const StereoFactor& f, double tol) const {
const StereoFactor* p = dynamic_cast<const StereoFactor*>(&f);
if (p == NULL) goto fail;
bool equals(const GenericStereoFactor& f, double tol) const {
const GenericStereoFactor* p = dynamic_cast<const GenericStereoFactor*> (&f);
if (p == NULL)
goto fail;
//if (cameraFrameNumber_ != p->cameraFrameNumber_ || landmarkNumber_ != p->landmarkNumber_) goto fail;
if (!z_.equals(p->z_,tol)) goto fail;
if (!z_.equals(p->z_, tol))
goto fail;
return true;
fail:
print("actual");
fail: print("actual");
p->print("expected");
return false;
}
@ -87,25 +90,26 @@ public:
/** h(x)-z */
Vector evaluateError(const Pose3& pose, const Point3& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
const Cal3_S2& K = *K_;
StereoCamera stereoCam(pose, K, baseline_);
return (stereoCam.project(point,H1,H2) - z_).vector();
StereoCamera stereoCam(pose, *K_, baseline_);
return (stereoCam.project(point, H1, H2) - z_).vector();
}
StereoPoint2 z(){return z_;}
StereoPoint2 z() {
return z_;
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
//ar & BOOST_SERIALIZATION_NVP(key1_);
//ar & BOOST_SERIALIZATION_NVP(key2_);
ar & BOOST_SERIALIZATION_NVP(z_);
ar & BOOST_SERIALIZATION_NVP(K_);
ar & BOOST_SERIALIZATION_NVP(baseline_);
}
};
// Typedef for general use
typedef GenericStereoFactor<Values, PoseKey, PointKey> StereoFactor;
}