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

Formatting

release/4.3a0
dellaert 2014-05-25 10:49:59 -04:00
parent 87c386d77f
commit 7aa0bd332a
1 changed files with 144 additions and 128 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

168
matlab.h
View File

@ -32,48 +32,48 @@
namespace gtsam { namespace gtsam {
namespace utilities { namespace utilities {
/// Extract all Point2 values into a single matrix [x y] /// Extract all Point2 values into a single matrix [x y]
Matrix extractPoint2(const Values& values) { Matrix extractPoint2(const Values& values) {
size_t j=0; size_t j = 0;
Values::ConstFiltered<Point2> points = values.filter<Point2>(); Values::ConstFiltered<Point2> points = values.filter<Point2>();
Matrix result(points.size(),2); Matrix result(points.size(), 2);
BOOST_FOREACH(const Values::ConstFiltered<Point2>::KeyValuePair& key_value, points) BOOST_FOREACH(const Values::ConstFiltered<Point2>::KeyValuePair& key_value, points)
result.row(j++) = key_value.value.vector(); result.row(j++) = key_value.value.vector();
return result; return result;
} }
/// 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) {
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; 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;
} }
/// 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) {
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; 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 /// Extract all Pose3 values
Values allPose3s(const Values& values) { Values allPose3s(const Values& values) {
return values.filter<Pose3>(); 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) {
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; 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);
@ -82,84 +82,100 @@ namespace gtsam {
j++; j++;
} }
return result; return result;
} }
/// Perturb all Point2 values using normally distributed noise /// Perturb all Point2 values 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);
Sampler sampler(model, seed); Sampler sampler(model, seed);
BOOST_FOREACH(const Values::ConstFiltered<Point2>::KeyValuePair& key_value, values.filter<Point2>()) { BOOST_FOREACH(const Values::ConstFiltered<Point2>::KeyValuePair& key_value, values.filter<Point2>()) {
values.update(key_value.key, key_value.value.retract(sampler.sample())); values.update(key_value.key, key_value.value.retract(sampler.sample()));
} }
} }
/// Perturb all Pose2 values using normally distributed noise /// Perturb all Pose2 values using normally distributed noise
void perturbPose2(Values& values, double sigmaT, double sigmaR, int32_t seed = 42u) { void perturbPose2(Values& values, double sigmaT, double sigmaR, int32_t seed =
42u) {
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas( noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(
Vector3(sigmaT, sigmaT, sigmaR)); Vector3(sigmaT, sigmaT, sigmaR));
Sampler sampler(model, seed); Sampler sampler(model, seed);
BOOST_FOREACH(const Values::ConstFiltered<Pose2>::KeyValuePair& key_value, values.filter<Pose2>()) { BOOST_FOREACH(const Values::ConstFiltered<Pose2>::KeyValuePair& key_value, values.filter<Pose2>()) {
values.update(key_value.key, key_value.value.retract(sampler.sample())); values.update(key_value.key, key_value.value.retract(sampler.sample()));
} }
} }
/// Perturb all Point3 values using normally distributed noise /// Perturb all Point3 values using normally distributed noise
void perturbPoint3(Values& values, double sigma, int32_t seed = 42u) { void perturbPoint3(Values& values, double sigma, int32_t seed = 42u) {
noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,sigma); noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,
sigma);
Sampler sampler(model, seed); Sampler sampler(model, seed);
BOOST_FOREACH(const Values::ConstFiltered<Point3>::KeyValuePair& key_value, values.filter<Point3>()) { BOOST_FOREACH(const Values::ConstFiltered<Point3>::KeyValuePair& key_value, values.filter<Point3>()) {
values.update(key_value.key, key_value.value.retract(sampler.sample())); values.update(key_value.key, key_value.value.retract(sampler.sample()));
} }
} }
/// Insert a number of initial point values by backprojecting /// Insert a number of initial point values by backprojecting
void insertBackprojections(Values& values, const SimpleCamera& camera, const Vector& J, const Matrix& Z, double depth) { void insertBackprojections(Values& values, const SimpleCamera& camera,
if (Z.rows() != 2) throw std::invalid_argument("insertBackProjections: Z must be 2*K"); const Vector& J, const Matrix& Z, double depth) {
if (Z.cols() != J.size()) throw std::invalid_argument("insertBackProjections: J and Z must have same number of entries"); if (Z.rows() != 2)
for(int k=0;k<Z.cols();k++) { throw std::invalid_argument("insertBackProjections: Z must be 2*K");
Point2 p(Z(0,k),Z(1,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); Point3 P = camera.backproject(p, depth);
values.insert(J(k), P); values.insert(J(k), P);
} }
} }
/// Insert multiple projection factors for a single pose key /// Insert multiple projection factors for a single pose key
void insertProjectionFactors(NonlinearFactorGraph& graph, Key i, const Vector& J, const Matrix& Z, void insertProjectionFactors(NonlinearFactorGraph& graph, Key i,
const SharedNoiseModel& model, const Cal3_S2::shared_ptr K, const Pose3& body_P_sensor = Pose3()) { const Vector& J, const Matrix& Z, const SharedNoiseModel& model,
if (Z.rows() != 2) throw std::invalid_argument("addMeasurements: Z must be 2*K"); const Cal3_S2::shared_ptr K, const Pose3& body_P_sensor = Pose3()) {
if (Z.cols() != J.size()) throw std::invalid_argument( 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"); "addMeasurements: J and Z must have same number of entries");
for (int k = 0; k < Z.cols(); k++) { for (int k = 0; k < Z.cols(); k++) {
graph.push_back( graph.push_back(
boost::make_shared<GenericProjectionFactor<Pose3, Point3> > boost::make_shared<GenericProjectionFactor<Pose3, Point3> >(
(Point2(Z(0, k), Z(1, k)), model, i, Key(J(k)), K, body_P_sensor)); 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 /// Calculate the errors of all projection factors in a graph
Matrix reprojectionErrors(const NonlinearFactorGraph& graph, const Values& values) { Matrix reprojectionErrors(const NonlinearFactorGraph& graph,
// first count const Values& values) {
size_t K = 0, k=0; // first count
BOOST_FOREACH(const NonlinearFactor::shared_ptr& f, graph) size_t K = 0, k = 0;
if (boost::dynamic_pointer_cast<const GenericProjectionFactor<Pose3, Point3> >(f)) ++K; BOOST_FOREACH(const NonlinearFactor::shared_ptr& f, graph)
// now fill if (boost::dynamic_pointer_cast<const GenericProjectionFactor<Pose3, Point3> >(
Matrix errors(2,K); f))
BOOST_FOREACH(const NonlinearFactor::shared_ptr& f, graph) { ++K;
boost::shared_ptr<const GenericProjectionFactor<Pose3, Point3> > p = boost::dynamic_pointer_cast<const GenericProjectionFactor<Pose3, Point3> >(f); // now fill
if (p) errors.col(k++) = p->unwhitenedError(values); Matrix errors(2, K);
} BOOST_FOREACH(const NonlinearFactor::shared_ptr& f, graph) {
return errors; boost::shared_ptr<const GenericProjectionFactor<Pose3, Point3> > p =
} boost::dynamic_pointer_cast<const GenericProjectionFactor<Pose3, Point3> >(
f);
// Create a Keyset from indices if (p)
FastSet<Key> createKeySet(string s, const Vector& I) { errors.col(k++) = p->unwhitenedError(values);
FastSet<Key> set; }
char c = s[0]; return errors;
for(int i=0;i<I.size();i++) }
set.insert(symbol(c,I[i]));
return set; // Create a Keyset from indices
} FastSet<Key> createKeySet(string s, const Vector& I) {
} FastSet<Key> set;
char c = s[0];
for (int i = 0; i < I.size(); i++)
set.insert(symbol(c, I[i]));
return set;
}
}
} }