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

remove calibration template from SmartStereoProjectionFactor

release/4.3a0
cbeall3 2015-07-28 14:56:45 -04:00
parent db64b48fda
commit 748877ff7e
2 changed files with 11 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
gtsam_unstable/slam/SmartStereoProjectionFactor.h
View File

@ -126,12 +126,10 @@ enum DegeneracyMode {
* This factor operates with StereoCamrea. This factor requires that values * This factor operates with StereoCamrea. This factor requires that values
* contains the involved camera poses. Calibration is assumed to be fixed. * contains the involved camera poses. Calibration is assumed to be fixed.
*/ */
template<class CALIBRATION>
class SmartStereoProjectionFactor: public SmartFactorBase<StereoCamera> { class SmartStereoProjectionFactor: public SmartFactorBase<StereoCamera> {
private: private:
typedef SmartFactorBase<StereoCamera> Base; typedef SmartFactorBase<StereoCamera> Base;
typedef SmartStereoProjectionFactor<CALIBRATION> This;
protected: protected:
@ -149,7 +147,7 @@ protected:
public: public:
/// shorthand for a smart pointer to a factor /// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr; typedef boost::shared_ptr<SmartStereoProjectionFactor> shared_ptr;
/// Vector of cameras /// Vector of cameras
typedef CameraSet<StereoCamera> Cameras; typedef CameraSet<StereoCamera> Cameras;
@ -184,7 +182,8 @@ public:
/// equals /// equals
virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
const This *e = dynamic_cast<const This*>(&p); const SmartStereoProjectionFactor *e =
dynamic_cast<const SmartStereoProjectionFactor*>(&p);
return e && params_.linearizationMode == e->params_.linearizationMode return e && params_.linearizationMode == e->params_.linearizationMode
&& Base::equals(p, tol); && Base::equals(p, tol);
} }
@ -467,7 +466,7 @@ public:
/// Assumes the point has been computed /// Assumes the point has been computed
/// Note E can be 2m*3 or 2m*2, in case point is degenerate /// Note E can be 2m*3 or 2m*2, in case point is degenerate
void computeJacobiansWithTriangulatedPoint( void computeJacobiansWithTriangulatedPoint(
std::vector<typename Base::MatrixZD>& Fblocks, Matrix& E, Vector& b, std::vector<Base::MatrixZD>& Fblocks, Matrix& E, Vector& b,
const Cameras& cameras) const { const Cameras& cameras) const {
if (!result_) { if (!result_) {
@ -486,7 +485,7 @@ public:
/// Version that takes values, and creates the point /// Version that takes values, and creates the point
bool triangulateAndComputeJacobians( bool triangulateAndComputeJacobians(
std::vector<typename Base::MatrixZD>& Fblocks, Matrix& E, Vector& b, std::vector<Base::MatrixZD>& Fblocks, Matrix& E, Vector& b,
const Values& values) const { const Values& values) const {
Cameras cameras = this->cameras(values); Cameras cameras = this->cameras(values);
bool nonDegenerate = triangulateForLinearize(cameras); bool nonDegenerate = triangulateForLinearize(cameras);
@ -497,7 +496,7 @@ public:
/// takes values /// takes values
bool triangulateAndComputeJacobiansSVD( bool triangulateAndComputeJacobiansSVD(
std::vector<typename Base::MatrixZD>& Fblocks, Matrix& Enull, Vector& b, std::vector<Base::MatrixZD>& Fblocks, Matrix& Enull, Vector& b,
const Values& values) const { const Values& values) const {
Cameras cameras = this->cameras(values); Cameras cameras = this->cameras(values);
bool nonDegenerate = triangulateForLinearize(cameras); bool nonDegenerate = triangulateForLinearize(cameras);
@ -595,9 +594,9 @@ private:
}; };
/// traits /// traits
template<class CALIBRATION> template<>
struct traits<SmartStereoProjectionFactor<CALIBRATION> > : public Testable< struct traits<SmartStereoProjectionFactor > : public Testable<
SmartStereoProjectionFactor<CALIBRATION> > { SmartStereoProjectionFactor> {
}; };
} // \ namespace gtsam } // \ namespace gtsam

7
gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h
View File

@ -39,10 +39,7 @@ namespace gtsam {
* @addtogroup SLAM * @addtogroup SLAM
*/ */
template<class CALIBRATION> template<class CALIBRATION>
class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor<CALIBRATION> { class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor {
public:
protected: protected:
@ -53,7 +50,7 @@ public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// shorthand for base class type /// shorthand for base class type
typedef SmartStereoProjectionFactor<CALIBRATION> Base; typedef SmartStereoProjectionFactor Base;
/// shorthand for this class /// shorthand for this class
typedef SmartStereoProjectionPoseFactor<CALIBRATION> This; typedef SmartStereoProjectionPoseFactor<CALIBRATION> This;