Restored reprojectionErrors -> reprojectionError
parent
ae1f534e66
commit
eb28d0ffa8
|
@ -140,7 +140,7 @@ public:
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Calculate vector of re-projection errors
|
/// Calculate vector of re-projection errors
|
||||||
Vector reprojectionErrors(const Point3& point, const std::vector<Z>& measured,
|
Vector reprojectionError(const Point3& point, const std::vector<Z>& measured,
|
||||||
boost::optional<FBlocks&> F = boost::none, //
|
boost::optional<FBlocks&> F = boost::none, //
|
||||||
boost::optional<Matrix&> E = boost::none) const {
|
boost::optional<Matrix&> E = boost::none) const {
|
||||||
return ErrorVector(project2(point,F,E), measured);
|
return ErrorVector(project2(point,F,E), measured);
|
||||||
|
@ -148,7 +148,7 @@ public:
|
||||||
|
|
||||||
/// Calculate vector of re-projection errors, from point at infinity
|
/// Calculate vector of re-projection errors, from point at infinity
|
||||||
// TODO: take Unit3 instead
|
// TODO: take Unit3 instead
|
||||||
Vector reprojectionErrorsAtInfinity(const Point3& point,
|
Vector reprojectionErrorAtInfinity(const Point3& point,
|
||||||
const std::vector<Z>& measured) const {
|
const std::vector<Z>& measured) const {
|
||||||
return ErrorVector(projectAtInfinity(point), measured);
|
return ErrorVector(projectAtInfinity(point), measured);
|
||||||
}
|
}
|
||||||
|
|
|
@ -73,16 +73,16 @@ TEST(CameraSet, Pinhole) {
|
||||||
measured.push_back(Point2(3, 4));
|
measured.push_back(Point2(3, 4));
|
||||||
Vector4 expectedV;
|
Vector4 expectedV;
|
||||||
|
|
||||||
// reprojectionErrors
|
// reprojectionError
|
||||||
expectedV << -1, -2, -3, -4;
|
expectedV << -1, -2, -3, -4;
|
||||||
Vector actualV = set.reprojectionErrors(p, measured);
|
Vector actualV = set.reprojectionError(p, measured);
|
||||||
EXPECT(assert_equal(expectedV, actualV));
|
EXPECT(assert_equal(expectedV, actualV));
|
||||||
|
|
||||||
// reprojectionErrorsAtInfinity
|
// reprojectionErrorAtInfinity
|
||||||
EXPECT(
|
EXPECT(
|
||||||
assert_equal(Point3(0, 0, 1),
|
assert_equal(Point3(0, 0, 1),
|
||||||
camera.backprojectPointAtInfinity(Point2())));
|
camera.backprojectPointAtInfinity(Point2())));
|
||||||
actualV = set.reprojectionErrorsAtInfinity(p, measured);
|
actualV = set.reprojectionErrorAtInfinity(p, measured);
|
||||||
EXPECT(assert_equal(expectedV, actualV));
|
EXPECT(assert_equal(expectedV, actualV));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -215,7 +215,7 @@ public:
|
||||||
|
|
||||||
/// Calculate vector of re-projection errors, noise model applied
|
/// Calculate vector of re-projection errors, noise model applied
|
||||||
Vector whitenedErrors(const Cameras& cameras, const Point3& point) const {
|
Vector whitenedErrors(const Cameras& cameras, const Point3& point) const {
|
||||||
Vector b = cameras.reprojectionErrors(point, measured_);
|
Vector b = cameras.reprojectionError(point, measured_);
|
||||||
if (noiseModel_)
|
if (noiseModel_)
|
||||||
noiseModel_->whitenInPlace(b);
|
noiseModel_->whitenInPlace(b);
|
||||||
return b;
|
return b;
|
||||||
|
@ -225,7 +225,7 @@ public:
|
||||||
// TODO: Unit3
|
// TODO: Unit3
|
||||||
Vector whitenedErrorsAtInfinity(const Cameras& cameras,
|
Vector whitenedErrorsAtInfinity(const Cameras& cameras,
|
||||||
const Point3& point) const {
|
const Point3& point) const {
|
||||||
Vector b = cameras.reprojectionErrorsAtInfinity(point, measured_);
|
Vector b = cameras.reprojectionErrorAtInfinity(point, measured_);
|
||||||
if (noiseModel_)
|
if (noiseModel_)
|
||||||
noiseModel_->whitenInPlace(b);
|
noiseModel_->whitenInPlace(b);
|
||||||
return b;
|
return b;
|
||||||
|
@ -252,8 +252,8 @@ public:
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Compute reprojection errors
|
/// Compute reprojection errors
|
||||||
Vector reprojectionErrors(const Cameras& cameras, const Point3& point) const {
|
Vector reprojectionError(const Cameras& cameras, const Point3& point) const {
|
||||||
return cameras.reprojectionErrors(point, measured_);
|
return cameras.reprojectionError(point, measured_);
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
@ -261,10 +261,10 @@ public:
|
||||||
* TODO: the treatment of body_P_sensor_ is weird: the transformation
|
* TODO: the treatment of body_P_sensor_ is weird: the transformation
|
||||||
* is applied in the caller, but the derivatives are computed here.
|
* is applied in the caller, but the derivatives are computed here.
|
||||||
*/
|
*/
|
||||||
Vector reprojectionErrors(const Cameras& cameras, const Point3& point,
|
Vector reprojectionError(const Cameras& cameras, const Point3& point,
|
||||||
typename Cameras::FBlocks& F, Matrix& E) const {
|
typename Cameras::FBlocks& F, Matrix& E) const {
|
||||||
|
|
||||||
Vector b = cameras.reprojectionErrors(point, measured_, F, E);
|
Vector b = cameras.reprojectionError(point, measured_, F, E);
|
||||||
|
|
||||||
// Apply sensor chain rule if needed TODO: no simpler way ??
|
// Apply sensor chain rule if needed TODO: no simpler way ??
|
||||||
if (body_P_sensor_) {
|
if (body_P_sensor_) {
|
||||||
|
@ -308,7 +308,7 @@ public:
|
||||||
/// Assumes non-degenerate !
|
/// Assumes non-degenerate !
|
||||||
void computeEP(Matrix& E, Matrix& P, const Cameras& cameras,
|
void computeEP(Matrix& E, Matrix& P, const Cameras& cameras,
|
||||||
const Point3& point) const {
|
const Point3& point) const {
|
||||||
cameras.reprojectionErrors(point, measured_, boost::none, E);
|
cameras.reprojectionError(point, measured_, boost::none, E);
|
||||||
P = PointCov(E);
|
P = PointCov(E);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -322,7 +322,7 @@ public:
|
||||||
|
|
||||||
// Project into Camera set and calculate derivatives
|
// Project into Camera set and calculate derivatives
|
||||||
typename Cameras::FBlocks F;
|
typename Cameras::FBlocks F;
|
||||||
b = reprojectionErrors(cameras, point, F, E);
|
b = reprojectionError(cameras, point, F, E);
|
||||||
|
|
||||||
// Now calculate f and divide up the F derivatives into Fblocks
|
// Now calculate f and divide up the F derivatives into Fblocks
|
||||||
double f = 0.0;
|
double f = 0.0;
|
||||||
|
|
|
@ -573,7 +573,7 @@ public:
|
||||||
/// Calculate vector of re-projection errors, before applying noise model
|
/// Calculate vector of re-projection errors, before applying noise model
|
||||||
/// Assumes triangulation was done and degeneracy handled
|
/// Assumes triangulation was done and degeneracy handled
|
||||||
Vector reprojectionError(const Cameras& cameras) const {
|
Vector reprojectionError(const Cameras& cameras) const {
|
||||||
return cameras.reprojectionErrors(point_,this->measured_);
|
return cameras.reprojectionError(point_,this->measured_);
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Calculate vector of re-projection errors, before applying noise model
|
/// Calculate vector of re-projection errors, before applying noise model
|
||||||
|
|
|
@ -142,10 +142,10 @@ TEST_UNSAFE( SmartProjectionPoseFactor, noiseless ) {
|
||||||
factor.computeEP(actualE, PointCov, values);
|
factor.computeEP(actualE, PointCov, values);
|
||||||
EXPECT(assert_equal(expectedE, actualE, 1e-7));
|
EXPECT(assert_equal(expectedE, actualE, 1e-7));
|
||||||
|
|
||||||
// Calculate using reprojectionErrors, note not yet divided by sigma !
|
// Calculate using reprojectionError, note not yet divided by sigma !
|
||||||
SmartFactor::Cameras::FBlocks F;
|
SmartFactor::Cameras::FBlocks F;
|
||||||
Matrix E;
|
Matrix E;
|
||||||
Vector actualErrors = factor.reprojectionErrors(cameras, *point, F, E);
|
Vector actualErrors = factor.reprojectionError(cameras, *point, F, E);
|
||||||
EXPECT(assert_equal(expectedE, E, 1e-7));
|
EXPECT(assert_equal(expectedE, E, 1e-7));
|
||||||
|
|
||||||
EXPECT(assert_equal(zero(4), actualErrors, 1e-7));
|
EXPECT(assert_equal(zero(4), actualErrors, 1e-7));
|
||||||
|
@ -379,10 +379,10 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ) {
|
||||||
// Calculate using whitenedError
|
// Calculate using whitenedError
|
||||||
Matrix E;
|
Matrix E;
|
||||||
SmartFactor::Cameras::FBlocks F;
|
SmartFactor::Cameras::FBlocks F;
|
||||||
Vector actualErrors = smartFactor1->reprojectionErrors(cameras, *point, F, E);
|
Vector actualErrors = smartFactor1->reprojectionError(cameras, *point, F, E);
|
||||||
EXPECT(assert_equal(expectedE, E, 1e-7));
|
EXPECT(assert_equal(expectedE, E, 1e-7));
|
||||||
|
|
||||||
// Success ! The derivatives of reprojectionErrors now agree with f !
|
// Success ! The derivatives of reprojectionError now agree with f !
|
||||||
EXPECT(assert_equal(f(*point) * sigma, actualErrors, 1e-7));
|
EXPECT(assert_equal(f(*point) * sigma, actualErrors, 1e-7));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -629,7 +629,7 @@ public:
|
||||||
Cameras cameras;
|
Cameras cameras;
|
||||||
bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
|
bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
|
||||||
if (nonDegenerate)
|
if (nonDegenerate)
|
||||||
return cameras.reprojectionErrors(point_);
|
return cameras.reprojectionError(point_);
|
||||||
else
|
else
|
||||||
return zero(cameras.size() * 3);
|
return zero(cameras.size() * 3);
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue