void Transform::SetTransform(const Pose2& rPose1, const Pose2& rPose2)
{
if (rPose1 == rPose2)
{
m_Rotation.SetToIdentity();
m_InverseRotation.SetToIdentity();
m_Transform = Pose2();
return;
}
// heading transformation
m_Rotation.FromAxisAngle(0, 0, 1, rPose2.GetHeading() - rPose1.GetHeading());
m_InverseRotation.FromAxisAngle(0, 0, 1, rPose1.GetHeading() - rPose2.GetHeading());
// position transformation
Pose2 newPosition;
if (rPose1.GetX() != 0.0 || rPose1.GetY() != 0.0)
{
newPosition = rPose2 - m_Rotation * rPose1;
}
else
{
newPosition = rPose2;
}
m_Transform = Pose2(newPosition.GetPosition(), rPose2.GetHeading() - rPose1.GetHeading());
}
/* ************************************************************************* */
Vector Pose2BetweenFactorEvaluateErrorDefault(const Pose2& measured, const Pose2& p1, const Pose2& p2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2)
{
Pose2 hx = p1.between(p2, H1, H2); // h(x)
// manifold equivalent of h(x)-z -> log(z,h(x))
return measured.localCoordinates(hx);
}
/* ************************************************************************* */
Pose2 Pose2betweenOptimized(const Pose2& r1, const Pose2& r2,
boost::optional<Matrix3&> H1, boost::optional<Matrix3&> H2)
{
// get cosines and sines from rotation matrices
const Rot2& R1 = r1.r(), R2 = r2.r();
double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s();
// Assert that R1 and R2 are normalized
assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5);
// Calculate delta rotation = between(R1,R2)
double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2;
Rot2 R(Rot2::atan2(s,c)); // normalizes
// Calculate delta translation = unrotate(R1, dt);
Point2 dt = r2.t() - r1.t();
double x = dt.x(), y = dt.y();
Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y);
// FD: This is just -AdjointMap(between(p2,p1)) inlined and re-using above
if (H1) {
double dt1 = -s2 * x + c2 * y;
double dt2 = -c2 * x - s2 * y;
*H1 = Matrix3();
(*H1) <<
-c, -s, dt1,
s, -c, dt2,
0.0, 0.0,-1.0;
}
if (H2) *H2 = Matrix3::Identity();
return Pose2(R,t);
}
/* ************************************************************************* */
int main()
{
int n = 50000000;
cout << "NOTE: Times are reported for " << n << " calls" << endl;
// create a random pose:
double x = 4.0, y = 2.0, r = 0.3;
Vector v = (Vector(3) << x, y, r).finished();
Pose2 X = Pose2(3,2,0.4), X2 = X.retract(v), X3(5,6,0.3);
TEST(Expmap, Pose2::Expmap(v));
TEST(Retract, X.retract(v));
TEST(Logmap, Pose2::Logmap(X2));
TEST(localCoordinates, X.localCoordinates(X2));
Matrix H1, H2;
Matrix3 H1f, H2f;
TEST(Pose2BetweenFactorEvaluateErrorDefault, Pose2BetweenFactorEvaluateErrorDefault(X3, X, X2, H1, H2));
TEST(Pose2BetweenFactorEvaluateErrorOptimizedBetween, Pose2BetweenFactorEvaluateErrorOptimizedBetween(X3, X, X2, H1, H2));
TEST(Pose2BetweenFactorEvaluateErrorOptimizedBetweenFixed, Pose2BetweenFactorEvaluateErrorOptimizedBetweenFixed(X3, X, X2, H1f, H2f));
// Print timings
tictoc_print_();
return 0;
}
/* ************************************************************************* */
CalibratedCamera CalibratedCamera::Level(const Pose2& pose2, double height) {
double st = sin(pose2.theta()), ct = cos(pose2.theta());
Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0);
Rot3 wRc(x, y, z);
Point3 t(pose2.x(), pose2.y(), height);
Pose3 pose3(wRc, t);
return CalibratedCamera(pose3);
}
/* ************************************************************************* */
Vector Pose2::localCoordinates(const Pose2& p2) const {
#ifdef SLOW_BUT_CORRECT_EXPMAP
return Logmap(between(p2));
#else
Pose2 r = between(p2);
return (Vector(3) << r.x(), r.y(), r.theta());
#endif
}
/* ************************************************************************* */
Rot2 Pose2::bearing(const Pose2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
Rot2 result = bearing(point.t(), H1, H2);
if (H2) {
Matrix H2_ = *H2 * point.r().matrix();
*H2 = zeros(1, 3);
insertSub(*H2, H2_, 0, 0);
}
return result;
}
// Using the NoiseModelFactor1 base class there are two functions that must be overridden.
// The first is the 'evaluateError' function. This function implements the desired measurement
// function, returning a vector of errors when evaluated at the provided variable value. It
// must also calculate the Jacobians for this measurement function, if requested.
Vector evaluateError(const Pose2& q, boost::optional<Matrix&> H = boost::none) const
{
// The measurement function for a GPS-like measurement is simple:
// error_x = pose.x - measurement.x
// error_y = pose.y - measurement.y
// Consequently, the Jacobians are:
// [ derror_x/dx derror_x/dy derror_x/dtheta ] = [1 0 0]
// [ derror_y/dx derror_y/dy derror_y/dtheta ] = [0 1 0]
if (H) (*H) = (Matrix(2,3) << 1.0,0.0,0.0, 0.0,1.0,0.0).finished();
return (Vector(2) << q.x() - mx_, q.y() - my_).finished();
}
/* ************************************************************************* */
Vector Pose2::Logmap(const Pose2& p) {
const Rot2& R = p.r();
const Point2& t = p.t();
double w = R.theta();
if (std::abs(w) < 1e-10)
return (Vector(3) << t.x(), t.y(), w);
else {
double c_1 = R.c()-1.0, s = R.s();
double det = c_1*c_1 + s*s;
Point2 p = R_PI_2 * (R.unrotate(t) - t);
Point2 v = (w/det) * p;
return (Vector(3) << v.x(), v.y(), w);
}
}
// see doc/math.lyx, SE(2) section
Pose2 Pose2::compose(const Pose2& p2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
// TODO: inline and reuse?
if(H1) *H1 = p2.inverse().AdjointMap();
if(H2) *H2 = I3;
return (*this)*p2;
}
/* ************************************************************************* */
double Pose2::range(const Pose2& pose2,
boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
Point2 d = pose2.t() - t_;
if (!H1 && !H2) return d.norm();
Matrix H;
double r = d.norm(H);
if (H1) *H1 = H * (Matrix(2, 3) <<
-r_.c(), r_.s(), 0.0,
-r_.s(), -r_.c(), 0.0);
if (H2) *H2 = H * (Matrix(2, 3) <<
pose2.r_.c(), -pose2.r_.s(), 0.0,
pose2.r_.s(), pose2.r_.c(), 0.0);
return r;
}
// main
int main(int argc, char** argv) {
// load Plaza1 data
list<TimedOdometry> odometry = readOdometry();
// size_t M = odometry.size();
vector<RangeTriple> triples = readTriples();
size_t K = triples.size();
// parameters
size_t start = 220, end=3000;
size_t minK = 100; // first batch of smart factors
size_t incK = 50; // minimum number of range measurements to process after
bool robust = true;
bool smart = true;
// Set Noise parameters
Vector priorSigmas = Vector3(1, 1, M_PI);
Vector odoSigmas = Vector3(0.05, 0.01, 0.2);
double sigmaR = 100; // range standard deviation
const NM::Base::shared_ptr // all same type
priorNoise = NM::Diagonal::Sigmas(priorSigmas), //prior
odoNoise = NM::Diagonal::Sigmas(odoSigmas), // odometry
gaussian = NM::Isotropic::Sigma(1, sigmaR), // non-robust
tukey = NM::Robust::Create(NM::mEstimator::Tukey::Create(15), gaussian), //robust
rangeNoise = robust ? tukey : gaussian;
// Initialize iSAM
ISAM2 isam;
// Add prior on first pose
Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120,
M_PI - 2.02108900000000);
NonlinearFactorGraph newFactors;
newFactors.push_back(PriorFactor<Pose2>(0, pose0, priorNoise));
ofstream os2(
"/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResultLM.txt");
ofstream os3(
"/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResultSR.txt");
// initialize points (Gaussian)
Values initial;
if (!smart) {
initial.insert(symbol('L', 1), Point2(-68.9265, 18.3778));
initial.insert(symbol('L', 6), Point2(-37.5805, 69.2278));
initial.insert(symbol('L', 0), Point2(-33.6205, 26.9678));
initial.insert(symbol('L', 5), Point2(1.7095, -5.8122));
}
Values landmarkEstimates = initial; // copy landmarks
initial.insert(0, pose0);
// initialize smart range factors
size_t ids[] = { 1, 6, 0, 5 };
typedef boost::shared_ptr<SmartRangeFactor> SmartPtr;
map<size_t, SmartPtr> smartFactors;
if (smart) {
for(size_t jj: ids) {
smartFactors[jj] = SmartPtr(new SmartRangeFactor(sigmaR));
newFactors.push_back(smartFactors[jj]);
}
}
// set some loop variables
size_t i = 1; // step counter
size_t k = 0; // range measurement counter
Pose2 lastPose = pose0;
size_t countK = 0, totalCount=0;
// Loop over odometry
gttic_(iSAM);
for(const TimedOdometry& timedOdometry: odometry) {
//--------------------------------- odometry loop -----------------------------------------
double t;
Pose2 odometry;
boost::tie(t, odometry) = timedOdometry;
// add odometry factor
newFactors.push_back(
BetweenFactor<Pose2>(i - 1, i, odometry,
NM::Diagonal::Sigmas(odoSigmas)));
// predict pose and add as initial estimate
Pose2 predictedPose = lastPose.compose(odometry);
lastPose = predictedPose;
initial.insert(i, predictedPose);
landmarkEstimates.insert(i, predictedPose);
// Check if there are range factors to be added
while (k < K && t >= boost::get<0>(triples[k])) {
size_t j = boost::get<1>(triples[k]);
double range = boost::get<2>(triples[k]);
if (i > start) {
if (smart && totalCount < minK) {
smartFactors[j]->addRange(i, range);
printf("adding range %g for %d on %d",range,(int)j,(int)i);cout << endl;
}
else {
RangeFactor<Pose2, Point2> factor(i, symbol('L', j), range,
rangeNoise);
// Throw out obvious outliers based on current landmark estimates
Vector error = factor.unwhitenedError(landmarkEstimates);
if (k <= 200 || fabs(error[0]) < 5)
newFactors.push_back(factor);
}
totalCount += 1;
}
k = k + 1;
countK = countK + 1;
}
// Check whether to update iSAM 2
if (k >= minK && countK >= incK) {
gttic_(update);
isam.update(newFactors, initial);
gttoc_(update);
gttic_(calculateEstimate);
Values result = isam.calculateEstimate();
gttoc_(calculateEstimate);
lastPose = result.at<Pose2>(i);
bool hasLandmarks = result.exists(symbol('L', ids[0]));
if (hasLandmarks) {
// update landmark estimates
landmarkEstimates = Values();
for(size_t jj: ids)
landmarkEstimates.insert(symbol('L', jj), result.at(symbol('L', jj)));
}
newFactors = NonlinearFactorGraph();
initial = Values();
if (smart && !hasLandmarks) {
cout << "initialize from smart landmarks" << endl;
for(size_t jj: ids) {
Point2 landmark = smartFactors[jj]->triangulate(result);
initial.insert(symbol('L', jj), landmark);
landmarkEstimates.insert(symbol('L', jj), landmark);
}
}
countK = 0;
for(const Values::ConstFiltered<Point2>::KeyValuePair& it: result.filter<Point2>())
os2 << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t1"
<< endl;
if (smart) {
for(size_t jj: ids) {
Point2 landmark = smartFactors[jj]->triangulate(result);
os3 << jj << "\t" << landmark.x() << "\t" << landmark.y() << "\t1"
<< endl;
}
}
}
i += 1;
if (i>end) break;
//--------------------------------- odometry loop -----------------------------------------
} // end for
gttoc_(iSAM);
// Print timings
tictoc_print_();
// Write result to file
Values result = isam.calculateEstimate();
ofstream os(
"/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResult.txt");
for(const Values::ConstFiltered<Pose2>::KeyValuePair& it: result.filter<Pose2>())
os << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t"
<< it.value.theta() << endl;
exit(0);
}
// main
int main (int argc, char** argv) {
// load Plaza2 data
list<TimedOdometry> odometry = readOdometry();
// size_t M = odometry.size();
vector<RangeTriple> triples = readTriples();
size_t K = triples.size();
// parameters
size_t minK = 150; // minimum number of range measurements to process initially
size_t incK = 25; // minimum number of range measurements to process after
bool groundTruth = false;
bool robust = true;
// Set Noise parameters
Vector priorSigmas = Vector3(1,1,M_PI);
Vector odoSigmas = Vector3(0.05, 0.01, 0.2);
double sigmaR = 100; // range standard deviation
const NM::Base::shared_ptr // all same type
priorNoise = NM::Diagonal::Sigmas(priorSigmas), //prior
odoNoise = NM::Diagonal::Sigmas(odoSigmas), // odometry
gaussian = NM::Isotropic::Sigma(1, sigmaR), // non-robust
tukey = NM::Robust::Create(NM::mEstimator::Tukey::Create(15), gaussian), //robust
rangeNoise = robust ? tukey : gaussian;
// Initialize iSAM
ISAM2 isam;
// Add prior on first pose
Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120,
M_PI - 2.02108900000000);
NonlinearFactorGraph newFactors;
newFactors.push_back(PriorFactor<Pose2>(0, pose0, priorNoise));
Values initial;
initial.insert(0, pose0);
// initialize points
if (groundTruth) { // from TL file
initial.insert(symbol('L', 1), Point2(-68.9265, 18.3778));
initial.insert(symbol('L', 6), Point2(-37.5805, 69.2278));
initial.insert(symbol('L', 0), Point2(-33.6205, 26.9678));
initial.insert(symbol('L', 5), Point2(1.7095, -5.8122));
} else { // drawn from sigma=1 Gaussian in matlab version
initial.insert(symbol('L', 1), Point2(3.5784, 2.76944));
initial.insert(symbol('L', 6), Point2(-1.34989, 3.03492));
initial.insert(symbol('L', 0), Point2(0.725404, -0.0630549));
initial.insert(symbol('L', 5), Point2(0.714743, -0.204966));
}
// set some loop variables
size_t i = 1; // step counter
size_t k = 0; // range measurement counter
bool initialized = false;
Pose2 lastPose = pose0;
size_t countK = 0;
// Loop over odometry
gttic_(iSAM);
BOOST_FOREACH(const TimedOdometry& timedOdometry, odometry) {
//--------------------------------- odometry loop -----------------------------------------
double t;
Pose2 odometry;
boost::tie(t, odometry) = timedOdometry;
// add odometry factor
newFactors.push_back(BetweenFactor<Pose2>(i-1, i, odometry,NM::Diagonal::Sigmas(odoSigmas)));
// predict pose and add as initial estimate
Pose2 predictedPose = lastPose.compose(odometry);
lastPose = predictedPose;
initial.insert(i, predictedPose);
// Check if there are range factors to be added
while (k < K && t >= boost::get<0>(triples[k])) {
size_t j = boost::get<1>(triples[k]);
double range = boost::get<2>(triples[k]);
newFactors.push_back(RangeFactor<Pose2, Point2>(i, symbol('L', j), range,rangeNoise));
k = k + 1;
countK = countK + 1;
}
// Check whether to update iSAM 2
if ((k > minK) && (countK > incK)) {
if (!initialized) { // Do a full optimize for first minK ranges
gttic_(batchInitialization);
LevenbergMarquardtOptimizer batchOptimizer(newFactors, initial);
initial = batchOptimizer.optimize();
gttoc_(batchInitialization);
initialized = true;
}
gttic_(update);
isam.update(newFactors, initial);
gttoc_(update);
gttic_(calculateEstimate);
Values result = isam.calculateEstimate();
gttoc_(calculateEstimate);
lastPose = result.at<Pose2>(i);
newFactors = NonlinearFactorGraph();
initial = Values();
countK = 0;
}
i += 1;
//--------------------------------- odometry loop -----------------------------------------
} // BOOST_FOREACH
/* ************************************************************************* */
Pose2 Pose2betweenDefault(const Pose2& r1, const Pose2& r2) {
return r1.inverse() * r2;
}
karto::String StringHelper::ToString(const Pose2& rValue)
{
return rValue.ToString();
}
