12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge branch 'develop' into fix/means 

# Conflicts:
#	gtsam/geometry/Point3.cpp
release/4.3a0
Frank Dellaert 2020-10-01 13:58:55 -04:00
parent eb4f5288e9 151923cf3b
commit eb0b48a10f
8 changed files with 83 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
gtsam/geometry/Cal3Bundler.cpp
View File

@ -25,13 +25,13 @@ namespace gtsam {
/* ************************************************************************* */
Cal3Bundler::Cal3Bundler() :
f_(1), k1_(0), k2_(0), u0_(0), v0_(0) {
f_(1), k1_(0), k2_(0), u0_(0), v0_(0), tol_(1e-5) {
}
/* ************************************************************************* */
Cal3Bundler::Cal3Bundler(double f, double k1, double k2, double u0, double v0) :
f_(f), k1_(k1), k2_(k2), u0_(u0), v0_(v0) {
}
Cal3Bundler::Cal3Bundler(double f, double k1, double k2, double u0, double v0,
double tol)
: f_(f), k1_(k1), k2_(k2), u0_(u0), v0_(v0), tol_(tol) {}
/* ************************************************************************* */
Matrix3 Cal3Bundler::K() const {
@ -94,21 +94,24 @@ Point2 Cal3Bundler::uncalibrate(const Point2& p, //
}
/* ************************************************************************* */
Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const {
Point2 Cal3Bundler::calibrate(const Point2& pi,
OptionalJacobian<2, 3> Dcal,
OptionalJacobian<2, 2> Dp) const {
// Copied from Cal3DS2 :-(
// but specialized with k1,k2 non-zero only and fx=fy and s=0
const Point2 invKPi((pi.x() - u0_)/f_, (pi.y() - v0_)/f_);
double x = (pi.x() - u0_)/f_, y = (pi.y() - v0_)/f_;
const Point2 invKPi(x, y);
// initialize by ignoring the distortion at all, might be problematic for pixels around boundary
Point2 pn = invKPi;
Point2 pn(x, y);
// iterate until the uncalibrate is close to the actual pixel coordinate
const int maxIterations = 10;
int iteration;
for (iteration = 0; iteration < maxIterations; ++iteration) {
if (distance2(uncalibrate(pn), pi) <= tol)
if (distance2(uncalibrate(pn), pi) <= tol_)
break;
const double x = pn.x(), y = pn.y(), xx = x * x, yy = y * y;
const double px = pn.x(), py = pn.y(), xx = px * px, yy = py * py;
const double rr = xx + yy;
const double g = (1 + k1_ * rr + k2_ * rr * rr);
pn = invKPi / g;
@ -118,6 +121,25 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const {
throw std::runtime_error(
"Cal3Bundler::calibrate fails to converge. need a better initialization");
// We make use of the Implicit Function Theorem to compute the Jacobians from uncalibrate
// Given f(pi, pn) = uncalibrate(pn) - pi, and g(pi) = calibrate, we can easily compute the Jacobians
// df/pi = -I (pn and pi are independent args)
// Dcal = -inv(H_uncal_pn) * df/pi = -inv(H_uncal_pn) * (-I) = inv(H_uncal_pn)
// Dp = -inv(H_uncal_pn) * df/K = -inv(H_uncal_pn) * H_uncal_K
Matrix23 H_uncal_K;
Matrix22 H_uncal_pn, H_uncal_pn_inv;
if (Dcal || Dp) {
// Compute uncalibrate Jacobians
uncalibrate(pn, Dcal ? &H_uncal_K : nullptr, H_uncal_pn);
H_uncal_pn_inv = H_uncal_pn.inverse();
if (Dp) *Dp = H_uncal_pn_inv;
if (Dcal) *Dcal = -H_uncal_pn_inv * H_uncal_K;
}
return pn;
}

19
gtsam/geometry/Cal3Bundler.h
View File

@ -33,6 +33,7 @@ private:
double f_; ///< focal length
double k1_, k2_; ///< radial distortion
double u0_, v0_; ///< image center, not a parameter to be optimized but a constant
double tol_; ///< tolerance value when calibrating
public:
@ -51,8 +52,10 @@ public:
* @param k2 second radial distortion coefficient (quartic)
* @param u0 optional image center (default 0), considered a constant
* @param v0 optional image center (default 0), considered a constant
* @param tol optional calibration tolerance value
*/
Cal3Bundler(double f, double k1, double k2, double u0 = 0, double v0 = 0);
Cal3Bundler(double f, double k1, double k2, double u0 = 0, double v0 = 0,
double tol = 1e-5);
virtual ~Cal3Bundler() {}
@ -117,8 +120,17 @@ public:
Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) const;
/// Convert a pixel coordinate to ideal coordinate
Point2 calibrate(const Point2& pi, const double tol = 1e-5) const;
/**
* Convert a pixel coordinate to ideal coordinate xy
* @param p point in image coordinates
* @param tol optional tolerance threshold value for iterative minimization
* @param Dcal optional 2*3 Jacobian wrpt Cal3Bundler parameters
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in intrinsic coordinates
*/
Point2 calibrate(const Point2& pi,
OptionalJacobian<2, 3> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) const;
/// @deprecated might be removed in next release, use uncalibrate
Matrix2 D2d_intrinsic(const Point2& p) const;
@ -164,6 +176,7 @@ private:
ar & BOOST_SERIALIZATION_NVP(k2_);
ar & BOOST_SERIALIZATION_NVP(u0_);
ar & BOOST_SERIALIZATION_NVP(v0_);
ar & BOOST_SERIALIZATION_NVP(tol_);
}
/// @}

1
gtsam/geometry/Point3.cpp
View File

@ -77,6 +77,7 @@ double dot(const Point3 &p, const Point3 &q, OptionalJacobian<1, 3> H1,
Point3Pair means(const std::vector<Point3Pair> &abPointPairs) {
const size_t n = abPointPairs.size();
if (n == 0) throw std::invalid_argument("Point3::mean input Point3Pair vector is empty");
Point3 aSum(0, 0, 0), bSum(0, 0, 0);
for (const Point3Pair &abPair : abPointPairs) {
aSum += abPair.first;

1
gtsam/geometry/Point3.h
View File

@ -62,6 +62,7 @@ GTSAM_EXPORT double dot(const Point3& p, const Point3& q,
/// mean
template <class CONTAINER>
GTSAM_EXPORT Point3 mean(const CONTAINER& points) {
if (points.size() == 0) throw std::invalid_argument("Point3::mean input container is empty");
Point3 sum(0, 0, 0);
sum = std::accumulate(points.begin(), points.end(), sum);
return sum / points.size();

23
gtsam/geometry/tests/testCal3Bundler.cpp
View File

@ -52,12 +52,14 @@ TEST( Cal3Bundler, calibrate )
Point2 pn(0.5, 0.5);
Point2 pi = K.uncalibrate(pn);
Point2 pn_hat = K.calibrate(pi);
CHECK( traits<Point2>::Equals(pn, pn_hat, 1e-5));
CHECK(traits<Point2>::Equals(pn, pn_hat, 1e-5));
}
/* ************************************************************************* */
Point2 uncalibrate_(const Cal3Bundler& k, const Point2& pt) { return k.uncalibrate(pt); }
Point2 calibrate_(const Cal3Bundler& k, const Point2& pt) { return k.calibrate(pt); }
/* ************************************************************************* */
TEST( Cal3Bundler, Duncalibrate)
{
@ -69,11 +71,20 @@ TEST( Cal3Bundler, Duncalibrate)
Matrix numerical2 = numericalDerivative22(uncalibrate_, K, p);
CHECK(assert_equal(numerical1,Dcal,1e-7));
CHECK(assert_equal(numerical2,Dp,1e-7));
CHECK(assert_equal(numerical1,K.D2d_calibration(p),1e-7));
CHECK(assert_equal(numerical2,K.D2d_intrinsic(p),1e-7));
Matrix Dcombined(2,5);
Dcombined << Dp, Dcal;
CHECK(assert_equal(Dcombined,K.D2d_intrinsic_calibration(p),1e-7));
}
/* ************************************************************************* */
TEST( Cal3Bundler, Dcalibrate)
{
Matrix Dcal, Dp;
Point2 pn(0.5, 0.5);
Point2 pi = K.uncalibrate(pn);
Point2 actual = K.calibrate(pi, Dcal, Dp);
CHECK(assert_equal(pn, actual, 1e-7));
Matrix numerical1 = numericalDerivative21(calibrate_, K, pi);
Matrix numerical2 = numericalDerivative22(calibrate_, K, pi);
CHECK(assert_equal(numerical1,Dcal,1e-5));
CHECK(assert_equal(numerical2,Dp,1e-5));
}
/* ************************************************************************* */

9
gtsam/geometry/tests/testPinholeCamera.cpp
View File

@ -336,6 +336,15 @@ TEST( PinholeCamera, range3) {
EXPECT(assert_equal(Hexpected2, D2, 1e-7));
}
/* ************************************************************************* */
TEST( PinholeCamera, Cal3Bundler) {
Cal3Bundler calibration;
Pose3 wTc;
PinholeCamera<Cal3Bundler> camera(wTc, calibration);
Point2 p(50, 100);
camera.backproject(p, 10);
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

10
gtsam/gtsam.i
View File

@ -959,6 +959,7 @@ class Cal3Bundler {
// Standard Constructors
Cal3Bundler();
Cal3Bundler(double fx, double k1, double k2, double u0, double v0);
Cal3Bundler(double fx, double k1, double k2, double u0, double v0, double tol);
// Testable
void print(string s) const;
@ -971,7 +972,7 @@ class Cal3Bundler {
Vector localCoordinates(const gtsam::Cal3Bundler& c) const;
// Action on Point2
gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const;
gtsam::Point2 calibrate(const gtsam::Point2& p) const;
gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
// Standard Interface
@ -1105,7 +1106,7 @@ typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
//TODO (Issue 237) due to lack of jacobians of Cal3DS2_Base::calibrate, PinholeCamera does not apply to Cal3DS2/Unified
//typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
//typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
//typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
#include <gtsam/geometry/StereoCamera.h>
class StereoCamera {
@ -2462,6 +2463,8 @@ class ISAM2Result {
size_t getVariablesRelinearized() const;
size_t getVariablesReeliminated() const;
size_t getCliques() const;
double getErrorBefore() const;
double getErrorAfter() const;
};
class ISAM2 {
@ -2766,8 +2769,7 @@ class SfmTrack {
class SfmData {
size_t number_cameras() const;
size_t number_tracks() const;
//TODO(Varun) Need to fix issue #237 first before this can work
// gtsam::PinholeCamera<gtsam::Cal3Bundler> camera(size_t idx) const;
gtsam::PinholeCamera<gtsam::Cal3Bundler> camera(size_t idx) const;
gtsam::SfmTrack track(size_t idx) const;
};

2
gtsam/nonlinear/ISAM2Result.h
View File

@ -176,6 +176,8 @@ struct ISAM2Result {
size_t getVariablesRelinearized() const { return variablesRelinearized; }
size_t getVariablesReeliminated() const { return variablesReeliminated; }
size_t getCliques() const { return cliques; }
double getErrorBefore() const { return errorBefore ? *errorBefore : std::nan(""); }
double getErrorAfter() const { return errorAfter ? *errorAfter : std::nan(""); }
};
} // namespace gtsam