RADICAL: Got rid of sensor_pose. In the new PinholePose philosophy, this is entirely the responsibility of the CAMERA. Just like PinholePose, we can have a camera class that has a shared extra transform: it is part of the calibration. PinholePose itself is not able to do this, as the calibration is assumed 2D only, but it's easy to create a class and have the correct derivatives in place.
dellaert
parent
0bf95ae7f6
commit
a132d959f5
|
|
@ -76,10 +76,6 @@ protected:
|
|||
typedef Eigen::Matrix<double, Dim, 1> VectorD;
|
||||
typedef Eigen::Matrix<double, ZDim, ZDim> Matrix2;
|
||||
|
||||
/// An optional sensor pose, in the body frame (one for all cameras)
|
||||
/// This seems to make sense for a CameraTrack, not for a CameraSet
|
||||
boost::optional<Pose3> body_P_sensor_;
|
||||
|
||||
// check that noise model is isotropic and the same
|
||||
// TODO, this is hacky, we should just do this via constructor, not add
|
||||
void maybeSetNoiseModel(const SharedNoiseModel& sharedNoiseModel) {
|
||||
|
|
@ -107,17 +103,10 @@ public:
|
|||
/// shorthand for a smart pointer to a factor
|
||||
typedef boost::shared_ptr<This> shared_ptr;
|
||||
|
||||
/// We use the new CameraSte data structure to refer to a set of cameras
|
||||
typedef CameraSet<CAMERA> Cameras;
|
||||
|
||||
/**
|
||||
* Constructor
|
||||
* @param body_P_sensor is the transform from sensor to body frame (default identity)
|
||||
*/
|
||||
SmartFactorBase(boost::optional<Pose3> body_P_sensor = boost::none) :
|
||||
body_P_sensor_(body_P_sensor) {
|
||||
}
|
||||
|
||||
/** Virtual destructor */
|
||||
/// Virtual destructor, subclasses from NonlinearFactor
|
||||
virtual ~SmartFactorBase() {
|
||||
}
|
||||
|
||||
|
|
@ -193,8 +182,6 @@ public:
|
|||
std::cout << "measurement, p = " << measured_[k] << "\t";
|
||||
noiseModel_->print("noise model = ");
|
||||
}
|
||||
if (this->body_P_sensor_)
|
||||
this->body_P_sensor_->print(" sensor pose in body frame: ");
|
||||
Base::print("", keyFormatter);
|
||||
}
|
||||
|
||||
|
|
@ -208,10 +195,7 @@ public:
|
|||
areMeasurementsEqual = false;
|
||||
break;
|
||||
}
|
||||
return e && Base::equals(p, tol) && areMeasurementsEqual
|
||||
&& ((!body_P_sensor_ && !e->body_P_sensor_)
|
||||
|| (body_P_sensor_ && e->body_P_sensor_
|
||||
&& body_P_sensor_->equals(*e->body_P_sensor_)));
|
||||
return e && Base::equals(p, tol) && areMeasurementsEqual;
|
||||
}
|
||||
|
||||
/// Compute reprojection errors
|
||||
|
|
@ -221,27 +205,10 @@ public:
|
|||
|
||||
/**
|
||||
* Compute reprojection errors and derivatives
|
||||
* TODO: the treatment of body_P_sensor_ is weird: the transformation
|
||||
* is applied in the caller, but the derivatives are computed here.
|
||||
*/
|
||||
Vector reprojectionError(const Cameras& cameras, const Point3& point,
|
||||
typename Cameras::FBlocks& F, Matrix& E) const {
|
||||
|
||||
Vector b = cameras.reprojectionError(point, measured_, F, E);
|
||||
|
||||
// Apply sensor chain rule if needed TODO: no simpler way ??
|
||||
if (body_P_sensor_) {
|
||||
size_t m = keys_.size();
|
||||
for (size_t i = 0; i < m; i++) {
|
||||
const Pose3& pose_i = cameras[i].pose();
|
||||
Pose3 w_Pose_body = pose_i.compose(body_P_sensor_->inverse());
|
||||
Matrix66 J;
|
||||
Pose3 world_P_body = w_Pose_body.compose(*body_P_sensor_, J);
|
||||
F[i].leftCols(6) *= J;
|
||||
}
|
||||
}
|
||||
|
||||
return b;
|
||||
return cameras.reprojectionError(point, measured_, F, E);
|
||||
}
|
||||
|
||||
/// Calculate vector of re-projection errors, noise model applied
|
||||
|
|
@ -582,7 +549,6 @@ private:
|
|||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
|
||||
ar & BOOST_SERIALIZATION_NVP(measured_);
|
||||
ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
|
||||
}
|
||||
};
|
||||
// end class SmartFactorBase
|
||||
|
|
|
|||
|
|
@ -56,17 +56,15 @@ public:
|
|||
* otherwise the factor is simply neglected
|
||||
* @param enableEPI if set to true linear triangulation is refined with embedded LM iterations
|
||||
* @param isImplicit if set to true linearize the factor to an implicit Schur factor
|
||||
* @param body_P_sensor is the transform from body to sensor frame (default identity)
|
||||
*/
|
||||
SmartProjectionCameraFactor(const double rankTol = 1,
|
||||
const double linThreshold = -1, const bool manageDegeneracy = false,
|
||||
const bool enableEPI = false, const bool isImplicit = false,
|
||||
boost::optional<Pose3> body_P_sensor = boost::none) :
|
||||
Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor), isImplicit_(
|
||||
const bool enableEPI = false, const bool isImplicit = false) :
|
||||
Base(rankTol, linThreshold, manageDegeneracy, enableEPI), isImplicit_(
|
||||
isImplicit) {
|
||||
if (linThreshold != -1) {
|
||||
std::cout << "SmartProjectionCameraFactor: linThreshold "
|
||||
<< linThreshold << std::endl;
|
||||
std::cout << "SmartProjectionCameraFactor: linThreshold " << linThreshold
|
||||
<< std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -116,16 +116,14 @@ public:
|
|||
* @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint,
|
||||
* otherwise the factor is simply neglected
|
||||
* @param enableEPI if set to true linear triangulation is refined with embedded LM iterations
|
||||
* @param body_P_sensor is the transform from sensor to body frame (default identity)
|
||||
*/
|
||||
SmartProjectionFactor(const double rankTol, const double linThreshold,
|
||||
const bool manageDegeneracy, const bool enableEPI,
|
||||
boost::optional<Pose3> body_P_sensor = boost::none,
|
||||
double landmarkDistanceThreshold = 1e10,
|
||||
double dynamicOutlierRejectionThreshold = -1, SmartFactorStatePtr state =
|
||||
SmartFactorStatePtr(new SmartProjectionFactorState())) :
|
||||
Base(body_P_sensor), rankTolerance_(rankTol), retriangulationThreshold_(
|
||||
1e-5), manageDegeneracy_(manageDegeneracy), enableEPI_(enableEPI), linearizationThreshold_(
|
||||
rankTolerance_(rankTol), retriangulationThreshold_(1e-5), manageDegeneracy_(
|
||||
manageDegeneracy), enableEPI_(enableEPI), linearizationThreshold_(
|
||||
linThreshold), degenerate_(false), cheiralityException_(false), throwCheirality_(
|
||||
false), verboseCheirality_(false), state_(state), landmarkDistanceThreshold_(
|
||||
landmarkDistanceThreshold), dynamicOutlierRejectionThreshold_(
|
||||
|
|
@ -336,8 +334,8 @@ public:
|
|||
m = zeros(Base::Dim, Base::Dim);
|
||||
BOOST_FOREACH(Vector& v, gs)
|
||||
v = zero(Base::Dim);
|
||||
return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_, Gs, gs,
|
||||
0.0);
|
||||
return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_,
|
||||
Gs, gs, 0.0);
|
||||
}
|
||||
|
||||
// instead, if we want to manage the exception..
|
||||
|
|
@ -372,7 +370,7 @@ public:
|
|||
{
|
||||
std::vector<typename Base::KeyMatrix2D> Fblocks;
|
||||
f = computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras);
|
||||
Base::FillDiagonalF(Fblocks,F); // expensive !
|
||||
Base::FillDiagonalF(Fblocks, F); // expensive !
|
||||
}
|
||||
|
||||
// Schur complement trick
|
||||
|
|
@ -390,7 +388,8 @@ public:
|
|||
// Note the minus sign above! g has negative b.
|
||||
// Informal reasoning: when we write the error as 0.5*|Ax-b|^2
|
||||
// the derivative is A'*(Ax-b), and at x=0, this becomes -A'*b
|
||||
gs_vector.noalias() = - F.transpose() * (b - (E * (P * (E.transpose() * b))));
|
||||
gs_vector.noalias() = -F.transpose()
|
||||
* (b - (E * (P * (E.transpose() * b))));
|
||||
|
||||
// Populate Gs and gs
|
||||
int GsCount2 = 0;
|
||||
|
|
@ -410,7 +409,8 @@ public:
|
|||
this->state_->gs = gs;
|
||||
this->state_->f = f;
|
||||
}
|
||||
return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_, Gs, gs, f);
|
||||
return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_, Gs,
|
||||
gs, f);
|
||||
}
|
||||
|
||||
// create factor
|
||||
|
|
@ -541,8 +541,9 @@ public:
|
|||
}
|
||||
|
||||
/// Version that takes values, and creates the point
|
||||
bool triangulateAndComputeJacobians(std::vector<typename Base::KeyMatrix2D>& Fblocks,
|
||||
Matrix& E, Vector& b, const Values& values) const {
|
||||
bool triangulateAndComputeJacobians(
|
||||
std::vector<typename Base::KeyMatrix2D>& Fblocks, Matrix& E, Vector& b,
|
||||
const Values& values) const {
|
||||
Cameras cameras;
|
||||
bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
|
||||
if (nonDegenerate)
|
||||
|
|
@ -566,7 +567,7 @@ public:
|
|||
Cameras cameras;
|
||||
bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
|
||||
if (nonDegenerate)
|
||||
return Base::reprojectionError(cameras, point_);
|
||||
return Base::reprojectionError(cameras, point_);
|
||||
else
|
||||
return zero(cameras.size() * 2);
|
||||
}
|
||||
|
|
@ -613,7 +614,7 @@ public:
|
|||
// 3D parameterization of point at infinity
|
||||
const Point2& zi = this->measured_.at(0);
|
||||
this->point_ = cameras.front().backprojectPointAtInfinity(zi);
|
||||
return Base::totalReprojectionErrorAtInfinity(cameras,this->point_);
|
||||
return Base::totalReprojectionErrorAtInfinity(cameras, this->point_);
|
||||
} else {
|
||||
// Just use version in base class
|
||||
return Base::totalReprojectionError(cameras, point_);
|
||||
|
|
|
|||
|
|
@ -64,15 +64,16 @@ public:
|
|||
* @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint,
|
||||
* otherwise the factor is simply neglected
|
||||
* @param enableEPI if set to true linear triangulation is refined with embedded LM iterations
|
||||
* @param body_P_sensor is the transform from sensor to body frame (default identity)
|
||||
*/
|
||||
SmartProjectionPoseFactor(const double rankTol = 1,
|
||||
const double linThreshold = -1, const bool manageDegeneracy = false,
|
||||
const bool enableEPI = false, boost::optional<Pose3> body_P_sensor = boost::none,
|
||||
LinearizationMode linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10,
|
||||
const bool enableEPI = false, LinearizationMode linearizeTo = HESSIAN,
|
||||
double landmarkDistanceThreshold = 1e10,
|
||||
double dynamicOutlierRejectionThreshold = -1) :
|
||||
Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor,
|
||||
landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), linearizeTo_(linearizeTo) {}
|
||||
Base(rankTol, linThreshold, manageDegeneracy, enableEPI,
|
||||
landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), linearizeTo_(
|
||||
linearizeTo) {
|
||||
}
|
||||
|
||||
/** Virtual destructor */
|
||||
virtual ~SmartProjectionPoseFactor() {}
|
||||
|
|
@ -150,12 +151,9 @@ public:
|
|||
*/
|
||||
typename Base::Cameras cameras(const Values& values) const {
|
||||
typename Base::Cameras cameras;
|
||||
size_t i=0;
|
||||
size_t i = 0;
|
||||
BOOST_FOREACH(const Key& k, this->keys_) {
|
||||
Pose3 pose = values.at<Pose3>(k);
|
||||
if(Base::body_P_sensor_)
|
||||
pose = pose.compose(*(Base::body_P_sensor_));
|
||||
|
||||
Camera camera(pose, sharedKs_[i++]);
|
||||
cameras.push_back(camera);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -41,8 +41,6 @@ Symbol l1('l', 1), l2('l', 2), l3('l', 3);
|
|||
Key c1 = 1, c2 = 2, c3 = 3;
|
||||
|
||||
static Point2 measurement1(323.0, 240.0);
|
||||
static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
|
||||
Point3(0.25, -0.10, 1.0));
|
||||
|
||||
typedef SmartProjectionCameraFactor<Cal3_S2> SmartFactor;
|
||||
typedef SmartProjectionCameraFactor<Cal3Bundler> SmartFactorBundler;
|
||||
|
|
@ -100,17 +98,6 @@ TEST( SmartProjectionCameraFactor, Constructor4) {
|
|||
factor1.add(measurement1, x1, unit2);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( SmartProjectionCameraFactor, ConstructorWithTransform) {
|
||||
using namespace vanilla;
|
||||
bool manageDegeneracy = true;
|
||||
bool isImplicit = false;
|
||||
bool enableEPI = false;
|
||||
SmartFactor factor1(rankTol, linThreshold, manageDegeneracy, isImplicit,
|
||||
enableEPI, body_P_sensor1);
|
||||
factor1.add(measurement1, x1, unit2);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( SmartProjectionCameraFactor, Equals ) {
|
||||
using namespace vanilla;
|
||||
|
|
|
|||
|
|
@ -50,9 +50,6 @@ static Symbol x2('X', 2);
|
|||
static Symbol x3('X', 3);
|
||||
|
||||
static Point2 measurement1(323.0, 240.0);
|
||||
static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
|
||||
Point3(0.25, -0.10, 1.0));
|
||||
|
||||
typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
|
||||
typedef SmartProjectionPoseFactor<Cal3Bundler> SmartFactorBundler;
|
||||
|
||||
|
|
@ -80,16 +77,6 @@ TEST( SmartProjectionPoseFactor, Constructor4) {
|
|||
factor1.add(measurement1, x1, model, sharedK);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( SmartProjectionPoseFactor, ConstructorWithTransform) {
|
||||
using namespace vanillaPose;
|
||||
bool manageDegeneracy = true;
|
||||
bool enableEPI = false;
|
||||
SmartFactor factor1(rankTol, linThreshold, manageDegeneracy, enableEPI,
|
||||
body_P_sensor1);
|
||||
factor1.add(measurement1, x1, model, sharedK);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( SmartProjectionPoseFactor, Equals ) {
|
||||
using namespace vanillaPose;
|
||||
|
|
@ -277,117 +264,6 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) {
|
|||
tictoc_print_();
|
||||
}
|
||||
|
||||
/* *************************************************************************/
|
||||
TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ) {
|
||||
|
||||
using namespace vanillaPose;
|
||||
|
||||
// create arbitrary body_Pose_sensor (transforms from sensor to body)
|
||||
Pose3 sensor_to_body = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2),
|
||||
Point3(1, 1, 1)); // Pose3(); //
|
||||
|
||||
// These are the poses we want to estimate, from camera measurements
|
||||
Pose3 bodyPose1 = level_pose.compose(sensor_to_body.inverse());
|
||||
Pose3 bodyPose2 = pose_right.compose(sensor_to_body.inverse());
|
||||
Pose3 bodyPose3 = pose_above.compose(sensor_to_body.inverse());
|
||||
|
||||
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
|
||||
|
||||
// Project three landmarks into three cameras
|
||||
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
|
||||
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
|
||||
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
|
||||
|
||||
// Create smart factors
|
||||
vector<Key> views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
|
||||
double rankTol = 1;
|
||||
double linThreshold = -1;
|
||||
bool manageDegeneracy = false;
|
||||
bool enableEPI = false;
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(
|
||||
new SmartFactor(rankTol, linThreshold, manageDegeneracy, enableEPI,
|
||||
sensor_to_body));
|
||||
smartFactor1->add(measurements_cam1, views, model, sharedK);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(
|
||||
new SmartFactor(rankTol, linThreshold, manageDegeneracy, enableEPI,
|
||||
sensor_to_body));
|
||||
smartFactor2->add(measurements_cam2, views, model, sharedK);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(
|
||||
new SmartFactor(rankTol, linThreshold, manageDegeneracy, enableEPI,
|
||||
sensor_to_body));
|
||||
smartFactor3->add(measurements_cam3, views, model, sharedK);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
||||
// Put all factors in factor graph, adding priors
|
||||
NonlinearFactorGraph graph;
|
||||
graph.push_back(smartFactor1);
|
||||
graph.push_back(smartFactor2);
|
||||
graph.push_back(smartFactor3);
|
||||
graph.push_back(PriorFactor<Pose3>(x1, bodyPose1, noisePrior));
|
||||
graph.push_back(PriorFactor<Pose3>(x2, bodyPose2, noisePrior));
|
||||
|
||||
// Check errors at ground truth poses
|
||||
Values gtValues;
|
||||
gtValues.insert(x1, bodyPose1);
|
||||
gtValues.insert(x2, bodyPose2);
|
||||
gtValues.insert(x3, bodyPose3);
|
||||
double actualError = graph.error(gtValues);
|
||||
double expectedError = 0.0;
|
||||
DOUBLES_EQUAL(expectedError, actualError, 1e-7)
|
||||
|
||||
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
|
||||
Point3(0.1, 0.1, 0.1));
|
||||
Values values;
|
||||
values.insert(x1, bodyPose1);
|
||||
values.insert(x2, bodyPose2);
|
||||
// initialize third pose with some noise, we expect it to move back to original pose_above
|
||||
values.insert(x3, bodyPose3 * noise_pose);
|
||||
|
||||
LevenbergMarquardtParams params;
|
||||
Values result;
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, params);
|
||||
result = optimizer.optimize();
|
||||
|
||||
// result.print("results of 3 camera, 3 landmark optimization \n");
|
||||
if (isDebugTest)
|
||||
result.at<Pose3>(x3).print("Smart: Pose3 after optimization: ");
|
||||
EXPECT(assert_equal(bodyPose3, result.at<Pose3>(x3)));
|
||||
|
||||
// Check derivatives
|
||||
|
||||
// Calculate E and P using computeEP, triangulates
|
||||
Matrix actualE;
|
||||
smartFactor1->triangulateAndComputeE(actualE, values);
|
||||
|
||||
// Calculate expected derivative for point (easiest to check)
|
||||
SmartFactor::Cameras cameras = smartFactor1->cameras(values);
|
||||
boost::function<Vector(Point3)> f = //
|
||||
boost::bind(&SmartFactor::reprojectionError, *smartFactor1, cameras, _1);
|
||||
boost::optional<Point3> point = smartFactor1->point();
|
||||
CHECK(point);
|
||||
|
||||
// TODO, this is really a test of CameraSet
|
||||
Matrix expectedE = numericalDerivative11<Vector, Point3>(f, *point);
|
||||
EXPECT(assert_equal(expectedE, actualE, 1e-7));
|
||||
|
||||
// Calculate using reprojectionError
|
||||
Matrix E;
|
||||
SmartFactor::Cameras::FBlocks F;
|
||||
Vector actualErrors = smartFactor1->reprojectionError(cameras, *point, F, E);
|
||||
EXPECT(assert_equal(expectedE, E, 1e-7));
|
||||
|
||||
// Success ! The derivatives of reprojectionError now agree with f !
|
||||
EXPECT(assert_equal(f(*point), actualErrors, 1e-7));
|
||||
}
|
||||
|
||||
/* *************************************************************************/
|
||||
TEST( SmartProjectionPoseFactor, Factors ) {
|
||||
|
||||
|
|
@ -599,15 +475,15 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) {
|
|||
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(
|
||||
new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD));
|
||||
new SmartFactor(1, -1, false, false, JACOBIAN_SVD));
|
||||
smartFactor1->add(measurements_cam1, views, model, sharedK);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(
|
||||
new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD));
|
||||
new SmartFactor(1, -1, false, false, JACOBIAN_SVD));
|
||||
smartFactor2->add(measurements_cam2, views, model, sharedK);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(
|
||||
new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD));
|
||||
new SmartFactor(1, -1, false, false, JACOBIAN_SVD));
|
||||
smartFactor3->add(measurements_cam3, views, model, sharedK);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
|
@ -654,17 +530,17 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) {
|
|||
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(
|
||||
new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
|
||||
new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
|
||||
excludeLandmarksFutherThanDist));
|
||||
smartFactor1->add(measurements_cam1, views, model, sharedK);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(
|
||||
new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
|
||||
new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
|
||||
excludeLandmarksFutherThanDist));
|
||||
smartFactor2->add(measurements_cam2, views, model, sharedK);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(
|
||||
new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
|
||||
new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
|
||||
excludeLandmarksFutherThanDist));
|
||||
smartFactor3->add(measurements_cam3, views, model, sharedK);
|
||||
|
||||
|
|
@ -720,22 +596,22 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) {
|
|||
measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10, 10); // add outlier
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(
|
||||
new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
|
||||
new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
|
||||
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
|
||||
smartFactor1->add(measurements_cam1, views, model, sharedK);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(
|
||||
new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
|
||||
new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
|
||||
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
|
||||
smartFactor2->add(measurements_cam2, views, model, sharedK);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(
|
||||
new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
|
||||
new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
|
||||
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
|
||||
smartFactor3->add(measurements_cam3, views, model, sharedK);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor4(
|
||||
new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
|
||||
new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
|
||||
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
|
||||
smartFactor4->add(measurements_cam4, views, model, sharedK);
|
||||
|
||||
|
|
@ -782,15 +658,15 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) {
|
|||
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(
|
||||
new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q));
|
||||
new SmartFactor(1, -1, false, false, JACOBIAN_Q));
|
||||
smartFactor1->add(measurements_cam1, views, model, sharedK);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(
|
||||
new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q));
|
||||
new SmartFactor(1, -1, false, false, JACOBIAN_Q));
|
||||
smartFactor2->add(measurements_cam2, views, model, sharedK);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(
|
||||
new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q));
|
||||
new SmartFactor(1, -1, false, false, JACOBIAN_Q));
|
||||
smartFactor3->add(measurements_cam3, views, model, sharedK);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
|
|
|||
|
|
@ -130,16 +130,15 @@ public:
|
|||
*/
|
||||
SmartStereoProjectionFactor(const double rankTol, const double linThreshold,
|
||||
const bool manageDegeneracy, const bool enableEPI,
|
||||
boost::optional<Pose3> body_P_sensor = boost::none,
|
||||
double landmarkDistanceThreshold = 1e10,
|
||||
double dynamicOutlierRejectionThreshold = -1,
|
||||
SmartFactorStatePtr state = SmartFactorStatePtr(new SmartStereoProjectionFactorState())) :
|
||||
Base(body_P_sensor), rankTolerance_(rankTol), retriangulationThreshold_(
|
||||
1e-5), manageDegeneracy_(manageDegeneracy), enableEPI_(enableEPI), linearizationThreshold_(
|
||||
double dynamicOutlierRejectionThreshold = -1, SmartFactorStatePtr state =
|
||||
SmartFactorStatePtr(new SmartStereoProjectionFactorState())) :
|
||||
rankTolerance_(rankTol), retriangulationThreshold_(1e-5), manageDegeneracy_(
|
||||
manageDegeneracy), enableEPI_(enableEPI), linearizationThreshold_(
|
||||
linThreshold), degenerate_(false), cheiralityException_(false), throwCheirality_(
|
||||
false), verboseCheirality_(false), state_(state),
|
||||
landmarkDistanceThreshold_(landmarkDistanceThreshold),
|
||||
dynamicOutlierRejectionThreshold_(dynamicOutlierRejectionThreshold) {
|
||||
false), verboseCheirality_(false), state_(state), landmarkDistanceThreshold_(
|
||||
landmarkDistanceThreshold), dynamicOutlierRejectionThreshold_(
|
||||
dynamicOutlierRejectionThreshold) {
|
||||
}
|
||||
|
||||
/** Virtual destructor */
|
||||
|
|
|
|||
|
|
@ -65,15 +65,16 @@ public:
|
|||
* @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint,
|
||||
* otherwise the factor is simply neglected
|
||||
* @param enableEPI if set to true linear triangulation is refined with embedded LM iterations
|
||||
* @param body_P_sensor is the transform from body to sensor frame (default identity)
|
||||
*/
|
||||
SmartStereoProjectionPoseFactor(const double rankTol = 1,
|
||||
const double linThreshold = -1, const bool manageDegeneracy = false,
|
||||
const bool enableEPI = false, boost::optional<Pose3> body_P_sensor = boost::none,
|
||||
LinearizationMode linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10,
|
||||
const bool enableEPI = false, LinearizationMode linearizeTo = HESSIAN,
|
||||
double landmarkDistanceThreshold = 1e10,
|
||||
double dynamicOutlierRejectionThreshold = -1) :
|
||||
Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor,
|
||||
landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), linearizeTo_(linearizeTo) {}
|
||||
Base(rankTol, linThreshold, manageDegeneracy, enableEPI,
|
||||
landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), linearizeTo_(
|
||||
linearizeTo) {
|
||||
}
|
||||
|
||||
/** Virtual destructor */
|
||||
virtual ~SmartStereoProjectionPoseFactor() {}
|
||||
|
|
|
|||
|
|
@ -106,15 +106,6 @@ TEST( SmartStereoProjectionPoseFactor, Constructor4) {
|
|||
factor1.add(measurement1, poseKey1, model, K);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( SmartStereoProjectionPoseFactor, ConstructorWithTransform) {
|
||||
bool manageDegeneracy = true;
|
||||
bool enableEPI = false;
|
||||
SmartFactor factor1(rankTol, linThreshold, manageDegeneracy, enableEPI,
|
||||
body_P_sensor1);
|
||||
factor1.add(measurement1, poseKey1, model, K);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( SmartStereoProjectionPoseFactor, Equals ) {
|
||||
SmartFactor::shared_ptr factor1(new SmartFactor());
|
||||
|
|
@ -345,15 +336,15 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
|
|||
cam2, cam3, landmark3);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(
|
||||
new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD));
|
||||
new SmartFactor(1, -1, false, false, JACOBIAN_SVD));
|
||||
smartFactor1->add(measurements_cam1, views, model, K);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(
|
||||
new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD));
|
||||
new SmartFactor(1, -1, false, false, JACOBIAN_SVD));
|
||||
smartFactor2->add(measurements_cam2, views, model, K);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(
|
||||
new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD));
|
||||
new SmartFactor(1, -1, false, false, JACOBIAN_SVD));
|
||||
smartFactor3->add(measurements_cam3, views, model, K);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
|
@ -414,17 +405,17 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
|
|||
cam2, cam3, landmark3);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(
|
||||
new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
|
||||
new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
|
||||
excludeLandmarksFutherThanDist));
|
||||
smartFactor1->add(measurements_cam1, views, model, K);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(
|
||||
new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
|
||||
new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
|
||||
excludeLandmarksFutherThanDist));
|
||||
smartFactor2->add(measurements_cam2, views, model, K);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(
|
||||
new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
|
||||
new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
|
||||
excludeLandmarksFutherThanDist));
|
||||
smartFactor3->add(measurements_cam3, views, model, K);
|
||||
|
||||
|
|
@ -493,22 +484,22 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
|
|||
measurements_cam4.at(0) = measurements_cam4.at(0) + StereoPoint2(10, 10, 1); // add outlier
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(
|
||||
new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
|
||||
new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
|
||||
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
|
||||
smartFactor1->add(measurements_cam1, views, model, K);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(
|
||||
new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
|
||||
new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
|
||||
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
|
||||
smartFactor2->add(measurements_cam2, views, model, K);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(
|
||||
new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
|
||||
new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
|
||||
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
|
||||
smartFactor3->add(measurements_cam3, views, model, K);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor4(
|
||||
new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
|
||||
new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
|
||||
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
|
||||
smartFactor4->add(measurements_cam4, views, model, K);
|
||||
|
||||
|
|
@ -567,13 +558,13 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
|
|||
// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
|
||||
// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
|
||||
//
|
||||
// SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q));
|
||||
// SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, JACOBIAN_Q));
|
||||
// smartFactor1->add(measurements_cam1, views, model, K);
|
||||
//
|
||||
// SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q));
|
||||
// SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, JACOBIAN_Q));
|
||||
// smartFactor2->add(measurements_cam2, views, model, K);
|
||||
//
|
||||
// SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q));
|
||||
// SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, JACOBIAN_Q));
|
||||
// smartFactor3->add(measurements_cam3, views, model, K);
|
||||
//
|
||||
// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
|
|
|||
Loading…
Reference in New Issue