diff --git a/gtsam.h b/gtsam.h index 71fcaeb17..99aec3f86 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2363,6 +2363,12 @@ virtual class CombinedImuFactor : gtsam::NonlinearFactor { namespace utilities { #include + gtsam::KeyList createKeyList(Vector I); + gtsam::KeyList createKeyList(string s, Vector I); + gtsam::KeyVector createKeyVector(Vector I); + gtsam::KeyVector createKeyVector(string s, Vector I); + gtsam::KeySet createKeySet(Vector I); + gtsam::KeySet createKeySet(string s, Vector I); Matrix extractPoint2(const gtsam::Values& values); Matrix extractPoint3(const gtsam::Values& values); Matrix extractPose2(const gtsam::Values& values); @@ -2375,7 +2381,6 @@ namespace utilities { void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K); void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K, const gtsam::Pose3& body_P_sensor); Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values); - gtsam::KeySet createKeySet(string s, Vector I); } //\namespace utilities diff --git a/matlab.h b/matlab.h index 261fd2d48..e9ad519bd 100644 --- a/matlab.h +++ b/matlab.h @@ -32,134 +32,192 @@ namespace gtsam { - namespace utilities { - - /// Extract all Point2 values into a single matrix [x y] - Matrix extractPoint2(const Values& values) { - size_t j=0; - Values::ConstFiltered points = values.filter(); - Matrix result(points.size(),2); - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, points) - result.row(j++) = key_value.value.vector(); - return result; - } - - /// Extract all Point3 values into a single matrix [x y z] - Matrix extractPoint3(const Values& values) { - Values::ConstFiltered points = values.filter(); - Matrix result(points.size(),3); - size_t j=0; - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, points) - result.row(j++) = key_value.value.vector(); - return result; - } - - /// Extract all Pose2 values into a single matrix [x y theta] - Matrix extractPose2(const Values& values) { - Values::ConstFiltered poses = values.filter(); - Matrix result(poses.size(),3); - size_t j=0; - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, poses) - result.row(j++) << key_value.value.x(), key_value.value.y(), key_value.value.theta(); - return result; - } - - /// Extract all Pose3 values - Values allPose3s(const Values& values) { - return values.filter(); - } - - /// 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) { - Values::ConstFiltered poses = values.filter(); - Matrix result(poses.size(),12); - size_t j=0; - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, poses) { - 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(6, 3) << key_value.value.rotation().matrix().row(2); - result.row(j).tail(3) = key_value.value.translation().vector(); - j++; - } - return result; - } - - /// Perturb all Point2 values using normally distributed noise - void perturbPoint2(Values& values, double sigma, int32_t seed = 42u) { - noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2,sigma); - Sampler sampler(model, seed); - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { - values.update(key_value.key, key_value.value.retract(sampler.sample())); - } - } - - /// Perturb all Pose2 values using normally distributed noise - void perturbPose2(Values& values, double sigmaT, double sigmaR, int32_t seed = 42u) { - noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas( - Vector3(sigmaT, sigmaT, sigmaR)); - Sampler sampler(model, seed); - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { - values.update(key_value.key, key_value.value.retract(sampler.sample())); - } - } - - /// Perturb all Point3 values using normally distributed noise - void perturbPoint3(Values& values, double sigma, int32_t seed = 42u) { - noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,sigma); - Sampler sampler(model, seed); - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { - values.update(key_value.key, key_value.value.retract(sampler.sample())); - } - } - - /// Insert a number of initial point values by backprojecting - void insertBackprojections(Values& values, const SimpleCamera& camera, const Vector& J, const Matrix& Z, double depth) { - if (Z.rows() != 2) throw std::invalid_argument("insertBackProjections: Z must be 2*K"); - if (Z.cols() != J.size()) throw std::invalid_argument("insertBackProjections: J and Z must have same number of entries"); - for(int k=0;k > - (Point2(Z(0, k), Z(1, k)), model, i, Key(J(k)), K, body_P_sensor)); - } - } - - /// Calculate the errors of all projection factors in a graph - Matrix reprojectionErrors(const NonlinearFactorGraph& graph, const Values& values) { - // first count - size_t K = 0, k=0; - BOOST_FOREACH(const NonlinearFactor::shared_ptr& f, graph) - if (boost::dynamic_pointer_cast >(f)) ++K; - // now fill - Matrix errors(2,K); - BOOST_FOREACH(const NonlinearFactor::shared_ptr& f, graph) { - boost::shared_ptr > p = boost::dynamic_pointer_cast >(f); - if (p) errors.col(k++) = p->unwhitenedError(values); - } - return errors; - } - - // Create a Keyset from indices - FastSet createKeySet(string s, const Vector& I) { - FastSet set; - char c = s[0]; - for(int i=0;i createKeyList(const Vector& I) { + FastList set; + for (int i = 0; i < I.size(); i++) + set.push_back(I[i]); + return set; +} + +// Create a KeyList from indices using symbol +FastList createKeyList(string s, const Vector& I) { + FastList set; + char c = s[0]; + for (int i = 0; i < I.size(); i++) + set.push_back(Symbol(c, I[i])); + return set; +} + +// Create a KeyVector from indices +FastVector createKeyVector(const Vector& I) { + FastVector set; + for (int i = 0; i < I.size(); i++) + set.push_back(I[i]); + return set; +} + +// Create a KeyVector from indices using symbol +FastVector createKeyVector(string s, const Vector& I) { + FastVector set; + char c = s[0]; + for (int i = 0; i < I.size(); i++) + set.push_back(Symbol(c, I[i])); + return set; +} + +// Create a KeySet from indices +FastSet createKeySet(const Vector& I) { + FastSet set; + for (int i = 0; i < I.size(); i++) + set.insert(I[i]); + return set; +} + +// Create a KeySet from indices using symbol +FastSet createKeySet(string s, const Vector& I) { + FastSet set; + char c = s[0]; + for (int i = 0; i < I.size(); i++) + set.insert(symbol(c, I[i])); + return set; +} + +/// Extract all Point2 values into a single matrix [x y] +Matrix extractPoint2(const Values& values) { + size_t j = 0; + Values::ConstFiltered points = values.filter(); + Matrix result(points.size(), 2); + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, points) + result.row(j++) = key_value.value.vector(); + return result; +} + +/// Extract all Point3 values into a single matrix [x y z] +Matrix extractPoint3(const Values& values) { + Values::ConstFiltered points = values.filter(); + Matrix result(points.size(), 3); + size_t j = 0; + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, points) + result.row(j++) = key_value.value.vector(); + return result; +} + +/// Extract all Pose2 values into a single matrix [x y theta] +Matrix extractPose2(const Values& values) { + Values::ConstFiltered poses = values.filter(); + Matrix result(poses.size(), 3); + size_t j = 0; + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, poses) + result.row(j++) << key_value.value.x(), key_value.value.y(), key_value.value.theta(); + return result; +} + +/// Extract all Pose3 values +Values allPose3s(const Values& values) { + return values.filter(); +} + +/// 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) { + Values::ConstFiltered poses = values.filter(); + Matrix result(poses.size(), 12); + size_t j = 0; + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, poses) { + 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(6, 3) << key_value.value.rotation().matrix().row(2); + result.row(j).tail(3) = key_value.value.translation().vector(); + j++; + } + return result; +} + +/// Perturb all Point2 values using normally distributed noise +void perturbPoint2(Values& values, double sigma, int32_t seed = 42u) { + noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2, + sigma); + Sampler sampler(model, seed); + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { + values.update(key_value.key, key_value.value.retract(sampler.sample())); + } +} + +/// Perturb all Pose2 values using normally distributed noise +void perturbPose2(Values& values, double sigmaT, double sigmaR, int32_t seed = + 42u) { + noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas( + Vector3(sigmaT, sigmaT, sigmaR)); + Sampler sampler(model, seed); + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { + values.update(key_value.key, key_value.value.retract(sampler.sample())); + } +} + +/// Perturb all Point3 values using normally distributed noise +void perturbPoint3(Values& values, double sigma, int32_t seed = 42u) { + noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3, + sigma); + Sampler sampler(model, seed); + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { + values.update(key_value.key, key_value.value.retract(sampler.sample())); + } +} + +/// Insert a number of initial point values by backprojecting +void insertBackprojections(Values& values, const SimpleCamera& camera, + const Vector& J, const Matrix& Z, double depth) { + if (Z.rows() != 2) + throw std::invalid_argument("insertBackProjections: Z must be 2*K"); + if (Z.cols() != J.size()) + throw std::invalid_argument( + "insertBackProjections: J and Z must have same number of entries"); + for (int k = 0; k < Z.cols(); k++) { + Point2 p(Z(0, k), Z(1, k)); + Point3 P = camera.backproject(p, depth); + values.insert(J(k), P); + } +} + +/// Insert multiple projection factors for a single pose key +void insertProjectionFactors(NonlinearFactorGraph& graph, Key i, + const Vector& J, const Matrix& Z, const SharedNoiseModel& model, + const Cal3_S2::shared_ptr K, const Pose3& body_P_sensor = Pose3()) { + if (Z.rows() != 2) + throw std::invalid_argument("addMeasurements: Z must be 2*K"); + if (Z.cols() != J.size()) + throw std::invalid_argument( + "addMeasurements: J and Z must have same number of entries"); + for (int k = 0; k < Z.cols(); k++) { + graph.push_back( + boost::make_shared >( + Point2(Z(0, k), Z(1, k)), model, i, Key(J(k)), K, body_P_sensor)); + } +} + +/// Calculate the errors of all projection factors in a graph +Matrix reprojectionErrors(const NonlinearFactorGraph& graph, + const Values& values) { + // first count + size_t K = 0, k = 0; + BOOST_FOREACH(const NonlinearFactor::shared_ptr& f, graph) + if (boost::dynamic_pointer_cast >( + f)) + ++K; + // now fill + Matrix errors(2, K); + BOOST_FOREACH(const NonlinearFactor::shared_ptr& f, graph) { + boost::shared_ptr > p = + boost::dynamic_pointer_cast >( + f); + if (p) + errors.col(k++) = p->unwhitenedError(values); + } + return errors; +} + +} } diff --git a/matlab/gtsam_tests/testUtilities.m b/matlab/gtsam_tests/testUtilities.m new file mode 100644 index 000000000..da8dec789 --- /dev/null +++ b/matlab/gtsam_tests/testUtilities.m @@ -0,0 +1,47 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% GTSAM Copyright 2010, Georgia Tech Research Corporation, +% Atlanta, Georgia 30332-0415 +% All Rights Reserved +% Authors: Frank Dellaert, et al. (see THANKS for the full author list) +% +% See LICENSE for the license information +% +% @brief Checks for results of functions in utilities namespace +% @author Frank Dellaert +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +import gtsam.* + +%% Create keys for variables +x1 = symbol('x',1); x2 = symbol('x',2); x3 = symbol('x',3); + +actual = utilities.createKeyList([1;2;3]); +CHECK('KeyList', isa(actual,'gtsam.KeyList')); +CHECK('size==3', actual.size==3); +CHECK('actual.front==1', actual.front==1); + +actual = utilities.createKeyList('x',[1;2;3]); +CHECK('KeyList', isa(actual,'gtsam.KeyList')); +CHECK('size==3', actual.size==3); +CHECK('actual.front==x1', actual.front==x1); + +actual = utilities.createKeyVector([1;2;3]); +CHECK('KeyVector', isa(actual,'gtsam.KeyVector')); +CHECK('size==3', actual.size==3); +CHECK('actual.at(0)==1', actual.at(0)==1); + +actual = utilities.createKeyVector('x',[1;2;3]); +CHECK('KeyVector', isa(actual,'gtsam.KeyVector')); +CHECK('size==3', actual.size==3); +CHECK('actual.at(0)==x1', actual.at(0)==x1); + +actual = utilities.createKeySet([1;2;3]); +CHECK('KeySet', isa(actual,'gtsam.KeySet')); +CHECK('size==3', actual.size==3); +CHECK('actual.count(1)', actual.count(1)); + +actual = utilities.createKeySet('x',[1;2;3]); +CHECK('KeySet', isa(actual,'gtsam.KeySet')); +CHECK('size==3', actual.size==3); +CHECK('actual.count(x1)', actual.count(x1)); + diff --git a/matlab/gtsam_tests/test_gtsam.m b/matlab/gtsam_tests/test_gtsam.m index 2cad40df8..c379179c5 100644 --- a/matlab/gtsam_tests/test_gtsam.m +++ b/matlab/gtsam_tests/test_gtsam.m @@ -33,5 +33,8 @@ testVisualISAMExample display 'Starting: testSerialization' testSerialization +display 'Starting: testUtilities' +testUtilities + % end of tests display 'Tests complete!'