Skip to content

O
opencv
Commit 5e4ca227 authored by Xavier Delacour's avatar Xavier Delacour
Browse files
Options

small updates to bundle adjustment implementation

parent ad4969d8
Show whitespace changes
Inline Side-by-side
Showing with 303 additions and 291 deletions
modules/contrib/include/opencv2/contrib/contrib.hpp
View file @ 5e4ca227
......@@ -431,8 +431,10 @@ namespace cv
DEFAULT_NUM_DISTANCE_BUCKETS = 7 };
};
class CV_EXPORTS LevMarqSparse
{
typedef bool (*BundleAdjustCallback)(int iteration, double norm_error, void* user_data);
class LevMarqSparse {
public:
LevMarqSparse();
LevMarqSparse(int npoints, // number of points
......@@ -453,8 +455,10 @@ namespace cv
// callback for estimation of backprojection errors
void (CV_CDECL * func)(int i, int j, Mat& point_params,
Mat& cam_params, Mat& estim, void* data),
void* data // user-specific data passed to the callbacks
void* data, // user-specific data passed to the callbacks
BundleAdjustCallback cb, void* user_data
);
virtual ~LevMarqSparse();
virtual void run( int npoints, // number of points
......@@ -480,24 +484,25 @@ namespace cv
virtual void clear();
// useful function to do simple bundle adjastment tasks
static void bundleAdjust(vector<Point3d>& points, //positions of points in global coordinate system (input and output)
const vector<vector<Point2d> >& imagePoints, //projections of 3d points for every camera
const vector<vector<int> >& visibility, //visibility of 3d points for every camera
vector<Mat>& cameraMatrix, //intrinsic matrices of all cameras (input and output)
vector<Mat>& R, //rotation matrices of all cameras (input and output)
vector<Mat>& T, //translation vector of all cameras (input and output)
vector<Mat>& distCoeffs, //distortion coefficients of all cameras (input and output)
// useful function to do simple bundle adjustment tasks
static void bundleAdjust(vector<Point3d>& points, // positions of points in global coordinate system (input and output)
const vector<vector<Point2d> >& imagePoints, // projections of 3d points for every camera
const vector<vector<int> >& visibility, // visibility of 3d points for every camera
vector<Mat>& cameraMatrix, // intrinsic matrices of all cameras (input and output)
vector<Mat>& R, // rotation matrices of all cameras (input and output)
vector<Mat>& T, // translation vector of all cameras (input and output)
vector<Mat>& distCoeffs, // distortion coefficients of all cameras (input and output)
const TermCriteria& criteria=
TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, DBL_EPSILON));
TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, DBL_EPSILON),
BundleAdjustCallback cb = 0, void* user_data = 0);
protected:
virtual void optimize(); //main function that runs minimization
public:
virtual void optimize(CvMat &_vis); //main function that runs minimization
//iteratively asks for measurement for visible camera-point pairs
void ask_for_proj();
void ask_for_proj(CvMat &_vis,bool once=false);
//iteratively asks for Jacobians for every camera_point pair
void ask_for_projac();
void ask_for_projac(CvMat &_vis);
CvMat* err; //error X-hX
double prevErrNorm, errNorm;
......@@ -509,9 +514,9 @@ namespace cv
CvMat** V; //size of array is equal to number of points
CvMat** inv_V_star; //inverse of V*
CvMat* A;
CvMat* B;
CvMat* W;
CvMat** A;
CvMat** B;
CvMat** W;
CvMat* X; //measurement
CvMat* hX; //current measurement extimation given new parameter vector
......@@ -543,11 +548,13 @@ namespace cv
//target function and jacobian pointers, which needs to be initialized
void (*fjac)(int i, int j, Mat& point_params, Mat& cam_params, Mat& A, Mat& B, void* data);
void (*func)(int i, int j, Mat& point_params, Mat& cam_params, Mat& estim, void* data );
void (*func)(int i, int j, Mat& point_params, Mat& cam_params, Mat& estim, void* data);
void* data;
};
BundleAdjustCallback cb;
void* user_data;
};
CV_EXPORTS int chamerMatching( Mat& img, Mat& templ,
vector<vector<Point> >& results, vector<float>& cost,
......
modules/contrib/src/ba.cpp
View file @ 5e4ca227
......@@ -41,19 +41,19 @@
#include "precomp.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include <iostream>
namespace cv {
using namespace cv;
LevMarqSparse::LevMarqSparse()
{
A = B = W = Vis_index = X = prevP = P = deltaP = err = JtJ_diag = S = hX = NULL;
LevMarqSparse::LevMarqSparse() {
Vis_index = X = prevP = P = deltaP = err = JtJ_diag = S = hX = NULL;
U = ea = V = inv_V_star = eb = Yj = NULL;
num_points = 0;
num_cams = 0;
num_cams = 0, num_points = 0, num_err_param = 0;
num_cam_param = 0, num_point_param = 0;
A = B = W = NULL;
}
LevMarqSparse::~LevMarqSparse()
{
LevMarqSparse::~LevMarqSparse() {
clear();
}
......@@ -75,69 +75,68 @@ LevMarqSparse::LevMarqSparse(int npoints, // number of points
// callback for estimation of backprojection errors
void (CV_CDECL * func)(int i, int j, Mat& point_params,
Mat& cam_params, Mat& estim, void* data),
void* data // user-specific data passed to the callbacks
)
{
A = B = W = Vis_index = X = prevP = P = deltaP = err = JtJ_diag = S = hX = NULL;
void* data, // user-specific data passed to the callbacks
BundleAdjustCallback _cb, void* _user_data
) {
Vis_index = X = prevP = P = deltaP = err = JtJ_diag = S = hX = NULL;
U = ea = V = inv_V_star = eb = Yj = NULL;
A = B = W = NULL;
cb = _cb;
user_data = _user_data;
run(npoints, ncameras, nPointParams, nCameraParams, nErrParams, visibility,
P0, X_, criteria, fjac, func, data);
}
void LevMarqSparse::clear()
{
for( int i = 0; i < num_points; i++ )
{
for(int j = 0; j < num_cams; j++ )
{
CvMat* tmp = ((CvMat**)(A->data.ptr + i * A->step))[j];
if( tmp )
void LevMarqSparse::clear() {
for( int i = 0; i < num_points; i++ ) {
for(int j = 0; j < num_cams; j++ ) {
//CvMat* tmp = ((CvMat**)(A->data.ptr + i * A->step))[j];
CvMat* tmp = A[j+i*num_cams];
if (tmp)
cvReleaseMat( &tmp );
tmp = ((CvMat**)(B->data.ptr + i * B->step))[j];
if( tmp )
//tmp = ((CvMat**)(B->data.ptr + i * B->step))[j];
tmp = B[j+i*num_cams];
if (tmp)
cvReleaseMat( &tmp );
tmp = ((CvMat**)(W->data.ptr + j * W->step))[i];
if( tmp )
//tmp = ((CvMat**)(W->data.ptr + j * W->step))[i];
tmp = W[j+i*num_cams];
if (tmp)
cvReleaseMat( &tmp );
}
}
cvReleaseMat( &A );
cvReleaseMat( &B );
cvReleaseMat( &W );
delete A; //cvReleaseMat(&A);
delete B;//cvReleaseMat(&B);
delete W;//cvReleaseMat(&W);
cvReleaseMat( &Vis_index);
for( int j = 0; j < num_cams; j++ )
{
for( int j = 0; j < num_cams; j++ ) {
cvReleaseMat( &U[j] );
}
delete U;
for( int j = 0; j < num_cams; j++ )
{
for( int j = 0; j < num_cams; j++ ) {
cvReleaseMat( &ea[j] );
}
delete ea;
//allocate V and inv_V_star
for( int i = 0; i < num_points; i++ )
{
for( int i = 0; i < num_points; i++ ) {
cvReleaseMat(&V[i]);
cvReleaseMat(&inv_V_star[i]);
}
delete V;
delete inv_V_star;
for( int i = 0; i < num_points; i++ )
{
for( int i = 0; i < num_points; i++ ) {
cvReleaseMat(&eb[i]);
}
delete eb;
for( int i = 0; i < num_points; i++ )
{
for( int i = 0; i < num_points; i++ ) {
cvReleaseMat(&Yj[i]);
}
delete Yj;
......@@ -178,9 +177,8 @@ void LevMarqSparse::run( int num_points_, //number of points
void (*fjac_)(int i, int j, Mat& point_params, Mat& cam_params, Mat& A, Mat& B, void* data),
void (*func_)(int i, int j, Mat& point_params, Mat& cam_params, Mat& estim, void* data),
void* data_
) //termination criteria
{
clear();
) { //termination criteria
//clear();
func = func_; //assign evaluation function
fjac = fjac_; //assign jacobian
......@@ -211,80 +209,94 @@ void LevMarqSparse::run( int num_points_, //number of points
//Allocate matrix A whose elements are nointers to Aij
//if Aij is zero (point i is not visible in camera j) then A(i,j) contains NULL
A = cvCreateMat( num_points, num_cams, CV_32S /*pointer is stored here*/ );
B = cvCreateMat( num_points, num_cams, CV_32S /*pointer is stored here*/ );
W = cvCreateMat( num_cams, num_points, CV_32S /*pointer is stored here*/ );
//A = cvCreateMat( num_points, num_cams, CV_32S /*pointer is stored here*/ );
//B = cvCreateMat( num_points, num_cams, CV_32S /*pointer is stored here*/ );
//W = cvCreateMat( num_cams, num_points, CV_32S /*pointer is stored here*/ );
A = new CvMat* [num_points * num_cams];
B = new CvMat* [num_points * num_cams];
W = new CvMat* [num_cams * num_points];
Vis_index = cvCreateMat( num_points, num_cams, CV_32S /*integer index is stored here*/ );
cvSetZero( A );
cvSetZero( B );
cvSetZero( W );
//cvSetZero( A );
//cvSetZero( B );
//cvSetZero( W );
cvSet( Vis_index, cvScalar(-1) );
//fill matrices A and B based on visibility
CvMat _vis = visibility;
int index = 0;
for( int i = 0; i < num_points; i++ )
{
for(int j = 0; j < num_cams; j++ )
{
if( ((int*)(_vis.data.ptr+ i * _vis.step))[j] )
{
for (int i = 0; i < num_points; i++ ) {
for (int j = 0; j < num_cams; j++ ) {
if (((int*)(_vis.data.ptr+ i * _vis.step))[j] ) {
((int*)(Vis_index->data.ptr + i * Vis_index->step))[j] = index;
index += num_err_param;
//create matrices Aij, Bij
CvMat* tmp = cvCreateMat( Aij_height, Aij_width, CV_64F );
((CvMat**)(A->data.ptr + i * A->step))[j] = tmp;
CvMat* tmp = cvCreateMat(Aij_height, Aij_width, CV_64F );
//((CvMat**)(A->data.ptr + i * A->step))[j] = tmp;
cvSet(tmp,cvScalar(1.0,1.0,1.0,1.0));
A[j+i*num_cams] = tmp;
tmp = cvCreateMat( Bij_height, Bij_width, CV_64F );
((CvMat**)(B->data.ptr + i * B->step))[j] = tmp;
//((CvMat**)(B->data.ptr + i * B->step))[j] = tmp;
cvSet(tmp,cvScalar(1.0,1.0,1.0,1.0));
B[j+i*num_cams] = tmp;
tmp = cvCreateMat( Wij_height, Wij_width, CV_64F );
((CvMat**)(W->data.ptr + j * W->step))[i] = tmp; //note indices i and j swapped
//((CvMat**)(W->data.ptr + j * W->step))[i] = tmp; //note indices i and j swapped
cvSet(tmp,cvScalar(1.0,1.0,1.0,1.0));
W[j+i*num_cams] = tmp;
} else{
A[j+i*num_cams] = NULL;
B[j+i*num_cams] = NULL;
W[j+i*num_cams] = NULL;
}
}
}
//allocate U
U = new CvMat* [num_cams];
for( int j = 0; j < num_cams; j++ )
{
for (int j = 0; j < num_cams; j++ ) {
U[j] = cvCreateMat( U_size, U_size, CV_64F );
cvSetZero(U[j]);
}
//allocate ea
ea = new CvMat* [num_cams];
for( int j = 0; j < num_cams; j++ )
{
for (int j = 0; j < num_cams; j++ ) {
ea[j] = cvCreateMat( U_size, 1, CV_64F );
cvSetZero(ea[j]);
}
//allocate V and inv_V_star
V = new CvMat* [num_points];
inv_V_star = new CvMat* [num_points];
for( int i = 0; i < num_points; i++ )
{
for (int i = 0; i < num_points; i++ ) {
V[i] = cvCreateMat( V_size, V_size, CV_64F );
inv_V_star[i] = cvCreateMat( V_size, V_size, CV_64F );
cvSetZero(V[i]);
cvSetZero(inv_V_star[i]);
}
//allocate eb
eb = new CvMat* [num_points];
for( int i = 0; i < num_points; i++ )
{
for (int i = 0; i < num_points; i++ ) {
eb[i] = cvCreateMat( V_size, 1, CV_64F );
cvSetZero(eb[i]);
}
//allocate Yj
Yj = new CvMat* [num_points];
for( int i = 0; i < num_points; i++ )
{
for (int i = 0; i < num_points; i++ ) {
Yj[i] = cvCreateMat( Wij_height, Wij_width, CV_64F ); //Yij has the same size as Wij
cvSetZero(Yj[i]);
}
//allocate matrix S
S = cvCreateMat( num_cams * num_cam_param, num_cams * num_cam_param, CV_64F);
cvSetZero(S);
JtJ_diag = cvCreateMat( num_cams * num_cam_param + num_points * num_point_param, 1, CV_64F );
cvSetZero(JtJ_diag);
//set starting parameters
CvMat _tmp_ = CvMat(P0);
......@@ -297,72 +309,90 @@ void LevMarqSparse::run( int num_points_, //number of points
X = cvCloneMat( &_tmp_ );
//create vector for estimated measurements
hX = cvCreateMat( X->rows, X->cols, CV_64F );
cvSetZero(hX);
//create error vector
err = cvCreateMat( X->rows, X->cols, CV_64F );
ask_for_proj();
cvSetZero(err);
ask_for_proj(_vis);
//compute initial error
cvSub( X, hX, err );
cvSub(X, hX, err );
/*
assert(X->rows == hX->rows);
std::cerr<<"X size = "<<X->rows<<" "<<X->cols<<std::endl;
std::cerr<<"hX size = "<<hX->rows<<" "<<hX->cols<<std::endl;
for (int j=0;j<X->rows;j+=2) {
double Xj1 = *(double*)(X->data.ptr + j * X->step);
double hXj1 = *(double*)(hX->data.ptr + j * hX->step);
double err1 = *(double*)(err->data.ptr + j * err->step);
double Xj2 = *(double*)(X->data.ptr + (j+1) * X->step);
double hXj2 = *(double*)(hX->data.ptr + (j+1) * hX->step);
double err2 = *(double*)(err->data.ptr + (j+1) * err->step);
std::cerr<<"("<<Xj1<<","<<Xj2<<") -> ("<<hXj1<<","<<hXj2<<"). err = ("<<err1<<","<<err2<<")"<<std::endl;
}
*/
prevErrNorm = cvNorm( err, 0, CV_L2 );
// std::cerr<<"prevErrNorm = "<<prevErrNorm<<std::endl;
iters = 0;
criteria = criteria_init;
optimize();
optimize(_vis);
ask_for_proj(_vis,true);
cvSub(X, hX, err );
errNorm = cvNorm( err, 0, CV_L2 );
}
void LevMarqSparse::ask_for_proj()
{
void LevMarqSparse::ask_for_proj(CvMat &_vis,bool once) {
//given parameter P, compute measurement hX
int ind = 0;
for( int i = 0; i < num_points; i++ )
{
for (int i = 0; i < num_points; i++ ) {
CvMat point_mat;
cvGetSubRect( P, &point_mat, cvRect( 0, num_cams * num_cam_param + num_point_param * i, 1, num_point_param ));
for( int j = 0; j < num_cams; j++ )
{
CvMat* Aij = ((CvMat**)(A->data.ptr + A->step * i))[j];
if( Aij ) //visible
{
for (int j = 0; j < num_cams; j++ ) {
//CvMat* Aij = ((CvMat**)(A->data.ptr + A->step * i))[j];
CvMat* Aij = A[j+i*num_cams];
if (Aij ) { //visible
CvMat cam_mat;
cvGetSubRect( P, &cam_mat, cvRect( 0, j * num_cam_param, 1, num_cam_param ));
CvMat measur_mat;
cvGetSubRect( hX, &measur_mat, cvRect( 0, ind * num_err_param, 1, num_err_param ));
Mat _point_mat(&point_mat), _cam_mat(&cam_mat), _measur_mat(&measur_mat);
func( i, j, _point_mat, _cam_mat, _measur_mat, data );
func( i, j, _point_mat, _cam_mat, _measur_mat, data);
assert( ind*num_err_param == ((int*)(Vis_index->data.ptr + i * Vis_index->step))[j]);
ind+=1;
}
}
}
}
//iteratively asks for Jacobians for every camera_point pair
void LevMarqSparse::ask_for_projac() //should be evaluated at point prevP
{
void LevMarqSparse::ask_for_projac(CvMat &_vis) { //should be evaluated at point prevP
// compute jacobians Aij and Bij
for( int i = 0; i < A->height; i++ )
{
for (int i = 0; i < num_points; i++ ) {
CvMat point_mat;
cvGetSubRect( prevP, &point_mat, cvRect( 0, num_cams * num_cam_param + num_point_param * i, 1, num_point_param ));
//CvMat** A_line = (CvMat**)(A->data.ptr + A->step * i);
//CvMat** B_line = (CvMat**)(B->data.ptr + B->step * i);
for( int j = 0; j < num_cams; j++ ) {
//CvMat* Aij = A_line[j];
//if( Aij ) //Aij is not zero
CvMat* Aij = A[j+i*num_cams];
CvMat* Bij = B[j+i*num_cams];
if(Aij) {
//CvMat** A_line = (CvMat**)(A->data.ptr + A->step * i);
//CvMat** B_line = (CvMat**)(B->data.ptr + B->step * i);
CvMat** A_line = (CvMat**)(A->data.ptr + A->step * i);
CvMat** B_line = (CvMat**)(B->data.ptr + B->step * i);
//CvMat* Aij = A_line[j];
//CvMat* Aij = ((CvMat**)(A->data.ptr + A->step * i))[j];
for( int j = 0; j < A->width; j++ )
{
CvMat* Aij = A_line[j];
if( Aij ) //Aij is not zero
{
CvMat cam_mat;
cvGetSubRect( prevP, &cam_mat, cvRect( 0, j * num_cam_param, 1, num_cam_param ));
CvMat* Bij = B_line[j];
//CvMat* Bij = B_line[j];
//CvMat* Bij = ((CvMat**)(B->data.ptr + B->step * i))[j];
Mat _point_mat(&point_mat), _cam_mat(&cam_mat), _Aij(Aij), _Bij(Bij);
(*fjac)(i, j, _point_mat, _cam_mat, _Aij, _Bij, data);
}
......@@ -370,58 +400,60 @@ void LevMarqSparse::ask_for_projac() //should be evaluated at point prevP
}
}
void LevMarqSparse::optimize() //main function that runs minimization
{
void LevMarqSparse::optimize(CvMat &_vis) { //main function that runs minimization
bool done = false;
CvMat* YWt = cvCreateMat( num_cam_param, num_cam_param, CV_64F ); //this matrix used to store Yij*Wik'
CvMat* E = cvCreateMat( S->height, 1 , CV_64F ); //this is right part of system with S
cvSetZero(YWt);
cvSetZero(E);
while(!done)
{
while(!done) {
// compute jacobians Aij and Bij
ask_for_projac();
ask_for_projac(_vis);
int invisible_count=0;
//compute U_j and ea_j
for( int j = 0; j < num_cams; j++ )
{
for (int j = 0; j < num_cams; j++ ) {
cvSetZero(U[j]);
cvSetZero(ea[j]);
//summ by i (number of points)
for( int i = 0; i < num_points; i++ )
{
for (int i = 0; i < num_points; i++ ) {
//get Aij
CvMat* Aij = ((CvMat**)(A->data.ptr + A->step * i))[j];
if( Aij )
{
//CvMat* Aij = ((CvMat**)(A->data.ptr + A->step * i))[j];
CvMat* Aij = A[j+i*num_cams];
if (Aij ) {
//Uj+= AijT*Aij
cvGEMM( Aij, Aij, 1, U[j], 1, U[j], CV_GEMM_A_T );
//ea_j += AijT * e_ij
CvMat eij;
int index = ((int*)(Vis_index->data.ptr + i * Vis_index->step))[j];
cvGetSubRect( err, &eij, cvRect( 0, index, 1, Aij->height /*width of transposed Aij*/ ) );
cvGetSubRect( err, &eij, cvRect( 0, index, 1, Aij->height ) ); //width of transposed Aij
cvGEMM( Aij, &eij, 1, ea[j], 1, ea[j], CV_GEMM_A_T );
}
else
invisible_count++;
}
} //U_j and ea_j computed for all j
// if (!(iters%100))
int nviz = X->rows / num_err_param;
double e2 = prevErrNorm*prevErrNorm, e2n = e2 / nviz;
std::cerr<<"Iteration: "<<iters<<", normError: "<<e2<<" ("<<e2n<<")"<<std::endl;
if (cb)
cb(iters, prevErrNorm, user_data);
//compute V_i and eb_i
for( int i = 0; i < num_points; i++ )
{
for (int i = 0; i < num_points; i++ ) {
cvSetZero(V[i]);
cvSetZero(eb[i]);
//summ by i (number of points)
for( int j = 0; j < num_cams; j++ )
{
for( int j = 0; j < num_cams; j++ ) {
//get Bij
CvMat* Bij = ((CvMat**)(B->data.ptr + B->step * i))[j];
if( Bij )
{
//CvMat* Bij = ((CvMat**)(B->data.ptr + B->step * i))[j];
CvMat* Bij = B[j+i*num_cams];
if (Bij ) {
//Vi+= BijT*Bij
cvGEMM( Bij, Bij, 1, V[i], 1, V[i], CV_GEMM_A_T );
......@@ -429,22 +461,22 @@ void LevMarqSparse::optimize() //main function that runs minimization
int index = ((int*)(Vis_index->data.ptr + i * Vis_index->step))[j];
CvMat eij;
cvGetSubRect( err, &eij, cvRect( 0, index, 1, Bij->height /*width of transposed Bij*/ ) );
cvGetSubRect( err, &eij, cvRect( 0, index, 1, Bij->height ) ); //width of transposed Bij
cvGEMM( Bij, &eij, 1, eb[i], 1, eb[i], CV_GEMM_A_T );
}
}
} //V_i and eb_i computed for all i
//compute W_ij
for( int i = 0; i < num_points; i++ )
{
for( int j = 0; j < num_cams; j++ )
{
CvMat* Aij = ((CvMat**)(A->data.ptr + A->step * i))[j];
if( Aij ) //visible
{
CvMat* Bij = ((CvMat**)(B->data.ptr + B->step * i))[j];
CvMat* Wij = ((CvMat**)(W->data.ptr + W->step * j))[i];
for( int i = 0; i < num_points; i++ ) {
for( int j = 0; j < num_cams; j++ ) {
//CvMat* Aij = ((CvMat**)(A->data.ptr + A->step * i))[j];
CvMat* Aij = A[j+i*num_cams];
if( Aij ) { //visible
//CvMat* Bij = ((CvMat**)(B->data.ptr + B->step * i))[j];
CvMat* Bij = B[j+i*num_cams];
//CvMat* Wij = ((CvMat**)(W->data.ptr + W->step * j))[i];
CvMat* Wij = W[j+i*num_cams];
//multiply
cvGEMM( Aij, Bij, 1, NULL, 0, Wij, CV_GEMM_A_T );
......@@ -456,15 +488,13 @@ void LevMarqSparse::optimize() //main function that runs minimization
{
CvMat dia;
CvMat subr;
for( int j = 0; j < num_cams; j++ )
{
for( int j = 0; j < num_cams; j++ ) {
cvGetDiag(U[j], &dia);
cvGetSubRect(JtJ_diag, &subr,
cvRect(0, j*num_cam_param, 1, num_cam_param ));
cvCopy( &dia, &subr );
}
for( int i = 0; i < num_points; i++ )
{
for( int i = 0; i < num_points; i++ ) {
cvGetDiag(V[i], &dia);
cvGetSubRect(JtJ_diag, &subr,
cvRect(0, num_cams*num_cam_param + i * num_point_param, 1, num_point_param ));
......@@ -472,29 +502,26 @@ void LevMarqSparse::optimize() //main function that runs minimization
}
}
if( iters == 0 )
{
if( iters == 0 ) {
//initialize lambda. It is set to 1e-3 * average diagonal element in JtJ
double average_diag = 0;
for( int j = 0; j < num_cams; j++ )
{
for( int j = 0; j < num_cams; j++ ) {
average_diag += cvTrace( U[j] ).val[0];
}
for( int i = 0; i < num_points; i++ )
{
for( int i = 0; i < num_points; i++ ) {
average_diag += cvTrace( V[i] ).val[0];
}
average_diag /= (num_cams*num_cam_param + num_points * num_point_param );
// lambda = 1e-3 * average_diag;
lambda = 1e-3 * average_diag;
lambda = 0.245560;
}
//now we are going to find good step and make it
for(;;)
{
for(;;) {
//augmentation of diagonal
for(int j = 0; j < num_cams; j++ )
{
for(int j = 0; j < num_cams; j++ ) {
CvMat diag;
cvGetDiag( U[j], &diag );
#if 1
......@@ -503,8 +530,7 @@ void LevMarqSparse::optimize() //main function that runs minimization
cvScale( &diag, &diag, 1 + lambda );
#endif
}
for(int i = 0; i < num_points; i++ )
{
for(int i = 0; i < num_points; i++ ) {
CvMat diag;
cvGetDiag( V[i], &diag );
#if 1
......@@ -516,48 +542,43 @@ void LevMarqSparse::optimize() //main function that runs minimization
bool error = false;
//compute inv(V*)
bool inverted_ok = true;
for(int i = 0; i < num_points; i++ )
{
for(int i = 0; i < num_points; i++ ) {
double det = cvInvert( V[i], inv_V_star[i] );
if( fabs(det) <= FLT_EPSILON )
{
if( fabs(det) <= FLT_EPSILON ) {
inverted_ok = false;
std::cerr<<"V["<<i<<"] failed"<<std::endl;
break;
} //means we did wrong augmentation, try to choose different lambda
}
if( inverted_ok )
{
if( inverted_ok ) {
cvSetZero( E );
//loop through cameras, compute upper diagonal blocks of matrix S
for( int j = 0; j < num_cams; j++ )
{
for( int j = 0; j < num_cams; j++ ) {
//compute Yij = Wij (V*_i)^-1 for all i (if Wij exists/nonzero)
for( int i = 0; i < num_points; i++ )
{
for( int i = 0; i < num_points; i++ ) {
//
CvMat* Wij = ((CvMat**)(W->data.ptr + W->step * j))[i];
if( Wij )
{
//CvMat* Wij = ((CvMat**)(W->data.ptr + W->step * j))[i];
CvMat* Wij = W[j+i*num_cams];
if( Wij ) {
cvMatMul( Wij, inv_V_star[i], Yj[i] );
}
}
//compute Sjk for k>=j (because Sjk = Skj)
for( int k = j; k < num_cams; k++ )
{
for( int k = j; k < num_cams; k++ ) {
cvSetZero( YWt );
for( int i = 0; i < num_points; i++ )
{
for( int i = 0; i < num_points; i++ ) {
//check that both Wij and Wik exist
CvMat* Wij = ((CvMat**)(W->data.ptr + W->step * j))[i];
CvMat* Wik = ((CvMat**)(W->data.ptr + W->step * k))[i];
// CvMat* Wij = ((CvMat**)(W->data.ptr + W->step * j))[i];
CvMat* Wij = W[j+i*num_cams];
//CvMat* Wik = ((CvMat**)(W->data.ptr + W->step * k))[i];
CvMat* Wik = W[k+i*num_cams];
if( Wij && Wik )
{
if( Wij && Wik ) {
//multiply YWt += Yj[i]*Wik'
cvGEMM( Yj[i], Wik, 1, YWt, 1, YWt, CV_GEMM_B_T /*transpose Wik*/ );
cvGEMM( Yj[i], Wik, 1, YWt, 1, YWt, CV_GEMM_B_T ); ///*transpose Wik
}
}
......@@ -569,13 +590,10 @@ void LevMarqSparse::optimize() //main function that runs minimization
//if j==k, add diagonal
if( j != k )
{
if( j != k ) {
//just copy with minus
cvScale( YWt, &Sjk, -1 ); //if we set initial S to zero then we can use cvSub( Sjk, YWt, Sjk);
}
else
{
} else {
//add diagonal value
//subtract YWt from augmented Uj
......@@ -591,9 +609,9 @@ void LevMarqSparse::optimize() //main function that runs minimization
//select submat
cvGetSubRect( E, &e_j, cvRect( 0, j * num_cam_param, 1, num_cam_param ) );
for( int i = 0; i < num_points; i++ )
{
CvMat* Wij = ((CvMat**)(W->data.ptr + W->step * j))[i];
for( int i = 0; i < num_points; i++ ) {
//CvMat* Wij = ((CvMat**)(W->data.ptr + W->step * j))[i];
CvMat* Wij = W[j+i*num_cams];
if( Wij )
cvMatMulAdd( Yj[i], eb[i], &e_j, &e_j );
}
......@@ -603,33 +621,29 @@ void LevMarqSparse::optimize() //main function that runs minimization
}
//fill below diagonal elements of matrix S
cvCompleteSymm( S, 0 /*from upper to low*/ ); //operation may be done by nonzero blocks or during upper diagonal computation
cvCompleteSymm( S, 0 ); ///*from upper to low //operation may be done by nonzero blocks or during upper diagonal computation
//Solve linear system S * deltaP_a = E
CvMat dpa;
cvGetSubRect( deltaP, &dpa, cvRect(0, 0, 1, S->width ) );
int res = cvSolve( S, E, &dpa );
int res = cvSolve( S, E, &dpa, CV_CHOLESKY );
if( res ) //system solved ok
{
if( res ) { //system solved ok
//compute db_i
for( int i = 0; i < num_points; i++ )
{
for( int i = 0; i < num_points; i++ ) {
CvMat dbi;
cvGetSubRect( deltaP, &dbi, cvRect( 0, dpa.height + i * num_point_param, 1, num_point_param ) );
/* compute \sum_j W_ij^T da_j */
for( int j = 0; j < num_cams; j++ )
{
// compute \sum_j W_ij^T da_j
for( int j = 0; j < num_cams; j++ ) {
//get Wij
CvMat* Wij = ((CvMat**)(W->data.ptr + W->step * j))[i];
if( Wij )
{
//CvMat* Wij = ((CvMat**)(W->data.ptr + W->step * j))[i];
CvMat* Wij = W[j+i*num_cams];
if( Wij ) {
//get da_j
CvMat daj;
cvGetSubRect( &dpa, &daj, cvRect( 0, j * num_cam_param, 1, num_cam_param ));
cvGEMM( Wij, &daj, 1, &dbi, 1, &dbi, CV_GEMM_A_T /* transpose Wij */ );
cvGEMM( Wij, &daj, 1, &dbi, 1, &dbi, CV_GEMM_A_T ); ///* transpose Wij
}
}
//finalize dbi
......@@ -641,53 +655,49 @@ void LevMarqSparse::optimize() //main function that runs minimization
cvAdd( prevP, deltaP, P );
//evaluate function with new parameters
ask_for_proj(); // func( P, hX );
ask_for_proj(_vis); // func( P, hX );
//compute error
errNorm = cvNorm( X, hX, CV_L2 );
}
else
{
} else {
error = true;
}
}
else
{
} else {
error = true;
}
//check solution
if( error || /* singularities somewhere */
errNorm > prevErrNorm ) //step was not accepted
{
if( error || ///* singularities somewhere
errNorm > prevErrNorm ) { //step was not accepted
//increase lambda and reject change
lambda *= 10;
int nviz = X->rows / num_err_param;
double e2 = errNorm*errNorm, e2_prev = prevErrNorm*prevErrNorm;
double e2n = e2/nviz, e2n_prev = e2_prev/nviz;
std::cerr<<"move failed: lambda = "<<lambda<<", e2 = "<<e2<<" ("<<e2n<<") > "<<e2_prev<<" ("<<e2n_prev<<")"<<std::endl;
//restore diagonal from backup
{
CvMat dia;
CvMat subr;
for( int j = 0; j < num_cams; j++ )
{
for( int j = 0; j < num_cams; j++ ) {
cvGetDiag(U[j], &dia);
cvGetSubRect(JtJ_diag, &subr,
cvRect(0, j*num_cam_param, 1, num_cam_param ));
cvCopy( &subr, &dia );
}
for( int i = 0; i < num_points; i++ )
{
for( int i = 0; i < num_points; i++ ) {
cvGetDiag(V[i], &dia);
cvGetSubRect(JtJ_diag, &subr,
cvRect(0, num_cams*num_cam_param + i * num_point_param, 1, num_point_param ));
cvCopy( &subr, &dia );
}
}
}
else //all is ok
{
} else { //all is ok
//accept change and decrease lambda
lambda /= 10;
lambda = MAX(lambda, 1e-16);
std::cerr<<"decreasing lambda to "<<lambda<<std::endl;
prevErrNorm = errNorm;
//compute new projection error vector
......@@ -700,13 +710,11 @@ void LevMarqSparse::optimize() //main function that runs minimization
double param_change_norm = cvNorm(P, prevP, CV_RELATIVE_L2);
//check termination criteria
if( (criteria.type&CV_TERMCRIT_ITER && iters > criteria.max_iter ) ||
(criteria.type&CV_TERMCRIT_EPS && param_change_norm < criteria.epsilon) )
{
(criteria.type&CV_TERMCRIT_EPS && param_change_norm < criteria.epsilon) ) {
// std::cerr<<"relative norm change "<<param_change_norm<<" lower than eps "<<criteria.epsilon<<", stopping"<<std::endl;
done = true;
break;
}
else
{
} else {
//copy new params and continue iterations
cvCopy( P, prevP );
}
......@@ -717,15 +725,14 @@ void LevMarqSparse::optimize() //main function that runs minimization
//Utilities
void fjac(int /*i*/, int /*j*/, CvMat *point_params, CvMat* cam_params, CvMat* A, CvMat* B, void* /*data*/)
{
void fjac(int /*i*/, int /*j*/, CvMat *point_params, CvMat* cam_params, CvMat* A, CvMat* B, void* /*data*/) {
//compute jacobian per camera parameters (i.e. Aij)
//take i-th point 3D current coordinates
CvMat _Mi;
cvReshape(point_params, &_Mi, 3, 1 );
CvMat* _mp = cvCreateMat(1, 2, CV_64F ); //projection of the point
CvMat* _mp = cvCreateMat(1, 1, CV_64FC2 ); //projection of the point
//split camera params into different matrices
CvMat _ri, _ti, _k;
......@@ -738,7 +745,7 @@ void fjac(int /*i*/, int /*j*/, CvMat *point_params, CvMat* cam_params, CvMat* A
intr_data[2] = cam_params->data.db[8];
intr_data[5] = cam_params->data.db[9];
CvMat matA = cvMat(3,3, CV_64F, intr_data );
CvMat _A = cvMat(3,3, CV_64F, intr_data );
CvMat _dpdr, _dpdt, _dpdf, _dpdc, _dpdk;
......@@ -749,12 +756,11 @@ void fjac(int /*i*/, int /*j*/, CvMat *point_params, CvMat* cam_params, CvMat* A
cvGetCols( A, &_dpdf, 6, 8 );
cvGetCols( A, &_dpdc, 8, 10 );
if( have_dk )
{
if( have_dk ) {
cvGetRows( cam_params, &_k, 10, cam_params->height );
cvGetCols( A, &_dpdk, 10, A->width );
}
cvProjectPoints2( &_Mi, &_ri, &_ti, &matA, have_dk ? &_k : NULL, _mp, &_dpdr, &_dpdt,
cvProjectPoints2(&_Mi, &_ri, &_ti, &_A, have_dk ? &_k : NULL, _mp, &_dpdr, &_dpdt,
&_dpdf, &_dpdc, have_dk ? &_dpdk : NULL, 0);
cvReleaseMat( &_mp );
......@@ -805,8 +811,8 @@ void fjac(int /*i*/, int /*j*/, CvMat *point_params, CvMat* cam_params, CvMat* A
//get rotation matrix
double R[9], t[3], fx = intr_data[0], fy = intr_data[4];
CvMat matR = cvMat( 3, 3, CV_64F, R );
cvRodrigues2(&_ri, &matR);
CvMat _R = cvMat( 3, 3, CV_64F, R );
cvRodrigues2(&_ri, &_R);
double X,Y,Z;
X = point_params->data.db[0];
......@@ -836,11 +842,10 @@ void fjac(int /*i*/, int /*j*/, CvMat *point_params, CvMat* cam_params, CvMat* A
CvMat coeffmat = cvMat( 2, 3, CV_64F, coeff );
CvMat* dstrike_dbig = cvCreateMat(2,3,CV_64F);
cvMatMul(&coeffmat, &matR, dstrike_dbig);
cvMatMul(&coeffmat, &_R, dstrike_dbig);
cvScale(dstrike_dbig, dstrike_dbig, 1/(z*z) );
if( have_dk )
{
if( have_dk ) {
double strike_[2] = {x_strike, y_strike};
CvMat strike = cvMat(1, 2, CV_64F, strike_);
......@@ -860,8 +865,7 @@ void fjac(int /*i*/, int /*j*/, CvMat *point_params, CvMat* cam_params, CvMat* A
double& p2 = _k.data.db[3];
double k3 = 0;
if( _k.cols*_k.rows == 5 )
{
if( _k.cols*_k.rows == 5 ) {
k3 = _k.data.db[4];
}
//compute dg/dbig
......@@ -898,9 +902,7 @@ void fjac(int /*i*/, int /*j*/, CvMat *point_params, CvMat* cam_params, CvMat* A
cvReleaseMat(&tmp);
cvReleaseMat(&dstrike2_dbig);
cvReleaseMat(&tmp);
}
else
{
} else {
cvCopy(dstrike_dbig, B);
}
//multiply by fx, fy
......@@ -928,13 +930,13 @@ void fjac(int /*i*/, int /*j*/, CvMat *point_params, CvMat* cam_params, CvMat* A
#endif
};
void func(int /*i*/, int /*j*/, CvMat *point_params, CvMat* cam_params, CvMat* estim, void* /*data*/)
{
void func(int /*i*/, int /*j*/, CvMat *point_params, CvMat* cam_params, CvMat* estim, void* /*data*/) {
//just do projections
CvMat _Mi;
cvReshape( point_params, &_Mi, 3, 1 );
CvMat* _mp = cvCreateMat(1, 2, CV_64F ); //projection of the point
CvMat* _mp = cvCreateMat(1, 1, CV_64FC2 ); //projection of the point
CvMat* _mp2 = cvCreateMat(1, 2, CV_64F ); //projection of the point
//split camera params into different matrices
CvMat _ri, _ti, _k;
......@@ -948,30 +950,32 @@ void func(int /*i*/, int /*j*/, CvMat *point_params, CvMat* cam_params, CvMat* e
intr_data[2] = cam_params->data.db[8];
intr_data[5] = cam_params->data.db[9];
CvMat matA = cvMat(3,3, CV_64F, intr_data );
CvMat _A = cvMat(3,3, CV_64F, intr_data );
//int cn = CV_MAT_CN(_Mi.type);
bool have_dk = cam_params->height - 10 ? true : false;
if( have_dk )
{
if( have_dk ) {
cvGetRows( cam_params, &_k, 10, cam_params->height );
}
cvProjectPoints2( &_Mi, &_ri, &_ti, &matA, have_dk ? &_k : NULL, _mp, NULL, NULL,
cvProjectPoints2( &_Mi, &_ri, &_ti, &_A, have_dk ? &_k : NULL, _mp, NULL, NULL,
NULL, NULL, NULL, 0);
cvTranspose( _mp, estim );
// std::cerr<<"_mp = "<<_mp->data.db[0]<<","<<_mp->data.db[1]<<std::endl;
//
_mp2->data.db[0] = _mp->data.db[0];
_mp2->data.db[1] = _mp->data.db[1];
cvTranspose( _mp2, estim );
cvReleaseMat( &_mp );
cvReleaseMat( &_mp2 );
};
void fjac_new(int i, int j, Mat& point_params, Mat& cam_params, Mat& A, Mat& B, void* data)
{
CvMat _point_params = point_params, _cam_params = cam_params, matA = A, matB = B;
fjac(i,j, &_point_params, &_cam_params, &matA, &matB, data);
void fjac_new(int i, int j, Mat& point_params, Mat& cam_params, Mat& A, Mat& B, void* data) {
CvMat _point_params = point_params, _cam_params = cam_params, _A = A, _B = B;
fjac(i,j, &_point_params, &_cam_params, &_A, &_B, data);
};
void func_new(int i, int j, Mat& point_params, Mat& cam_params, Mat& estim, void* data)
{
void func_new(int i, int j, Mat& point_params, Mat& cam_params, Mat& estim, void* data) {
CvMat _point_params = point_params, _cam_params = cam_params, _estim = estim;
func(i,j,&_point_params,&_cam_params,&_estim,data);
};
......@@ -983,11 +987,11 @@ void LevMarqSparse::bundleAdjust( vector<Point3d>& points, //positions of points
vector<Mat>& R, //rotation matrices of all cameras (input and output)
vector<Mat>& T, //translation vector of all cameras (input and output)
vector<Mat>& distCoeffs, //distortion coefficients of all cameras (input and output)
const TermCriteria& criteria)
const TermCriteria& criteria,
BundleAdjustCallback cb, void* user_data) {
//,enum{MOTION_AND_STRUCTURE,MOTION,STRUCTURE})
{
int num_points = (int)points.size();
int num_cameras = (int)cameraMatrix.size();
int num_points = points.size();
int num_cameras = cameraMatrix.size();
CV_Assert( imagePoints.size() == (size_t)num_cameras &&
visibility.size() == (size_t)num_cameras &&
......@@ -1006,8 +1010,7 @@ void LevMarqSparse::bundleAdjust( vector<Point3d>& points, //positions of points
Mat params( num_cameras * num_cam_param + num_points * num_point_param, 1, CV_64F );
//fill camera params
for( int i = 0; i < num_cameras; i++ )
{
for( int i = 0; i < num_cameras; i++ ) {
//rotation
Mat rot_vec; Rodrigues( R[i], rot_vec );
Mat dst = params.rowRange(i*num_cam_param, i*num_cam_param+3);
......@@ -1028,8 +1031,7 @@ void LevMarqSparse::bundleAdjust( vector<Point3d>& points, //positions of points
intr[3] = intr_data[5]; //cy
//add distortion if exists
if( distCoeffs.size() )
{
if( distCoeffs.size() ) {
dst = params.rowRange(i*num_cam_param + 10, i*num_cam_param+10+numdist);
distCoeffs[i].copyTo(dst);
}
......@@ -1043,8 +1045,7 @@ void LevMarqSparse::bundleAdjust( vector<Point3d>& points, //positions of points
//convert visibility vectors to visibility matrix
Mat vismat(num_points, num_cameras, CV_32S);
for( int i = 0; i < num_cameras; i++ )
{
for( int i = 0; i < num_cameras; i++ ) {
//get row
Mat col = vismat.col(i);
Mat((int)visibility[i].size(), 1, vismat.type(), (void*)&visibility[i][0]).copyTo( col );
......@@ -1056,34 +1057,41 @@ void LevMarqSparse::bundleAdjust( vector<Point3d>& points, //positions of points
Mat X(num_proj*2,1,CV_64F); //measurement vector
int counter = 0;
for(int i = 0; i < num_points; i++ )
{
for(int j = 0; j < num_cameras; j++ )
{
for(int i = 0; i < num_points; i++ ) {
for(int j = 0; j < num_cameras; j++ ) {
//check visibility
if( visibility[j][i] )
{
if( visibility[j][i] ) {
//extract point and put tu vector
Point2d p = imagePoints[j][i];
((double*)(X.data))[counter] = p.x;
((double*)(X.data))[counter+1] = p.y;
assert(p.x != -1 || p.y != -1);
counter+=2;
}
}
}
LevMarqSparse levmar( num_points, num_cameras, num_point_param, num_cam_param, 2, vismat, params, X,
TermCriteria(criteria), fjac_new, func_new, NULL );
TermCriteria(criteria), fjac_new, func_new, NULL,
cb, user_data);
//extract results
//fill point params
Mat final_points(num_points, 1, CV_64FC3,
/*Mat final_points(num_points, 1, CV_64FC3,
levmar.P->data.db + num_cameras*num_cam_param *levmar.P->step);
CV_Assert(_points.size() == final_points.size() && _points.type() == final_points.type());
final_points.copyTo(_points);
final_points.copyTo(_points);*/
points.clear();
for( int i = 0; i < num_points; i++ ) {
CvMat point_mat;
cvGetSubRect( levmar.P, &point_mat, cvRect( 0, levmar.num_cams * levmar.num_cam_param+ levmar.num_point_param * i, 1, levmar.num_point_param ));
CvScalar x = cvGet2D(&point_mat,0,0); CvScalar y = cvGet2D(&point_mat,1,0); CvScalar z = cvGet2D(&point_mat,2,0);
points.push_back(Point3f(x.val[0],y.val[0],z.val[0]));
//std::cerr<<"point"<<points[points.size()-1].x<<","<<points[points.size()-1].y<<","<<points[points.size()-1].z<<std::endl;
}
//fill camera params
for( int i = 0; i < num_cameras; i++ )
{
//R.clear();T.clear();cameraMatrix.clear();
for( int i = 0; i < num_cameras; i++ ) {
//rotation
Mat rot_vec = Mat(levmar.P).rowRange(i*num_cam_param, i*num_cam_param+3);
Rodrigues( rot_vec, R[i] );
......@@ -1101,11 +1109,8 @@ void LevMarqSparse::bundleAdjust( vector<Point3d>& points, //positions of points
intr_data[5] = intr[3]; //cy
//add distortion if exists
if( distCoeffs.size() )
{
params.rowRange(i*num_cam_param + 10, i*num_cam_param+10+numdist).copyTo(distCoeffs[i]);
if( distCoeffs.size() ) {
Mat(levmar.P).rowRange(i*num_cam_param + 10, i*num_cam_param+10+numdist).copyTo(distCoeffs[i]);
}
}
}
}// end of namespace cv
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment