18 MakeFromKF(kf, dBlur);
36 mimSmall.resize(mirSize);
37 mimTemplate.resize(mirSize);
42 unsigned int nSum = 0;
45 while(ir.
next(mirSize));
47 float fMean = ((float) nSum) / mirSize.
area();
51 mimTemplate[
ir] = mimSmall[
ir] - fMean;
52 while(ir.
next(mirSize));
61 mimImageJacs.resize(mirSize);
67 if(mimTemplate.in_image_with_border(ir,1))
69 v2Grad[0] = mimTemplate[ir +
ImageRef(1,0)] - mimTemplate[ir -
ImageRef(1,0)];
70 v2Grad[1] = mimTemplate[ir +
ImageRef(0,1)] - mimTemplate[ir -
ImageRef(0,1)];
76 while(ir.
next(mirSize));
89 dSSD += dDiff * dDiff;
91 while(ir.
next(mirSize));
105 pair<SE2<>,
double> result_pair;
108 cerr <<
"You spanner, you didn't make the jacs for the target." << endl;
112 double dMeanOffset = 0.0;
118 double dFinalScore = 0.0;
119 for(
int it = 0; it<nIterations; it++)
127 SE2<> se2XForm = se2WfromC * se2CtoC * se2WfromC.
inverse();
145 if(l + r + u + d + here < -9999.9)
149 v2CurrentGrad[0] = r - l;
150 v2CurrentGrad[1] = d - u;
157 v4Jac[0] = v2SumGrad[0];
158 v4Jac[1] = v2SumGrad[1];
159 v4Jac[2] = -(ir.y - irCenter.
y) * v2SumGrad[0] + (ir.x - irCenter.
x) * v2SumGrad[1];
163 dFinalScore += dDiff * dDiff;
165 v4Accum += dDiff * v4Jac;
168 double *p = &v10Triangle[0];
169 *p++ += v4Jac[0] * v4Jac[0];
170 *p++ += v4Jac[1] * v4Jac[0];
171 *p++ += v4Jac[1] * v4Jac[1];
172 *p++ += v4Jac[2] * v4Jac[0];
173 *p++ += v4Jac[2] * v4Jac[1];
174 *p++ += v4Jac[2] * v4Jac[2];
180 while(ir.next(mirSize));
188 for(
int j=0; j<4; j++)
189 for(
int i=0; i<=j; i++)
190 m4[j][i] = m4[i][j] = v10Triangle[v++];
192 v4Update = chol.
backsub(v4Accum);
198 se2CtoC = se2CtoC * se2Update;
199 dMeanOffset -= v4Update[3];
202 result_pair.first = se2CtoC;
203 result_pair.second = dFinalScore;
227 for(
int it = 0; it<3; it++)
231 for(
int i=0; i<2; i++)
234 Vector<3> v3Cam = so3 * av3OrigPoints[i];
237 Vector<2> v2Error = av2Turned[i] - v2Pixels;
241 double dOneOverCameraZ = 1.0 / v3Cam[2];
242 for(
int m=0; m<3; m++)
246 v2CamFrameMotion[0] = (v3Motion[0] - v3Cam[0] * v3Motion[2] * dOneOverCameraZ) * dOneOverCameraZ;
247 v2CamFrameMotion[1] = (v3Motion[1] - v3Cam[1] * v3Motion[2] * dOneOverCameraZ) * dOneOverCameraZ;
248 m23Jacobian.T()[m] = m2CamDerivs * v2CamFrameMotion;
250 wls.
add_mJ(v2Error[0], m23Jacobian[0], 1.0);
251 wls.
add_mJ(v2Error[1], m23Jacobian[1], 1.0);
SE2 inverse() const
compute the inverse of the transformation
void halfSample(const BasicImage< T > &in, BasicImage< T > &out)
std::pair< SE2<>, double > IteratePosRelToTarget(SmallBlurryImage &other, int nIterations=10)
void MakeFromKF(KeyFrame &kf, double dBlur=2.5)
SO3< Precision > & get_rotation()
Returns the rotation part of the transformation as a SO3.
Vector< 2 > Project(const Vector< 2 > &camframe)
Vector< Size, Precision > backsub(const Vector< Size2, P2, B2 > &v) const
static CVD::ImageRef mirSize
TooN::Vector< 2 > vec(const ImageRef &ir)
void convolveGaussian(BasicImage< T > &I, double sigma, double sigmas=3.0)
CVD::Image< CVD::byte > im
Vector<(Size==Dynamic?Dynamic:Size+1), Precision > unproject(const Vector< Size, Precision, Base > &v)
void add_mJ(Precision m, const Vector< Size, Precision, B2 > &J, Precision weight=1)
int area() const
Area (product of x and y; signed)
SO2< Precision > & get_rotation()
Returns the rotation part of the transformation as a SO2.
CVD::Image< float > mimTemplate
Vector< 2 > UnProject(const Vector< 2 > &imframe)
Vector< Size, Precision > & get_mu()
Returns the update. With no prior, this is the result of .
bool next(const ImageRef &max)
int transform(const BasicImage< S > &in, BasicImage< T > &out, const TooN::Matrix< 2 > &M, const TooN::Vector< 2 > &inOrig, const TooN::Vector< 2 > &outOrig, const T defaultValue=T())
Matrix< R, C, P > exp(const Matrix< R, C, P, B > &m)
CVD::Image< Vector< 2 > > mimImageJacs
ImageRef size() const
What is the size of this image?
bool in_image_with_border(const ImageRef &ir, int border) const
ImageRef ir(const TooN::Vector< 2 > &v)
Vector< 2, Precision > & get_translation()
Returns the translation part of the transformation as a Vector.
void SetImageSize(Vector< 2 > v2ImageSize)
void home()
Resets the ImageRef to (0,0)
double ZMSSD(SmallBlurryImage &other)
void add_prior(Precision val)
Matrix< 2, 2 > GetProjectionDerivs()
static Operator< Internal::Zero > Zeros
Vector<(Size==Dynamic?Dynamic:Size-1), Precision > project(const Vector< Size, Precision, Base > &v)
static SE3 SE3fromSE2(SE2<> se2, ATANCamera camera)