/* Given a parameter vector p made up of the 3D coordinates of n points, compute in
* jac the jacobian of the predicted measurements, i.e. the jacobian of the projections of 3D points in the m images.
* The jacobian is returned in the order (B_11, ..., B_1m, ..., B_n1, ..., B_nm), where B_ij=dx_ij/db_i (see HZ).
* Caller supplies rcidxs and rcsubs which can be used as working memory.
* Notice that depending on idxij, some of the B_ij might be missing
*
*/
static void sba_str_Qs_jac(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata)
{
register int i, j;
int pnp, mnp;
double *pbi, *pBij;
//int n;
int m, nnz, Bsz, idx;
struct wrap_str_data_ *wdata;
void (*projac)(int j, int i, double *bi, double *Bij, void *projac_adata);
void *projac_adata;
wdata=(struct wrap_str_data_ *)adata;
pnp=wdata->pnp; mnp=wdata->mnp;
projac=wdata->projac;
projac_adata=wdata->adata;
//n=idxij->nr;
m=idxij->nc;
Bsz=mnp*pnp;
for(j=0; j<m; ++j){
nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); /* find nonzero hx_ij, i=0...n-1 */
for(i=0; i<nnz; ++i){
pbi=p + rcsubs[i]*pnp;
idx=idxij->val[rcidxs[i]];
pBij=jac + idx*Bsz; // set pBij to point to B_ij
(*projac)(j, rcsubs[i], pbi, pBij, projac_adata); // evaluate dQ/db in pBij
}
}
}
/* Given a parameter vector p made up of the 3D coordinates of n points and the parameters of m cameras, compute in
* hx the prediction of the measurements, i.e. the projections of 3D points in the m images. The measurements
* are returned in the order (hx_11^T, .. hx_1m^T, ..., hx_n1^T, .. hx_nm^T)^T, where hx_ij is the predicted
* projection of the i-th point on the j-th camera.
* Caller supplies rcidxs and rcsubs which can be used as working memory.
* Notice that depending on idxij, some of the hx_ij might be missing
*
*/
static void sba_motstr_Qs(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *hx, void *adata)
{
register int i, j;
int cnp, pnp, mnp;
double *pa, *pb, *paj, *pbi, *pxij;
int n, m, nnz;
struct wrap_motstr_data_ *wdata;
void (*proj)(int j, int i, double *aj, double *bi, double *xij, void *proj_adata);
void *proj_adata;
wdata=(struct wrap_motstr_data_ *)adata;
cnp=wdata->cnp; pnp=wdata->pnp; mnp=wdata->mnp;
proj=wdata->proj;
proj_adata=wdata->adata;
n=idxij->nr; m=idxij->nc;
pa=p; pb=p+m*cnp;
for(j=0; j<m; ++j){
/* j-th camera parameters */
paj=pa+j*cnp;
nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); /* find nonzero hx_ij, i=0...n-1 */
for(i=0; i<nnz; ++i){
pbi=pb + rcsubs[i]*pnp;
pxij=hx + idxij->val[rcidxs[i]]*mnp; // set pxij to point to hx_ij
(*proj)(j, rcsubs[i], paj, pbi, pxij, proj_adata); // evaluate Q in pxij
}
}
}
/* Given a parameter vector p made up of the parameters of m cameras, compute in jac
* the jacobian of the predicted measurements, i.e. the jacobian of the projections of 3D points in the m images.
* The jacobian is returned in the order (A_11, ..., A_1m, ..., A_n1, ..., A_nm),
* where A_ij=dx_ij/db_j (see HZ).
* Caller supplies rcidxs and rcsubs which can be used as working memory.
* Notice that depending on idxij, some of the A_ij might be missing
*
*/
static void sba_mot_Qs_jac(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata)
{
register int i, j;
int cnp, mnp;
double *paj, *pAij;
//int n;
int m, nnz, Asz, idx;
struct wrap_mot_data_ *wdata;
void (*projac)(int j, int i, double *aj, double *Aij, void *projac_adata);
void *projac_adata;
wdata=(struct wrap_mot_data_ *)adata;
cnp=wdata->cnp; mnp=wdata->mnp;
projac=wdata->projac;
projac_adata=wdata->adata;
//n=idxij->nr;
m=idxij->nc;
Asz=mnp*cnp;
for(j=0; j<m; ++j){
/* j-th camera parameters */
paj=p+j*cnp;
nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); /* find nonzero hx_ij, i=0...n-1 */
for(i=0; i<nnz; ++i){
idx=idxij->val[rcidxs[i]];
pAij=jac + idx*Asz; // set pAij to point to A_ij
(*projac)(j, rcsubs[i], paj, pAij, projac_adata); // evaluate dQ/da in pAij
}
}
}
/* Given a parameter vector p made up of the 3D coordinates of n points, compute in
* hx the prediction of the measurements, i.e. the projections of 3D points in the m images. The measurements
* are returned in the order (hx_11^T, .. hx_1m^T, ..., hx_n1^T, .. hx_nm^T)^T, where hx_ij is the predicted
* projection of the i-th point on the j-th camera.
* Caller supplies rcidxs and rcsubs which can be used as working memory.
* Notice that depending on idxij, some of the hx_ij might be missing
*
*/
static void sba_str_Qs(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *hx, void *adata)
{
register int i, j;
int pnp, mnp;
double *pbi, *pxij;
//int n;
int m, nnz;
struct wrap_str_data_ *wdata;
void (*proj)(int j, int i, double *bi, double *xij, void *proj_adata);
void *proj_adata;
wdata=(struct wrap_str_data_ *)adata;
pnp=wdata->pnp; mnp=wdata->mnp;
proj=wdata->proj;
proj_adata=wdata->adata;
//n=idxij->nr;
m=idxij->nc;
for(j=0; j<m; ++j){
nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); /* find nonzero hx_ij, i=0...n-1 */
for(i=0; i<nnz; ++i){
pbi=p + rcsubs[i]*pnp;
pxij=hx + idxij->val[rcidxs[i]]*mnp; // set pxij to point to hx_ij
(*proj)(j, rcsubs[i], pbi, pxij, proj_adata); // evaluate Q in pxij
}
}
}
void sba_pinhole_model(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *hx, void *adata) {
const sba_model_data_t<double> *sba_data = static_cast< sba_model_data_t<double>* >(adata);
if(!sba_data) return;
const cv::Mat &camera_matrix = sba_data->camera_matrix;
const int num_cols = idxij->nc;
double *p_pose = p;
double *p_points = p+num_cols*num_params_per_cam;
double rotation_quat[quaternion_size];
double total_rot_quat[quaternion_size];
for(int j=0; j<num_cols; ++j) {
// get j-th camera parameters
double *p_quat = p_pose + j*num_params_per_cam;
double *p_transl = p_quat + 3; // rotation vector part has 3 elements
// get rotation estimation
vec2quat(p_quat, rotation_quat);
const double *p_rotation_guess = &(sba_data->initial_rotations[j*quaternion_size]);
multiply_quaternion(rotation_quat, p_rotation_guess, total_rot_quat);
// find number of nonzero hx_ij, i=0...n-1
int num_nonzero = sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs);
for(int i=0; i<num_nonzero; ++i) {
double *p_point = p_points + rcsubs[i]*num_params_per_point;
double *p_measurement = hx + idxij->val[rcidxs[i]]*num_params_per_measuremnt; // p_measurement = hx_ij
quaternion_pinhole_model<double>(p_point, total_rot_quat, p_transl, camera_matrix, p_measurement);
}
}
}
main()
{
int mat[7][6]= {
{ 10, 0, 0, 0, -2, 0},
{ 3, 9, 0, 0, 0, 3},
{ 0, 7, 8, 7, 0, 0},
{ 3, 0, 8, 7, 5, 0},
{ 0, 8, 0, 9, 9, 13},
{ 0, 4, 0, 0, 2, -1},
{ 3, 7, 0, 9, 2, 0}
};
struct sba_crsm sm;
int idx1, idx2, k, pyramidLevel;
int vidxs[7], /* max(6, 7) */
jidxs[6], iidxs[7];
sba_crsm_build(&sm, mat[0], 7, 6);
sba_crsm_print(&sm, stdout);
for(idx1=0; idx1<7; ++idx1) {
for(idx2=0; idx2<6; ++idx2)
printf("%3d ", ((k=sba_crsm_elmidx(&sm, idx1, idx2))!=-1)? sm.val[k] : 0);
printf("\n");
}
for(idx1=0; idx1<7; ++idx1) {
k=sba_crsm_row_elmidxs(&sm, idx1, vidxs, jidxs);
printf("row %d\n", idx1);
for(pyramidLevel=0; pyramidLevel<k; ++pyramidLevel) {
idx2=jidxs[pyramidLevel];
printf("%d %d ", idx2, sm.val[vidxs[pyramidLevel]]);
}
printf("\n");
}
for(idx2=0; idx2<6; ++idx2) {
k=sba_crsm_col_elmidxs(&sm, idx2, vidxs, iidxs);
printf("col %d\n", idx2);
for(pyramidLevel=0; pyramidLevel<k; ++pyramidLevel) {
idx1=iidxs[pyramidLevel];
printf("%d %d ", idx1, sm.val[vidxs[pyramidLevel]]);
}
printf("\n");
}
sba_crsm_free(&sm);
}
void sba_pinhole_model_jac(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata) {
const sba_model_data_t<double> *sba_data = static_cast< sba_model_data_t<double>* >(adata);
if(!sba_data) return;
const cv::Mat &camera_matrix = sba_data->camera_matrix;
const int num_cols = idxij->nc;
double *p_pose = p;
double *p_points = p+num_cols*num_params_per_cam;
static const int size_A = num_params_per_measuremnt*num_params_per_cam;
static const int size_B = num_params_per_measuremnt*num_params_per_point;
static const int size_AB = size_A+size_B;
for(int j=0; j<num_cols; ++j) {
// get j-th camera parameters
double *p_quat = p_pose + j*num_params_per_cam;
double *p_transl = p_quat + 3; // rotation vector part has 3 elements
const double *p_rotation_guess = &(sba_data->initial_rotations[j*quaternion_size]);
// find number of nonzero hx_ij, i=0...n-1
int num_nonzero = sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs);
for(int i=0; i<num_nonzero; ++i) {
double *p_point = p_points + rcsubs[i]*num_params_per_point;
double *p_Aij = jac + idxij->val[rcidxs[i]]*size_AB;
double *p_Bij = p_Aij + size_A;
quaternion_pinhole_model_jac<double>(p_point,
p_quat,
p_transl,
camera_matrix,
p_rotation_guess,
(double (*)[6])p_Aij,
(double (*)[3])p_Bij);
}
}
}
/* Given a parameter vector p made up of the 3D coordinates of n points and the parameters of m cameras, compute in
* jac the jacobian of the predicted measurements, i.e. the jacobian of the projections of 3D points in the m images.
* The jacobian is returned in the order (A_11, B_11, ..., A_1m, B_1m, ..., A_n1, B_n1, ..., A_nm, B_nm),
* where A_ij=dx_ij/db_j and B_ij=dx_ij/db_i (see HZ).
* Caller supplies rcidxs and rcsubs which can be used as working memory.
* Notice that depending on idxij, some of the A_ij, B_ij might be missing
*
*/
static void sba_motstr_Qs_jac(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata)
{
register int i, j;
int cnp, pnp, mnp;
double *pa, *pb, *paj, *pbi, *pAij, *pBij;
int n, m, nnz, Asz, Bsz, ABsz, idx;
struct wrap_motstr_data_ *wdata;
void (*projac)(int j, int i, double *aj, double *bi, double *Aij, double *Bij, void *projac_adata);
void *projac_adata;
wdata=(struct wrap_motstr_data_ *)adata;
cnp=wdata->cnp; pnp=wdata->pnp; mnp=wdata->mnp;
projac=wdata->projac;
projac_adata=wdata->adata;
n=idxij->nr; m=idxij->nc;
pa=p; pb=p+m*cnp;
Asz=mnp*cnp; Bsz=mnp*pnp; ABsz=Asz+Bsz;
for(j=0; j<m; ++j){
/* j-th camera parameters */
paj=pa+j*cnp;
nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); /* find nonzero hx_ij, i=0...n-1 */
for(i=0; i<nnz; ++i){
pbi=pb + rcsubs[i]*pnp;
idx=idxij->val[rcidxs[i]];
pAij=jac + idx*ABsz; // set pAij to point to A_ij
pBij=pAij + Asz; // set pBij to point to B_ij
(*projac)(j, rcsubs[i], paj, pbi, pAij, pBij, projac_adata); // evaluate dQ/da, dQ/db in pAij, pBij
}
}
}
/* Given a parameter vector p made up of the parameters of m cameras, compute in jac the jacobian
* of the predicted measurements, i.e. the jacobian of the projections of 3D points in the m images.
* The jacobian is approximated with the aid of finite differences and is returned in the order
* (A_11, ..., A_1m, ..., A_n1, ..., A_nm), where A_ij=dx_ij/da_j (see HZ).
* Notice that depending on idxij, some of the A_ij might be missing
*
* Problem-specific information is assumed to be stored in a structure pointed to by "dat".
*
* NOTE: This function is provided mainly for illustration purposes; in case that execution time is a concern,
* the jacobian should be computed analytically
*/
static void sba_mot_Qs_fdjac(
double *p, /* I: current parameter estimate, (m*cnp)x1 */
struct sba_crsm *idxij, /* I: sparse matrix containing the location of x_ij in hx */
int *rcidxs, /* work array for the indexes of nonzero elements of a single sparse matrix row/column */
int *rcsubs, /* work array for the subscripts of nonzero elements in a single sparse matrix row/column */
double *jac, /* O: array for storing the approximated jacobian */
void *dat) /* I: points to a "wrap_mot_data_" structure */
{
register int i, j, ii, jj;
double *paj;
register double *pA;
//int n;
int m, nnz, Asz;
double tmp;
register double d, d1;
struct wrap_mot_data_ *fdjd;
void (*proj)(int j, int i, double *aj, double *xij, void *adata);
double *hxij, *hxxij;
int cnp, mnp;
void *adata;
/* retrieve problem-specific information passed in *dat */
fdjd=(struct wrap_mot_data_ *)dat;
proj=fdjd->proj;
cnp=fdjd->cnp; mnp=fdjd->mnp;
adata=fdjd->adata;
//n=idxij->nr;
m=idxij->nc;
Asz=mnp*cnp;
/* allocate memory for hxij, hxxij */
if((hxij=malloc(2*mnp*sizeof(double)))==NULL){
fprintf(stderr, "memory allocation request failed in sba_mot_Qs_fdjac()!\n");
exit(1);
}
hxxij=hxij+mnp;
/* compute A_ij */
for(j=0; j<m; ++j){
paj=p+j*cnp; // j-th camera parameters
nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); /* find nonzero A_ij, i=0...n-1 */
for(jj=0; jj<cnp; ++jj){
/* determine d=max(SBA_DELTA_SCALE*|paj[jj]|, SBA_MIN_DELTA), see HZ */
d=(double)(SBA_DELTA_SCALE)*paj[jj]; // force evaluation
d=FABS(d);
if(d<SBA_MIN_DELTA) d=SBA_MIN_DELTA;
d1=1.0/d; /* invert so that divisions can be carried out faster as multiplications */
for(i=0; i<nnz; ++i){
(*proj)(j, rcsubs[i], paj, hxij, adata); // evaluate supplied function on current solution
tmp=paj[jj];
paj[jj]+=d;
(*proj)(j, rcsubs[i], paj, hxxij, adata);
paj[jj]=tmp; /* restore */
pA=jac + idxij->val[rcidxs[i]]*Asz; // set pA to point to A_ij
for(ii=0; ii<mnp; ++ii)
pA[ii*cnp+jj]=(hxxij[ii]-hxij[ii])*d1;
}
}
}
free(hxij);
}
