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

made cameras virtual and simplified pose factor (with Frank)

release/4.3a0
Luca 2015-06-19 15:33:18 -04:00
parent dcce21639f
commit 2d9fddbcaa
2 changed files with 3 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/slam/SmartFactorBase.h
View File

@ -142,7 +142,7 @@ public:
} }
/** /**
* Add a bunch of measurements and uses the same noise model for all of them * Add a bunch of measurements and use the same noise model for all of them
*/ */
void add(std::vector<Z>& measurements, std::vector<Key>& cameraKeys, void add(std::vector<Z>& measurements, std::vector<Key>& cameraKeys,
const SharedNoiseModel& noise) { const SharedNoiseModel& noise) {
@ -177,7 +177,7 @@ public:
} }
/// Collect all cameras: important that in key order /// Collect all cameras: important that in key order
Cameras cameras(const Values& values) const { virtual Cameras cameras(const Values& values) const {
Cameras cameras; Cameras cameras;
BOOST_FOREACH(const Key& k, this->keys_) BOOST_FOREACH(const Key& k, this->keys_)
cameras.push_back(values.at<CAMERA>(k)); cameras.push_back(values.at<CAMERA>(k));
@ -214,7 +214,7 @@ public:
return e && Base::equals(p, tol) && areMeasurementsEqual; return e && Base::equals(p, tol) && areMeasurementsEqual;
} }
///Compute reprojection errors [h(x)-z] = [cameras.project(p)-z] and derivatives /// Compute reprojection errors [h(x)-z] = [cameras.project(p)-z] and derivatives
template<class POINT> template<class POINT>
Vector unwhitenedError(const Cameras& cameras, const POINT& point, Vector unwhitenedError(const Cameras& cameras, const POINT& point,
boost::optional<typename Cameras::FBlocks&> Fs = boost::none, // boost::optional<typename Cameras::FBlocks&> Fs = boost::none, //

27
gtsam/slam/SmartProjectionPoseFactor.h
View File

@ -103,24 +103,6 @@ public:
return e && Base::equals(p, tol); return e && Base::equals(p, tol);
} }
/**
* Linearize to Gaussian Factor
* @param values Values structure which must contain camera poses for this factor
* @return a Gaussian factor
*/
boost::shared_ptr<GaussianFactor> linearizeDamped(const Values& values,
const double lambda = 0.0) const {
// depending on flag set on construction we may linearize to different linear factors
typename Base::Cameras cameras = this->cameras(values);
return Base::linearizeDamped(cameras, lambda);
}
/// linearize
virtual boost::shared_ptr<GaussianFactor> linearize(
const Values& values) const {
return linearizeDamped(values);
}
/** /**
* error calculates the error of the factor. * error calculates the error of the factor.
*/ */
@ -156,15 +138,6 @@ public:
return cameras; return cameras;
} }
/**
* Triangulate and compute derivative of error with respect to point
* @return whether triangulation worked
*/
bool triangulateAndComputeE(Matrix& E, const Values& values) const {
typename Base::Cameras cameras = this->cameras(values);
return Base::triangulateAndComputeE(E, cameras);
}
private: private:
/// Serialization function /// Serialization function