{
for(int i = 0; i < N; i++)
{
- p.at<double>(0,i) = opoints.at<OpointType>(0,i).x;
- p.at<double>(1,i) = opoints.at<OpointType>(0,i).y;
- p.at<double>(2,i) = opoints.at<OpointType>(0,i).z;
+ p.at<double>(0,i) = opoints.at<OpointType>(i).x;
+ p.at<double>(1,i) = opoints.at<OpointType>(i).y;
+ p.at<double>(2,i) = opoints.at<OpointType>(i).z;
// compute mean of object points
mn.at<double>(0) += p.at<double>(0,i);
mn.at<double>(2) += p.at<double>(2,i);
// make z into unit vectors from normalized pixel coords
- double sr = std::pow(ipoints.at<IpointType>(0,i).x, 2) +
- std::pow(ipoints.at<IpointType>(0,i).y, 2) + (double)1;
+ double sr = std::pow(ipoints.at<IpointType>(i).x, 2) +
+ std::pow(ipoints.at<IpointType>(i).y, 2) + (double)1;
sr = std::sqrt(sr);
- z.at<double>(0,i) = ipoints.at<IpointType>(0,i).x / sr;
- z.at<double>(1,i) = ipoints.at<IpointType>(0,i).y / sr;
+ z.at<double>(0,i) = ipoints.at<IpointType>(i).x / sr;
+ z.at<double>(1,i) = ipoints.at<IpointType>(i).y / sr;
z.at<double>(2,i) = (double)1 / sr;
}
{
for(int i = 0; i < number_of_correspondences; i++)
{
- pws[3 * i ] = opoints.at<OpointType>(0,i).x;
- pws[3 * i + 1] = opoints.at<OpointType>(0,i).y;
- pws[3 * i + 2] = opoints.at<OpointType>(0,i).z;
+ pws[3 * i ] = opoints.at<OpointType>(i).x;
+ pws[3 * i + 1] = opoints.at<OpointType>(i).y;
+ pws[3 * i + 2] = opoints.at<OpointType>(i).z;
- us[2 * i ] = ipoints.at<IpointType>(0,i).x*fu + uc;
- us[2 * i + 1] = ipoints.at<IpointType>(0,i).y*fv + vc;
+ us[2 * i ] = ipoints.at<IpointType>(i).x*fu + uc;
+ us[2 * i + 1] = ipoints.at<IpointType>(i).y*fv + vc;
}
}
double reprojection_error(const double R[3][3], const double t[3]);
points.resize(20);
for(int i = 0; i < 4; i++)
{
- points[i*5] = ipoints.at<IpointType>(0,i).x*fx + cx;
- points[i*5+1] = ipoints.at<IpointType>(0,i).y*fy + cy;
- points[i*5+2] = opoints.at<OpointType>(0,i).x;
- points[i*5+3] = opoints.at<OpointType>(0,i).y;
- points[i*5+4] = opoints.at<OpointType>(0,i).z;
+ points[i*5] = ipoints.at<IpointType>(i).x*fx + cx;
+ points[i*5+1] = ipoints.at<IpointType>(i).y*fy + cy;
+ points[i*5+2] = opoints.at<OpointType>(i).x;
+ points[i*5+3] = opoints.at<OpointType>(i).y;
+ points[i*5+4] = opoints.at<OpointType>(i).z;
}
}
void init_inverse_parameters();