added calibrate with jacobians for Cal3Bundler
							parent
							
								
									a4c3623701
								
							
						
					
					
						commit
						921dc848a0
					
				|  | @ -121,6 +121,39 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const { | |||
|   return pn; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| Point2 Cal3Bundler::calibrate(const Point2& p, OptionalJacobian<2, 3> Dcal, | ||||
|                               OptionalJacobian<2, 2> Dp) const { | ||||
|   Point2 pn = calibrate(p, 1e-5); | ||||
| 
 | ||||
|   // Approximate the jacobians via a single iteration of g.
 | ||||
|   if (Dcal) { | ||||
|     const double u = p.x(), v = p.y(), x = (u - u0_) / f_, y = (v - v0_) / f_; | ||||
|     const double xx = x * x, yy = y * y; | ||||
|     const double rr = xx + yy; | ||||
|     const double g = (1 + k1_ * rr + k2_ * rr * rr); | ||||
|     const double inv_f = 1 / f_, inv_g = 1 / g; | ||||
| 
 | ||||
|     *Dcal << -inv_f * x, -x * inv_g * rr, -x * inv_g * rr * rr, -inv_f * y, | ||||
|         -y * inv_g * rr, -y * inv_g * rr * rr; | ||||
|   } | ||||
|   if (Dp) { | ||||
|     const double u = p.x(), v = p.y(), x = (u - u0_) / f_, y = (v - v0_) / f_; | ||||
|     const double xx = x * x, yy = y * y; | ||||
|     const double rr = xx + yy; | ||||
|     const double g = (1 + k1_ * rr + k2_ * rr * rr); | ||||
|     const double inv_f = 1 / f_, inv_g = 1 / g; | ||||
| 
 | ||||
|     const double dg_du = (2 * k1_ * x * inv_f) + (4 * k2_ * rr * x * inv_f); | ||||
|     const double dg_dv = (2 * k1_ * y * inv_f) + (4 * k2_ * rr * y * inv_f); | ||||
| 
 | ||||
|     *Dp << (inv_f * inv_g) - (pn.x() * inv_g * dg_du), -pn.x() * inv_g * dg_dv, | ||||
|         -pn.y() * inv_g * dg_du, (inv_f * inv_g) - (pn.x() * inv_g * dg_dv); | ||||
|   } | ||||
| 
 | ||||
|   return pn; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| Matrix2 Cal3Bundler::D2d_intrinsic(const Point2& p) const { | ||||
|   Matrix2 Dp; | ||||
|  |  | |||
|  | @ -120,6 +120,16 @@ public: | |||
|   /// Convert a pixel coordinate to ideal coordinate
 | ||||
|   Point2 calibrate(const Point2& pi, const double tol = 1e-5) const; | ||||
| 
 | ||||
|   /**
 | ||||
|    * Convert image coordinates uv to intrinsic coordinates xy | ||||
|    * @param p point in image coordinates | ||||
|    * @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& p, 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; | ||||
| 
 | ||||
|  |  | |||
|  | @ -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); } | ||||
| /* ************************************************************************* */ | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue