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

136
gtsam/slam/StereoFactor.h
View File

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