[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[libcvd-members] libcvd cvd/camera.h cvd/gl_helpers.h cvd/image_...
|
From: |
Gerhard Reitmayr |
|
Subject: |
[libcvd-members] libcvd cvd/camera.h cvd/gl_helpers.h cvd/image_... |
|
Date: |
Tue, 14 Apr 2009 12:57:58 +0000 |
CVSROOT: /cvsroot/libcvd
Module name: libcvd
Changes by: Gerhard Reitmayr <gerhard> 09/04/14 12:57:57
Modified files:
cvd : camera.h gl_helpers.h image_interpolate.h
tensor_voting.h
cvd_src : tensor_voting.cc
progs : calibrate.cxx se3_exp.cxx se3_inv.cxx
se3_ln.cxx se3_post_mul.cxx se3_pre_mul.cxx
Log message:
Make libcvd TooN2 compatible.
This will break compilation against TooN1 and requires an update to
TooN2.
The last version working with TooN1 was tagged with BEFORE_TOON2 , use
this
tag to checkout a TooN1 compatible version.
CVSWeb URLs:
http://cvs.savannah.gnu.org/viewcvs/libcvd/cvd/camera.h?cvsroot=libcvd&r1=1.22&r2=1.23
http://cvs.savannah.gnu.org/viewcvs/libcvd/cvd/gl_helpers.h?cvsroot=libcvd&r1=1.41&r2=1.42
http://cvs.savannah.gnu.org/viewcvs/libcvd/cvd/image_interpolate.h?cvsroot=libcvd&r1=1.18&r2=1.19
http://cvs.savannah.gnu.org/viewcvs/libcvd/cvd/tensor_voting.h?cvsroot=libcvd&r1=1.6&r2=1.7
http://cvs.savannah.gnu.org/viewcvs/libcvd/cvd_src/tensor_voting.cc?cvsroot=libcvd&r1=1.2&r2=1.3
http://cvs.savannah.gnu.org/viewcvs/libcvd/progs/calibrate.cxx?cvsroot=libcvd&r1=1.13&r2=1.14
http://cvs.savannah.gnu.org/viewcvs/libcvd/progs/se3_exp.cxx?cvsroot=libcvd&r1=1.5&r2=1.6
http://cvs.savannah.gnu.org/viewcvs/libcvd/progs/se3_inv.cxx?cvsroot=libcvd&r1=1.4&r2=1.5
http://cvs.savannah.gnu.org/viewcvs/libcvd/progs/se3_ln.cxx?cvsroot=libcvd&r1=1.3&r2=1.4
http://cvs.savannah.gnu.org/viewcvs/libcvd/progs/se3_post_mul.cxx?cvsroot=libcvd&r1=1.4&r2=1.5
http://cvs.savannah.gnu.org/viewcvs/libcvd/progs/se3_pre_mul.cxx?cvsroot=libcvd&r1=1.4&r2=1.5
Patches:
Index: cvd/camera.h
===================================================================
RCS file: /cvsroot/libcvd/libcvd/cvd/camera.h,v
retrieving revision 1.22
retrieving revision 1.23
diff -u -b -r1.22 -r1.23
--- cvd/camera.h 4 Nov 2008 12:52:15 -0000 1.22
+++ cvd/camera.h 14 Apr 2009 12:57:53 -0000 1.23
@@ -615,8 +615,8 @@
}
TooN::Matrix<2,2> Camera::Cubic::get_derivative(){
- TooN::Matrix<2,2> result;
-
Identity(result,1+my_camera_parameters[4]*(my_last_camframe*my_last_camframe));
+ TooN::Matrix<2,2> result = TooN::Identity;
+ result *= 1+my_camera_parameters[4]*(my_last_camframe*my_last_camframe);
result += (2*my_camera_parameters[4]*my_last_camframe.as_col()) *
my_last_camframe.as_row();
result[0] *= my_camera_parameters[0];
result[1] *= my_camera_parameters[1];
@@ -731,10 +731,10 @@
TooN::Matrix<2,2> Camera::Quintic::get_derivative() const {
- TooN::Matrix<2,2> result;
+ TooN::Matrix<2,2> result = TooN::Identity;
double temp1=my_last_camframe*my_last_camframe;
double temp2=my_camera_parameters[5]*temp1;
- Identity(result,1+temp1*(my_camera_parameters[4]+temp2));
+ result *= 1+temp1*(my_camera_parameters[4]+temp2);
result += (2*(my_camera_parameters[4]+2*temp2)*my_last_camframe.as_col()) *
my_last_camframe.as_row();
result[0] *= my_camera_parameters[0];
result[1] *= my_camera_parameters[1];
@@ -744,12 +744,12 @@
TooN::Matrix<2,2> Camera::Quintic::get_inv_derivative() const {
- TooN::Matrix<2,2> result;
+ TooN::Matrix<2,2> result = TooN::Identity;
double temp1=my_last_camframe*my_last_camframe;
double temp2=my_camera_parameters[5]*temp1;
double temp3=2.0*(my_camera_parameters[4]+2.0*temp2);
- Identity(result,1+temp1*(my_camera_parameters[4]+temp2));
+ result *= 1+temp1*(my_camera_parameters[4]+temp2);
result[0][0] += my_last_camframe[1]*my_last_camframe[1]*temp3;
result[0][1] =-(temp3*my_last_camframe[0]*my_last_camframe[1]);
@@ -768,12 +768,13 @@
TooN::Matrix<2,2> Camera::Quintic::get_inv_derivative(const TooN::Vector<2>&
x) const
{
- TooN::Matrix<2,2> result;
+ TooN::Matrix<2,2> result = TooN::Identity;
double temp1=x*x;
double temp2=my_camera_parameters[5]*temp1;
double temp3=2.0*(my_camera_parameters[4]+2.0*temp2);
- Identity(result,1+temp1*(my_camera_parameters[4]+temp2));
+ result *= 1+temp1*(my_camera_parameters[4]+temp2);
+ //Identity(result,1+temp1*(my_camera_parameters[4]+temp2));
result[0][0] += x[1]*x[1]*temp3;
result[0][1] =-(temp3*x[0]*x[1]);
@@ -791,10 +792,11 @@
}
TooN::Matrix<2,2> Camera::Quintic::get_derivative(const TooN::Vector<2>& x)
const {
- TooN::Matrix<2,2> result;
+ TooN::Matrix<2,2> result = TooN::Identity;
double temp1=x*x;
double temp2=my_camera_parameters[5]*temp1;
- Identity(result,1+temp1*(my_camera_parameters[4]+temp2));
+ result *= 1+temp1*(my_camera_parameters[4]+temp2);
+ //Identity(result,1+temp1*(my_camera_parameters[4]+temp2));
result += (2*(my_camera_parameters[4]+2*temp2)*x.as_col()) * x.as_row();
result[0] *= my_camera_parameters[0];
result[1] *= my_camera_parameters[1];
Index: cvd/gl_helpers.h
===================================================================
RCS file: /cvsroot/libcvd/libcvd/cvd/gl_helpers.h,v
retrieving revision 1.41
retrieving revision 1.42
diff -u -b -r1.41 -r1.42
--- cvd/gl_helpers.h 18 Dec 2008 16:35:53 -0000 1.41
+++ cvd/gl_helpers.h 14 Apr 2009 12:57:53 -0000 1.42
@@ -243,7 +243,7 @@
/// transposed to account for GL's column major format.
/// @param m the transformation matrix
/// @ingroup gGL
- template <int N, class A> inline void glMultMatrix( const
TooN::FixedMatrix<N,N,A> & m )
+ template <int N, class P, class A> inline void glMultMatrix( const
TooN::Matrix<N,N,P,A> & m )
{
GLdouble glm[16];
glm[0] = m[0][0]; glm[1] = m[1][0]; glm[2] = m[2][0]; glm[3] =
m[3][0];
@@ -258,7 +258,7 @@
/// The matrix is also transposed to account for GL's column major
format.
/// @param m the transformation matrix
/// @ingroup gGL
- template <class A> inline void glMultMatrix( const
TooN::FixedMatrix<3,3,A> & m )
+ template <class P, class A> inline void glMultMatrix( const
TooN::Matrix<3,3,P,A> & m )
{
GLdouble glm[16];
glm[0] = m[0][0]; glm[1] = m[1][0]; glm[2] = m[2][0]; glm[3] =
0;
@@ -273,7 +273,7 @@
/// identity matrix. The matrix is also transposed to account for GL's
column major format.
/// @param m the transformation matrix
/// @ingroup gGL
- template <class A> inline void glMultMatrix( const
TooN::FixedMatrix<2,2,A> & m )
+ template <class P, class A> inline void glMultMatrix( const
TooN::Matrix<2,2,P,A> & m )
{
GLdouble glm[16];
glm[0] = m[0][0]; glm[1] = m[1][0]; glm[2] = 0; glm[3] = 0;
@@ -286,7 +286,8 @@
/// multiplies a SO3 onto the current matrix stack
/// @param so3 the SO3
/// @ingroup gGL
- inline void glMultMatrix( const CVD::SO3 & so3 )
+ template <typename P>
+ inline void glMultMatrix( const TooN::SO3<P> & so3 )
{
glMultMatrix( so3.get_matrix());
}
@@ -295,7 +296,8 @@
/// the SO3 and the translation in order.
/// @param se3 the SE3
/// @ingroup gGL
- inline void glMultMatrix( const CVD::SE3 & se3 )
+ template <typename P>
+ inline void glMultMatrix( const TooN::SE3<P> & se3 )
{
glTranslate( se3.get_translation());
glMultMatrix( se3.get_rotation());
@@ -304,7 +306,8 @@
/// multiplies a SO2 onto the current matrix stack
/// @param so2 the SO2
/// @ingroup gGL
- inline void glMultMatrix( const CVD::SO2 & so2 )
+ template <typename P>
+ inline void glMultMatrix( const TooN::SO2<P> & so2 )
{
glMultMatrix( so2.get_matrix());
}
@@ -313,7 +316,8 @@
/// the SO2 and the translation in order.
/// @param se3 the SE2
/// @ingroup gGL
- inline void glMultMatrix( const CVD::SE2 & se2 )
+ template <typename P>
+ inline void glMultMatrix( const TooN::SE2<P> & se2 )
{
glTranslate( se2.get_translation());
glMultMatrix( se2.get_rotation());
@@ -352,8 +356,8 @@
/// @param near near clipping plane
/// @param far far clipping plane
/// @ingroup gGL
- template <class A>
- inline void glFrustum( const TooN::FixedVector<4,A> & params, double
width, double height, double near = 0.1, double far = 100)
+ template <typename P, typename A>
+ inline void glFrustum( const TooN::Vector<4,P,A> & params, double
width, double height, double near = 0.1, double far = 100)
{
GLdouble left, right, bottom, top;
left = -near * params[2] / params[0];
Index: cvd/image_interpolate.h
===================================================================
RCS file: /cvsroot/libcvd/libcvd/cvd/image_interpolate.h,v
retrieving revision 1.18
retrieving revision 1.19
diff -u -b -r1.18 -r1.19
--- cvd/image_interpolate.h 18 Jan 2008 20:30:18 -0000 1.18
+++ cvd/image_interpolate.h 14 Apr 2009 12:57:54 -0000 1.19
@@ -130,7 +130,7 @@
TooN::Vector<2> min()
{
- return (TooN::make_Vector, 0, 0);
+ return TooN::makeVector( 0, 0);
}
TooN::Vector<2> max()
@@ -148,12 +148,12 @@
TooN::Vector<2> floor(const TooN::Vector<2>& v)
{
- return (TooN::make_Vector, ::floor(v[0]),
::floor(v[1]));
+ return TooN::makeVector( ::floor(v[0]),
::floor(v[1]));
}
TooN::Vector<2> ceil(const TooN::Vector<2>& v)
{
- return (TooN::make_Vector, ::ceil(v[0]),
::ceil(v[1]));
+ return TooN::makeVector( ::ceil(v[0]),
::ceil(v[1]));
}
typedef typename Pixel::traits<T>::float_type FT;
@@ -202,7 +202,7 @@
TooN::Vector<2> min()
{
- return (TooN::make_Vector, 0, 0);
+ return TooN::makeVector( 0, 0);
}
TooN::Vector<2> max()
@@ -265,12 +265,12 @@
TooN::Vector<2> min()
{
- return (TooN::make_Vector, 1, 1);
+ return TooN::makeVector( 1, 1);
}
TooN::Vector<2> max()
{
- return (TooN::make_Vector, im->size().x - 2,
im->size().y - 2);
+ return TooN::makeVector( im->size().x - 2,
im->size().y - 2);
}
};
Index: cvd/tensor_voting.h
===================================================================
RCS file: /cvsroot/libcvd/libcvd/cvd/tensor_voting.h,v
retrieving revision 1.6
retrieving revision 1.7
diff -u -b -r1.6 -r1.7
--- cvd/tensor_voting.h 18 Aug 2008 23:54:13 -0000 1.6
+++ cvd/tensor_voting.h 14 Apr 2009 12:57:54 -0000 1.7
@@ -84,8 +84,7 @@
using std::vector;
using TensorVoting::TV_coord;
- Matrix<2> zero;
- TooN::Zero(zero);
+ Matrix<2> zero = TooN::Zero;
Image<Matrix<2> > field(image.size(), zero);
Index: cvd_src/tensor_voting.cc
===================================================================
RCS file: /cvsroot/libcvd/libcvd/cvd_src/tensor_voting.cc,v
retrieving revision 1.2
retrieving revision 1.3
diff -u -b -r1.2 -r1.3
--- cvd_src/tensor_voting.cc 18 Apr 2008 16:51:36 -0000 1.2
+++ cvd_src/tensor_voting.cc 14 Apr 2009 12:57:55 -0000 1.3
@@ -15,7 +15,9 @@
Matrix<2> rot(double angle)
{
- double v[]= {cos(angle), sin(angle), -sin(angle),
cos(angle)};
+ Matrix<2> v;
+ v[0] = makeVector(cos(angle), sin(angle));
+ v[1] = makeVector(-sin(angle), cos(angle));
return v;
}
Index: progs/calibrate.cxx
===================================================================
RCS file: /cvsroot/libcvd/libcvd/progs/calibrate.cxx,v
retrieving revision 1.13
retrieving revision 1.14
diff -u -b -r1.13 -r1.14
--- progs/calibrate.cxx 2 Dec 2008 17:30:41 -0000 1.13
+++ progs/calibrate.cxx 14 Apr 2009 12:57:55 -0000 1.14
@@ -20,6 +20,8 @@
#include <cvd/colourspaces.h>
#include <cvd/colourspace_convert.h>
+#include <cvd/Linux/dvbuffer3.h>
+
using namespace std;
using namespace TooN;
using namespace CVD;
@@ -27,11 +29,8 @@
#include <X11/keysym.h>
#include <X11/Xlib.h>
-#ifdef CVD_HAVE_QTBUFFER
-typedef vuy422 CAMERA_PIXEL;
-#else
-typedef byte CAMERA_PIXEL;
-#endif
+//typedef byte CAMERA_PIXEL;
+typedef bayer_rggb CAMERA_PIXEL;
VideoBuffer<CAMERA_PIXEL>* videoBuffer=0;
@@ -106,39 +105,94 @@
glEnd ();
}
+template <typename B>
+Matrix<2> inverse(const Matrix<2, 2, double, B>& A)
+{
+ Matrix<2> result;
+ const double idet = 1/(A(0,0)*A(1,1)-A(0,1)*A(1,0));
+ result(0,0) = A(1,1);
+ result(1,0) = - A(0,1);
+ result(0,1) = - A(1,0);
+ result(1,1) = A(0,0);
+ result *= idet;
+ return result;
+}
+
+template <class A1, class A2, class A3> inline
+Vector<2> project_transformed_point(const SE3<> & pose, const
Vector<3,double,A1>& in_frame, Matrix<2,3,double,A2>& J_x,
Matrix<2,6,double,A3>& J_pose)
+{
+ const double z_inv = 1.0/in_frame[2];
+ const double x_z_inv = in_frame[0]*z_inv;
+ const double y_z_inv = in_frame[1]*z_inv;
+ const double cross = x_z_inv * y_z_inv;
+ J_pose[0][0] = J_pose[1][1] = z_inv;
+ J_pose[0][1] = J_pose[1][0] = 0;
+ J_pose[0][2] = -x_z_inv * z_inv;
+ J_pose[1][2] = -y_z_inv * z_inv;
+ J_pose[0][3] = -cross;
+ J_pose[0][4] = 1 + x_z_inv*x_z_inv;
+ J_pose[0][5] = -y_z_inv;
+ J_pose[1][3] = -1 - y_z_inv*y_z_inv;
+ J_pose[1][4] = cross;
+ J_pose[1][5] = x_z_inv;
+
+ const TooN::Matrix<3>& R = pose.get_rotation().get_matrix();
+ J_x[0][0] = z_inv*(R[0][0] - x_z_inv * R[2][0]);
+ J_x[0][1] = z_inv*(R[0][1] - x_z_inv * R[2][1]);
+ J_x[0][2] = z_inv*(R[0][2] - x_z_inv * R[2][2]);
+ J_x[1][0] = z_inv*(R[1][0] - y_z_inv * R[2][0]);
+ J_x[1][1] = z_inv*(R[1][1] - y_z_inv * R[2][1]);
+ J_x[1][2] = z_inv*(R[1][2] - y_z_inv * R[2][2]);
+
+ return makeVector(x_z_inv, y_z_inv);
+}
+
+
+template <class A1> inline
+Vector<2> transform_and_project(const SE3<>& pose, const Vector<3,double,A1>&
x)
+{
+ return project(pose * x);
+}
+
+template <class A1, class A2, class A3> inline
+Vector<2> transform_and_project(const SE3<>& pose, const Vector<3,double,A1>&
x, Matrix<2,3,double,A2>& J_x, Matrix<2,6,double,A3>& J_pose)
+{
+return project_transformed_point(pose, pose * x, J_x, J_pose);
+}
+
template <class CamModel, class P>
-SE3 find_pose(const SE3& start, const vector<Vector<3> >& x, const
vector<pair<size_t,P> >& p, CamModel& camModel, double noise)
+SE3<> find_pose(const SE3<>& start, const vector<Vector<3> >& x, const
vector<pair<size_t,P> >& p, CamModel& camModel, double noise)
{
vector<pair<Vector<2>, Matrix<2> > > unprojected(p.size());
for (size_t i=0; i<p.size(); ++i) {
unprojected[i].first = camModel.unproject(p[i].second);
Matrix<2> Jinv = inverse(camModel.get_derivative());
- unprojected[i].second = Jinv * Identity<2>(noise) * Jinv.T();
+ unprojected[i].second = Jinv * (noise * Jinv.T());
}
- SE3 bestSE3;
+ SE3<> bestSE3;
double bestError = numeric_limits<double>::max();
vector<Vector<2> > up(p.size());
for (size_t i=0; i<p.size(); i++)
up[i] = camModel.unproject(p[i].second);
- SE3 se3;
+ SE3<> se3;
se3 = start;
for (int iter=0; iter<4; ++iter) {
- Matrix<6> I = zeros<6,6>();
- Vector<6> b = zeros<6>();
+ Matrix<6> I = Zero;
+ Vector<6> b = Zero;
for (size_t i=0; i<p.size(); ++i) {
Matrix<2,3> J_x;
Matrix<2,6> J_pose;
Vector<2> v = unprojected[i].first - transform_and_project(se3,
x[p[i].first], J_x, J_pose);
const Matrix<2> Rinv = inverse(unprojected[i].second);
- transformCovariance<util::PlusEquals>(J_pose.T(), Rinv, I);
+ I += J_pose.T() * Rinv * J_pose;
+ //transformCovariance<util::PlusEquals>(J_pose.T(), Rinv, I);
b += J_pose.T() * (Rinv * v);
}
Cholesky<6> chol(I);
- assert(chol.get_rank() == 6);
- se3.left_multiply_by(SE3::exp(chol.inverse_times(b)));
+ se3.left_multiply_by(SE3<>::exp(chol.backsub(b)));
double residual = 0;
for (size_t i=0; i<p.size(); ++i) {
@@ -155,14 +209,16 @@
}
template <class CamModel, class P>
-SE3 find_pose_and_params(const SE3& start, const vector<Vector<3> >& x, const
vector<pair<size_t,P> >& p, CamModel& camModel, double noise)
+SE3<> find_pose_and_params(const SE3<>& start, const vector<Vector<3> >& x,
const vector<pair<size_t,P> >& p, CamModel& camModel, double noise)
{
static const int NCP = CamModel::num_parameters;
static const int NP = NCP + 6;
- const Matrix<2> Rinv = Identity<2>(1.0/noise);
- SE3 best_pose = start;
+ //const Matrix<2> Rinv = Identity<2>(1.0/noise);
+ Matrix<2> Rinv = Identity;
+ Rinv /= noise;
+ SE3<> best_pose = start;
Vector<NCP> best_params = camModel.get_parameters();
- SE3 pose = start;
+ SE3<> pose = start;
double residual = 0;
for (size_t i=0; i<p.size(); ++i) {
@@ -173,9 +229,9 @@
double lambda = 1.0;
for (int iter=0; iter<10; ++iter) {
- Matrix<NP> I;
- Identity(I, lambda);
- Vector<NP> b = zeros<NP>();
+ Matrix<NP> I = Identity;
+ I *= lambda;
+ Vector<NP> b = Zero;
for (size_t i=0; i<p.size(); ++i) {
Matrix<2,3> J_x;
Matrix<2,6> J_pose;
@@ -184,14 +240,14 @@
J.template slice<0,0,2,NCP>() = camModel.get_parameter_derivs().T();
J.template slice<0,NCP,2,6>() = camModel.get_derivative() * J_pose;
- transformCovariance<util::PlusEquals>(J.T(), Rinv, I);
+ I += J.T() * Rinv * J;
+ // transformCovariance<util::PlusEquals>(J.T(), Rinv, I);
b += J.T() * (Rinv * v);
}
Cholesky<NP> chol(I);
- assert(chol.get_rank() == NP);
- Vector<NP> delta = chol.inverse_times(b);
+ Vector<NP> delta = chol.backsub(b);
camModel.get_parameters() += delta.template slice<0,NCP>();
- pose.left_multiply_by(SE3::exp(delta.template slice<NCP,6>()));
+ pose.left_multiply_by(SE3<>::exp(delta.template slice<NCP,6>()));
double residual = 0;
for (size_t i=0; i<p.size(); ++i) {
@@ -253,7 +309,8 @@
}
try {
cerr << "opening " << videoDevice << endl;
- videoBuffer = open_video_source<CAMERA_PIXEL>(videoDevice);
+ //videoBuffer = open_video_source<CAMERA_PIXEL>(videoDevice);
+ videoBuffer = new DVBuffer3<bayer_rggb>(0, ImageRef(720,480), 60);
}
catch (CVD::Exceptions::All& e) {
cerr << e.what << endl;
@@ -264,20 +321,20 @@
vector<Vector<2> > makeGrid(int gx, int gy, double cellSize)
{
vector<Vector<2> > grid;
- Vector<2> center = (make_Vector, gx, gy);
+ Vector<2> center = makeVector(gx,gy);
center *= cellSize/2.0;
for (int y=0; y<=gy; y++)
{
for (int x=0; x<=gx; x++)
{
- grid.push_back(Vector<2>((make_Vector,x,y))*cellSize - center);
+ grid.push_back(Vector<2>(makeVector(x,y))*cellSize - center);
}
}
return grid;
}
template <class CM>
-void drawPoints(const vector<Vector<2> >& points, const SE3& pose, CM& cm)
+void drawPoints(const vector<Vector<2> >& points, const SE3<>& pose, CM& cm)
{
glColor3f(0,1,0);
for (size_t i=0; i<points.size(); i++)
@@ -296,7 +353,7 @@
};
template <class CM>
-double getReprojectionError(const vector<MeasurementSet>& ms, const
vector<SE3>& poses, CM& cm)
+double getReprojectionError(const vector<MeasurementSet>& ms, const
vector<SE3<> >& poses, CM& cm)
{
double error = 0;
for (size_t i=0; i<ms.size(); i++)
@@ -313,7 +370,7 @@
}
template <class CM>
-pair<double, double> getReprojectionError(const vector<MeasurementSet>& ms,
const vector<SE3>& poses, CM& cm,
+pair<double, double> getReprojectionError(const vector<MeasurementSet>& ms,
const vector<SE3<> >& poses, CM& cm,
vector<pair<Vector<2>,Vector<2> > >&
v)
{
double error = 0, maxError = 0;
@@ -351,24 +408,21 @@
}
template <class CM>
-void improveLM(vector<MeasurementSet>& ms, vector<SE3>& pose, CM& cm, double
lambda)
+void improveLM(vector<MeasurementSet>& ms, vector<SE3<> >& pose, CM& cm,
double lambda)
{
Matrix<>
JTJ(CM::num_parameters+ms.size()*6,CM::num_parameters+ms.size()*6);
Vector<> JTe(JTJ.num_rows());
- Zero(JTJ);
- Vector<CM::num_parameters> JTep = zeros<CM::num_parameters>();
+ JTJ= Zero;
+ Vector<CM::num_parameters> JTep = Zero;
for (size_t i=0; i<ms.size(); i++)
{
- Matrix<6> poseBlock;
- Matrix<CM::num_parameters> paramBlock;
- Matrix<CM::num_parameters, 6> offDiag;
- Zero(poseBlock);
- Zero(paramBlock);
- Zero(offDiag);
+ Matrix<6> poseBlock = Zero;
+ Matrix<CM::num_parameters> paramBlock = Zero;
+ Matrix<CM::num_parameters, 6> offDiag = Zero;
- Vector<6> JTei = zeros<6>();
+ Vector<6> JTei = Zero;
for (size_t j=0; j<ms[i].im.size(); j++)
{
@@ -391,7 +445,7 @@
JTJ.slice(CM::num_parameters + i*6, CM::num_parameters + i*6, 6,6) =
poseBlock;
JTJ.slice(0,0,CM::num_parameters, CM::num_parameters) += paramBlock;
JTJ.slice(0,CM::num_parameters + i*6, CM::num_parameters, 6) = offDiag;
- //JTJ.slice(CM::num_parameters + i*6,0, 6, CM::num_parameters) =
offDiag.T();
+ JTJ.slice(CM::num_parameters + i*6,0, 6, CM::num_parameters) =
offDiag.T();
}
JTe.slice(0,CM::num_parameters) = JTep;
@@ -401,23 +455,20 @@
cm.get_parameters() += delta.template slice<0,CM::num_parameters>();
for (size_t i=0; i<pose.size(); i++)
{
- pose[i].left_multiply_by(SE3::exp(delta.slice(CM::num_parameters+i*6,
6)));
+ pose[i].left_multiply_by(SE3<>::exp(delta.slice(CM::num_parameters+i*6,
6)));
}
}
template <class CM>
-void getUncertainty(const vector<MeasurementSet>& ms, const vector<SE3>& pose,
CM& cm, Matrix<CM::num_parameters>& C)
+void getUncertainty(const vector<MeasurementSet>& ms, const vector<SE3<> >&
pose, CM& cm, Matrix<CM::num_parameters>& C)
{
Matrix<>
JTJ(CM::num_parameters+ms.size()*6,CM::num_parameters+ms.size()*6);
- Zero(JTJ);
+ JTJ = Zero;
for (size_t i=0; i<ms.size(); i++)
{
- Matrix<6> poseBlock;
- Matrix<CM::num_parameters> paramBlock;
- Matrix<CM::num_parameters, 6> offDiag;
- Zero(poseBlock);
- Zero(paramBlock);
- Zero(offDiag);
+ Matrix<6> poseBlock = Zero;
+ Matrix<CM::num_parameters> paramBlock = Zero;
+ Matrix<CM::num_parameters, 6> offDiag = Zero;
for (size_t j=0; j<ms[i].im.size(); j++)
{
Matrix<2,3> J_x;
@@ -437,17 +488,17 @@
}
Cholesky<> chol(JTJ);
Vector<> v(JTJ.num_cols());
- Zero(v);
+ v = Zero;
for (int i=0; i<CM::num_parameters; ++i) {
v[i]=1;
- Vector<> Cv = chol.inverse_times(v);
+ Vector<> Cv = chol.backsub(v);
v[i]=0;
C.T()[i] = Cv.template slice<0,CM::num_parameters>();
}
}
-inline Vector<2> imagePoint(const Vector<2>& inPoint, const SE3& pose,
CameraModel& cm, const double& factor)
+inline Vector<2> imagePoint(const Vector<2>& inPoint, const SE3<> & pose,
CameraModel& cm, const double& factor)
{
Vector<3> point3D = unproject(inPoint);
Vector<2> plane = project(pose*point3D);
@@ -456,7 +507,7 @@
return im;
}
-inline float imageVal(image_interpolate<Interpolate::Bilinear, float>
&imgInter, const Vector<2>& inPoint, const SE3& pose,
+inline float imageVal(image_interpolate<Interpolate::Bilinear, float>
&imgInter, const Vector<2>& inPoint, const SE3<> & pose,
CameraModel& cm, const double& factor, const bool&
boundsCheck)
{
Vector<2> imageVec = imagePoint(inPoint, pose, cm, factor);
@@ -498,11 +549,11 @@
std::min(maxCoord[0] - imagePoint[0], maxCoord[1]
- imagePoint[1])));
}
-inline float minMarginSquare(const Vector<2>& inPoint,
image_interpolate<Interpolate::Bilinear, float>& imgInter, const SE3& pose,
+inline float minMarginSquare(const Vector<2>& inPoint,
image_interpolate<Interpolate::Bilinear, float>& imgInter, const SE3<> & pose,
CameraModel& cm, double factor, float cellSize)
{
- Vector<2> posVec = (make_Vector, cellSize/2, cellSize/2);
- Vector<2> negVec = (make_Vector, cellSize/2, -cellSize/2);
+ Vector<2> posVec = makeVector( cellSize/2, cellSize/2);
+ Vector<2> negVec = makeVector( cellSize/2, -cellSize/2);
Vector<2> minCoord = imgInter.min();
Vector<2> maxCoord = imgInter.max();
@@ -516,11 +567,11 @@
return minVal;
}
-bool sanityCheck(const Vector<2>& inPoint,
image_interpolate<Interpolate::Bilinear, float>& imgInter, const SE3& pose,
+bool sanityCheck(const Vector<2>& inPoint,
image_interpolate<Interpolate::Bilinear, float>& imgInter, const SE3<> & pose,
CameraModel& cm, double factor, bool blWhite, float cellSize)
{
- Vector<2> posVec = (make_Vector, cellSize/2, cellSize/2);
- Vector<2> negVec = (make_Vector, cellSize/2, -cellSize/2);
+ Vector<2> posVec = makeVector( cellSize/2, cellSize/2);
+ Vector<2> negVec = makeVector( cellSize/2, -cellSize/2);
// Don't need to bounds check these as it's done after the minMarginSquare
> 0 check
float midVal = imageVal(imgInter, inPoint, pose, cm, factor, false);
@@ -556,13 +607,13 @@
}
bool findInitialIntersectionEstimate(image_interpolate<Interpolate::Bilinear,
float> &imgInter, Vector<2>& initialPoint,
- const SE3& pose, CameraModel& cm, double
factor,
+ const SE3<> & pose, CameraModel& cm,
double factor,
bool boundsCheck, const Vector<4>&
likelySquare, double cellSize)
{
bool looksOK = true;
//very roughly find a sensible starting place
- Vector<2> testPoint = (make_Vector, likelySquare[0], likelySquare[1]);
+ Vector<2> testPoint = makeVector( likelySquare[0], likelySquare[1]);
float startPx = imageVal(imgInter, testPoint, pose, cm, factor,
boundsCheck);
while(looksOK && fabs(imageVal(imgInter, testPoint, pose, cm, factor,
boundsCheck) - startPx) < 0.1)
{
@@ -587,13 +638,13 @@
}
bool optimiseIntersection(image_interpolate<Interpolate::Bilinear, float>
&imgInter, Vector<2>& inPoint,
- const SE3& pose, CameraModel& cm, double factor,
bool boundsCheck,
+ const SE3<> & pose, CameraModel& cm, double factor,
bool boundsCheck,
const Vector<4>& likelySquare, double cellSize, bool
blWhite)
{
- Vector<2> largeX = (make_Vector, 0.004, 0);
- Vector<2> largeY = (make_Vector, 0, 0.004);
- Vector<2> smallX = (make_Vector, cellSize/10, 0);
- Vector<2> smallY = (make_Vector, 0, cellSize/10);
+ Vector<2> largeX = makeVector( 0.004, 0);
+ Vector<2> largeY = makeVector( 0, 0.004);
+ Vector<2> smallX = makeVector( cellSize/10, 0);
+ Vector<2> smallY = makeVector( 0, cellSize/10);
float aboveVal = imageVal(imgInter, inPoint + largeY, pose, cm, factor,
boundsCheck);
float belowVal = imageVal(imgInter, inPoint - largeY, pose, cm, factor,
boundsCheck);
@@ -665,12 +716,12 @@
CameraModel cameraModel;
double factor=1.0;
vector<MeasurementSet> measurementSets;
- vector<SE3> poses;
+ vector<SE3<> > poses;
ImageRef imageSize;
if(cameraParameters[0] == -1)
{
- Zero(cameraParameters);
+ cameraParameters = Zero;
cameraParameters[0] = 500;
cameraParameters[1] = 500;
cameraParameters[2] = videoBuffer->size().x/2;
@@ -698,9 +749,9 @@
for (size_t i=0; i<grid.size(); i++)
grid3d[i] = unproject(grid[i]);
- SE3 pose;
+ SE3<> pose;
pose.get_translation()[2] = -0.7;
- const SE3 init_pose = pose;
+ const SE3<> init_pose = pose;
glPixelStorei(GL_UNPACK_ALIGNMENT,1);
glDrawBuffer(GL_BACK);
@@ -752,7 +803,7 @@
}
- videoBuffer->flush();
+ //videoBuffer->flush();
VideoFrame<CAMERA_PIXEL>* vframe = videoBuffer->get_frame();
// leave this in, we cannot assume that vframe has a datatype that can
be
@@ -792,7 +843,7 @@
int guessCount = 0;
int totalPass = 0, totalFail = 0;
- SE3 guessPose = pose;
+ SE3<> guessPose = pose;
bool correctPose = false;
do
@@ -824,8 +875,8 @@
if (bottomLeftWhite)
blWhite = !blWhite;
- Vector<2> inPoint = (make_Vector, xNo*cellSize -
gridx*cellSize/2, yNo*cellSize - gridy*cellSize/2);
- Vector<4> likelySquare = (make_Vector, inPoint[0] - cellSize/2,
inPoint[1] + cellSize/2,
+ Vector<2> inPoint = makeVector( xNo*cellSize -
gridx*cellSize/2, yNo*cellSize - gridy*cellSize/2);
+ Vector<4> likelySquare = makeVector( inPoint[0] - cellSize/2,
inPoint[1] + cellSize/2,
inPoint[0] + cellSize/2, inPoint[1] -
cellSize/2); //l,t,r,b
float minMarginDist = minMarginSquare(inPoint, imgInter,
guessPose, cameraModel, factor, cellSize);
@@ -932,10 +983,10 @@
{
//Didn't track correctly - let's guess! - just change the pose
a bit
guessCount++;
- guessPose = CVD::SE3::exp((make_Vector, rand_g()/300,
rand_g()/300, rand_g()/40, rand_g()/10, rand_g()/10, rand_g()/10));
+ guessPose = SE3<>::exp(makeVector( rand_g()/300, rand_g()/300,
rand_g()/40, rand_g()/10, rand_g()/10, rand_g()/10));
//Apply the change in grid coordinates
//Grid coordinates actually centred on (0,0,1) in 3D, so need
to shift before and after
- guessPose = SE3::exp((make_Vector, 0, 0, 1, 0, 0, 0)) *
guessPose * SE3::exp((make_Vector, 0, 0, -1, 0, 0, 0));
+ guessPose = SE3<>::exp(makeVector( 0, 0, 1, 0, 0, 0)) *
guessPose * SE3<>::exp(makeVector( 0, 0, -1, 0, 0, 0));
guessPose = pose * guessPose;
}
@@ -1047,7 +1098,7 @@
while (lambda < 1e12)
{
Vector<NumCameraParams> params = cameraModel.get_parameters();
- vector<SE3> oldposes = poses;
+ vector<SE3<> > oldposes = poses;
improveLM(measurementSets, poses, cameraModel, lambda);
double error = getReprojectionError(measurementSets, poses,
cameraModel);
if (minError - error > 1e-19)
Index: progs/se3_exp.cxx
===================================================================
RCS file: /cvsroot/libcvd/libcvd/progs/se3_exp.cxx,v
retrieving revision 1.5
retrieving revision 1.6
diff -u -b -r1.5 -r1.6
--- progs/se3_exp.cxx 27 May 2005 14:52:48 -0000 1.5
+++ progs/se3_exp.cxx 14 Apr 2009 12:57:56 -0000 1.6
@@ -27,11 +27,11 @@
int main()
{
- SE3 se3;
+ SE3<> se3;
Vector<6> v;
cin >> v;
- se3 = SE3::exp(v);
+ se3 = SE3<>::exp(v);
Matrix<3,4> m;
m.slice<0,0,3,3>() = se3.get_rotation().get_matrix();
Index: progs/se3_inv.cxx
===================================================================
RCS file: /cvsroot/libcvd/libcvd/progs/se3_inv.cxx,v
retrieving revision 1.4
retrieving revision 1.5
diff -u -b -r1.4 -r1.5
--- progs/se3_inv.cxx 27 May 2005 14:46:58 -0000 1.4
+++ progs/se3_inv.cxx 14 Apr 2009 12:57:56 -0000 1.5
@@ -27,11 +27,11 @@
int main()
{
- SE3 se3;
+ SE3<> se3;
Vector<6> v;
cin >> v;
- se3 = SE3::exp(v);
+ se3 = SE3<>::exp(v);
cout << (se3.inverse()).ln() << endl;
return 0;
Index: progs/se3_ln.cxx
===================================================================
RCS file: /cvsroot/libcvd/libcvd/progs/se3_ln.cxx,v
retrieving revision 1.3
retrieving revision 1.4
diff -u -b -r1.3 -r1.4
--- progs/se3_ln.cxx 9 May 2005 11:55:00 -0000 1.3
+++ progs/se3_ln.cxx 14 Apr 2009 12:57:57 -0000 1.4
@@ -26,7 +26,7 @@
int main()
{
- SE3 se3;
+ SE3<> se3;
cin >> se3;
cout << se3.ln() << endl;
Index: progs/se3_post_mul.cxx
===================================================================
RCS file: /cvsroot/libcvd/libcvd/progs/se3_post_mul.cxx,v
retrieving revision 1.4
retrieving revision 1.5
diff -u -b -r1.4 -r1.5
--- progs/se3_post_mul.cxx 27 May 2005 14:46:58 -0000 1.4
+++ progs/se3_post_mul.cxx 14 Apr 2009 12:57:57 -0000 1.5
@@ -45,9 +45,9 @@
Vector<6> in;
- SE3 s3, c;
+ SE3<> s3, c;
- c = SE3::exp(camera);
+ c = SE3<>::exp(camera);
for(;;)
@@ -57,7 +57,7 @@
if(!cin.good())
return 0;
- s3=SE3::exp(in) * c;
+ s3=SE3<>::exp(in) * c;
cout << s3.ln() << endl;
}
Index: progs/se3_pre_mul.cxx
===================================================================
RCS file: /cvsroot/libcvd/libcvd/progs/se3_pre_mul.cxx,v
retrieving revision 1.4
retrieving revision 1.5
diff -u -b -r1.4 -r1.5
--- progs/se3_pre_mul.cxx 27 May 2005 14:46:58 -0000 1.4
+++ progs/se3_pre_mul.cxx 14 Apr 2009 12:57:57 -0000 1.5
@@ -45,9 +45,9 @@
Vector<6> in;
- SE3 s3, c;
+ SE3<> s3, c;
- c = SE3::exp(camera);
+ c = SE3<>::exp(camera);
for(;;)
{
@@ -56,7 +56,7 @@
if(!cin.good())
return 0;
- s3=c * SE3::exp(in);
+ s3=c * SE3<>::exp(in);
cout << s3.ln() << endl;
}
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [libcvd-members] libcvd cvd/camera.h cvd/gl_helpers.h cvd/image_...,
Gerhard Reitmayr <=