67 lines
2.1 KiB
C++
67 lines
2.1 KiB
C++
/*
|
|
* @file TransformConstraint.h
|
|
* @brief A constraint for combining graphs by common landmarks and a transform node
|
|
* @author Alex Cunningham
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include "NonlinearConstraint.h"
|
|
|
|
namespace gtsam {
|
|
|
|
/**
|
|
* A constraint between two landmarks in separate maps
|
|
* Templated on:
|
|
* Config : The overall config
|
|
* PKey : Key of landmark being constrained
|
|
* Point : Type of landmark
|
|
* TKey : Key of transform used
|
|
* Transform : Transform variable class
|
|
*
|
|
* The Point and Transform concepts must be Lie types, and the transform
|
|
* relationship "Point = transform_from(Transform, Point)" must exist.
|
|
*
|
|
* This base class should be specialized to implement the cost function for
|
|
* specific classes of landmarks
|
|
*/
|
|
template<class Config, class PKey, class Point, class TKey, class Transform>
|
|
class TransformConstraint : public NonlinearEqualityConstraint3<Config, PKey, Point, TKey, Transform, PKey, Point> {
|
|
|
|
public:
|
|
typedef NonlinearEqualityConstraint3<Config, PKey, Point, TKey, Transform, PKey, Point> Base;
|
|
typedef TransformConstraint<Config, PKey, Point, TKey, Transform> This;
|
|
|
|
/**
|
|
* General constructor with all of the keys to variables in the map
|
|
* Extracts everything necessary from the key types
|
|
*/
|
|
TransformConstraint(const PKey& foreignKey, const TKey& transKey, const PKey& localKey, double mu = 1000.0)
|
|
: Base(foreignKey, transKey, localKey, Point().dim(), mu) {}
|
|
|
|
virtual ~TransformConstraint(){}
|
|
|
|
/** Combined cost and derivative function using boost::optional */
|
|
virtual Vector evaluateError(const Point& foreign, const Transform& trans, const Point& local,
|
|
boost::optional<Matrix&> Dforeign = boost::none,
|
|
boost::optional<Matrix&> Dtrans = boost::none,
|
|
boost::optional<Matrix&> Dlocal = boost::none) const {
|
|
if (Dforeign) {
|
|
Matrix Af;
|
|
transform_from(trans, foreign, boost::none, Af);
|
|
*Dforeign = Af;
|
|
}
|
|
if (Dtrans) {
|
|
Matrix At;
|
|
transform_from(trans, foreign, At, boost::none);
|
|
*Dtrans = At;
|
|
}
|
|
if (Dlocal) {
|
|
Point dummy;
|
|
*Dlocal = -1* eye(dummy.dim());
|
|
}
|
|
return gtsam::logmap(gtsam::between<Point>(local, transform_from(trans, foreign)));
|
|
}
|
|
};
|
|
} // \namespace gtsam
|