c matlab.h,matlab.h

/* ----------------------------------------------------------------------------

* 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

* -------------------------------------------------------------------------- */

/**

* @file matlab.h

* @brief Contains *generic* global functions designed particularly for the matlab interface

* @author Stephen Williams

*/

#pragma once

#include

#include

#include

#include

#include

#include

#include

#include

#include

#include

#include

namespace gtsam {

namespace utilities {

// Create a KeyList from indices

FastList 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

KeySet createKeySet(const Vector& I) {

KeySet set;

for (int i = 0; i < I.size(); i++)

set.insert(I[i]);

return set;

}

// Create a KeySet from indices using symbol

KeySet createKeySet(string s, const Vector& I) {

KeySet 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);

for(const auto& key_value: points)

result.row(j++) = key_value.value;

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;

for(const auto& key_value: points)

result.row(j++) = key_value.value;

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;

for(const auto& 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;

for(const auto& 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();

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);

for(const auto& key_value: values.filter()) {

values.update(key_value.key, key_value.value + Point2(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);

for(const auto& 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);

for(const auto& key_value: values.filter()) {

values.update(key_value.key, key_value.value + Point3(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;

for(const NonlinearFactor::shared_ptr& f: graph)

if (boost::dynamic_pointer_cast >(

f))

++K;

// now fill

Matrix errors(2, K);

for(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;

}

/// Convert from local to world coordinates

Values localToWorld(const Values& local, const Pose2& base,

const FastVector user_keys = FastVector()) {

Values world;

// if no keys given, get all keys from local values

FastVector keys(user_keys);

if (keys.size()==0)

keys = local.keys();

// Loop over all keys

for(Key key: keys) {

try {

// if value is a Pose2, compose it with base pose

Pose2 pose = local.at(key);

world.insert(key, base.compose(pose));

} catch (std::exception e1) {

try {

// if value is a Point2, transform it from base pose

Point2 point = local.at(key);

world.insert(key, base.transform_from(point));

} catch (std::exception e2) {

// if not Pose2 or Point2, do nothing

}

}

}

return world;

}

}

}

一键复制

编辑

Web IDE

原始数据

按行查看

历史