i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-07 18:47:48 +08:00
Code Issues Packages Projects Releases Wiki Activity

Marker priors (#859)

Browse Source 
* Added MarkerPriors parameter

* Fixed Marker/Priors format to use '|' instead ';'. Fixed landmark priors not used.

* Marker: added priors variance parameters

* g2o: refactored backward compatibility includes

* fixed build with old g2o
This commit is contained in:
matlabbe
2022-04-28 09:18:30 -04:00
committed by GitHub
parent 190071678f
commit b646c5e1db
14 changed files with 731 additions and 283 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/optimizer/gtsam/GPSPose2XYFactor.h → corelib/src/optimizer/gtsam/XYFactor.h
+5 -4
View File
@@ -20,7 +20,8 @@
namespace rtabmap {
class GPSPose2XYFactor: public gtsam::NoiseModelFactor1<gtsam::Pose2> {
template<class VALUE>
class XYFactor: public gtsam::NoiseModelFactor1<VALUE> {
private:
// measurement information
@@ -34,13 +35,13 @@ public:
* @param model noise model for GPS snesor, in X-Y
* @param m Point2 measurement
*/
GPSPose2XYFactor(gtsam::Key poseKey, const gtsam::Point2 m, gtsam::SharedNoiseModel model) :
gtsam::NoiseModelFactor1<gtsam::Pose2>(model, poseKey), mx_(m.x()), my_(m.y()) {}
XYFactor(gtsam::Key poseKey, const gtsam::Point2 m, gtsam::SharedNoiseModel model) :
gtsam::NoiseModelFactor1<VALUE>(model, poseKey), mx_(m.x()), my_(m.y()) {}
// error function
// @param p the pose in Pose2
// @param H the optional Jacobian matrix, which use boost optional and has default null pointer
gtsam::Vector evaluateError(const gtsam::Pose2& p, boost::optional<gtsam::Matrix&> H = boost::none) const {
gtsam::Vector evaluateError(const VALUE& p, boost::optional<gtsam::Matrix&> H = boost::none) const {
// note that use boost optional like a pointer
// only calculate jacobian matrix when non-null pointer exists
corelib/src/optimizer/gtsam/GPSPose3XYZFactor.h → corelib/src/optimizer/gtsam/XYZFactor.h
+7 -3
View File
@@ -20,7 +20,8 @@
namespace rtabmap {
class GPSPose3XYZFactor: public gtsam::NoiseModelFactor1<gtsam::Pose3> {
template<class VALUE>
class XYZFactor: public gtsam::NoiseModelFactor1<VALUE> {
private:
// measurement information
@@ -34,8 +35,8 @@ public:
* @param model noise model for GPS sensor, in X-Y
* @param m Point2 measurement
*/
GPSPose3XYZFactor(gtsam::Key poseKey, const gtsam::Point3 m, gtsam::SharedNoiseModel model) :
gtsam::NoiseModelFactor1<gtsam::Pose3>(model, poseKey), mx_(m.x()), my_(m.y()), mz_(m.z()) {}
XYZFactor(gtsam::Key poseKey, const gtsam::Point3 m, gtsam::SharedNoiseModel model) :
gtsam::NoiseModelFactor1<VALUE>(model, poseKey), mx_(m.x()), my_(m.y()), mz_(m.z()) {}
// error function
// @param p the pose in Pose
@@ -47,6 +48,9 @@ public:
}
return (gtsam::Vector3() << p.x() - mx_, p.y() - my_, p.z() - mz_).finished();
}
gtsam::Vector evaluateError(const gtsam::Point3& p, boost::optional<gtsam::Matrix&> H = boost::none) const {
return (gtsam::Vector3() << p.x() - mx_, p.y() - my_, p.z() - mz_).finished();
}
};
} // namespace gtsamexamples