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

new allPose3s function

release/4.3a0
Frank Dellaert 2012-11-17 19:24:09 +00:00
parent afa6a1020d
commit 583cdacc11
1 changed files with 8 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
matlab.h
View File

@ -45,9 +45,9 @@ namespace gtsam {
/// Extract all Point3 values into a single matrix [x y z] /// Extract all Point3 values into a single matrix [x y z]
Matrix extractPoint3(const Values& values) { Matrix extractPoint3(const Values& values) {
size_t j=0;
Values::ConstFiltered<Point3> points = values.filter<Point3>(); Values::ConstFiltered<Point3> points = values.filter<Point3>();
Matrix result(points.size(),3); Matrix result(points.size(),3);
size_t j=0;
BOOST_FOREACH(const Values::ConstFiltered<Point3>::KeyValuePair& key_value, points) BOOST_FOREACH(const Values::ConstFiltered<Point3>::KeyValuePair& key_value, points)
result.row(j++) = key_value.value.vector(); result.row(j++) = key_value.value.vector();
return result; return result;
@ -55,19 +55,24 @@ namespace gtsam {
/// Extract all Pose2 values into a single matrix [x y theta] /// Extract all Pose2 values into a single matrix [x y theta]
Matrix extractPose2(const Values& values) { Matrix extractPose2(const Values& values) {
size_t j=0;
Values::ConstFiltered<Pose2> poses = values.filter<Pose2>(); Values::ConstFiltered<Pose2> poses = values.filter<Pose2>();
Matrix result(poses.size(),3); Matrix result(poses.size(),3);
size_t j=0;
BOOST_FOREACH(const Values::ConstFiltered<Pose2>::KeyValuePair& key_value, poses) BOOST_FOREACH(const Values::ConstFiltered<Pose2>::KeyValuePair& key_value, poses)
result.row(j++) << key_value.value.x(), key_value.value.y(), key_value.value.theta(); result.row(j++) << key_value.value.x(), key_value.value.y(), key_value.value.theta();
return result; return result;
} }
/// Extract all Pose3 values
Values allPose3s(const Values& values) {
return values.filter<Pose3>();
}
/// Extract all Pose3 values into a single matrix [r11 r12 r13 r21 r22 r23 r31 r32 r33 x y z] /// Extract all Pose3 values into a single matrix [r11 r12 r13 r21 r22 r23 r31 r32 r33 x y z]
Matrix extractPose3(const Values& values) { Matrix extractPose3(const Values& values) {
size_t j=0;
Values::ConstFiltered<Pose3> poses = values.filter<Pose3>(); Values::ConstFiltered<Pose3> poses = values.filter<Pose3>();
Matrix result(poses.size(),12); Matrix result(poses.size(),12);
size_t j=0;
BOOST_FOREACH(const Values::ConstFiltered<Pose3>::KeyValuePair& key_value, poses) { BOOST_FOREACH(const Values::ConstFiltered<Pose3>::KeyValuePair& key_value, poses) {
result.row(j).segment(0, 3) << key_value.value.rotation().matrix().row(0); result.row(j).segment(0, 3) << key_value.value.rotation().matrix().row(0);
result.row(j).segment(3, 3) << key_value.value.rotation().matrix().row(1); result.row(j).segment(3, 3) << key_value.value.rotation().matrix().row(1);
@ -78,7 +83,6 @@ namespace gtsam {
return result; return result;
} }
/// perturb all Point2 using normally distributed noise /// perturb all Point2 using normally distributed noise
void perturbPoint2(Values& values, double sigma, int32_t seed = 42u) { void perturbPoint2(Values& values, double sigma, int32_t seed = 42u) {
noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2,sigma); noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2,sigma);