void CameraCalibration::SetScale( const cv::Size& scale )
{
double xRatio = ( (double) scale.width ) / origInfo.width;
double yRatio = ( (double) scale.height ) / origInfo.height;
cameraInfo.width = scale.width;
cameraInfo.height = scale.height;
GetFx( cameraInfo ) = GetFx( origInfo ) * xRatio;
GetCx( cameraInfo ) = GetCx( origInfo ) * xRatio;
GetFy( cameraInfo ) = GetFy( origInfo ) * yRatio;
GetCy( cameraInfo ) = GetCy( origInfo ) * yRatio;
currentScale = scale;
ReadIntrinsicMatrix( cameraInfo );
}
void CameraCalibration::ReadIntrinsicMatrix( const sensor_msgs::CameraInfo& info )
{
intrinsicMatrix = cv::Matx33d::eye();
intrinsicMatrix(0,0) = GetFx( info );
intrinsicMatrix(1,1) = GetFy( info );
intrinsicMatrix(0,2) = GetCx( info );
intrinsicMatrix(1,2) = GetCy( info );
}
wxSVGRect wxSVGEllipseElement::GetBBox(wxSVG_COORDINATES coordinates) {
if (coordinates == wxSVG_COORDINATES_USER)
return wxSVGRect(GetCx().GetBaseVal() - GetRx().GetBaseVal(), GetCy().GetBaseVal() - GetRy().GetBaseVal(),
2 * GetRx().GetBaseVal(), 2 * GetRy().GetBaseVal());
wxSVGMatrix matrix = GetMatrix(coordinates);
double angles[4];
angles[0] = atan((GetRy().GetBaseVal() * matrix.GetC()) / (GetRx().GetBaseVal() * matrix.GetA()));
angles[1] = atan((GetRy().GetBaseVal() * matrix.GetD()) / (GetRx().GetBaseVal() * matrix.GetB()));
angles[2] = angles[0] + pi;
angles[3] = angles[1] + pi;
wxSVGPointList points = wxSVGPointList();
for (int i = 0; i < 4; i++) {
points.Add(wxSVGPoint(GetRx().GetBaseVal() * cos(angles[i]) + GetCx().GetBaseVal(),
GetRy().GetBaseVal() * sin(angles[i]) + GetCy().GetBaseVal()));
}
wxSVGPoint p0 = points[0].MatrixTransform(matrix);
wxSVGRect bbox(p0.GetX(), p0.GetY(), 0, 0);
wxSVGPoint pi = wxSVGPoint();
for (int i = 1; i < (int) points.Count(); i++) {
pi = points[i].MatrixTransform(matrix);
if (bbox.GetX() > pi.GetX()) {
bbox.SetWidth(bbox.GetWidth() + bbox.GetX() - pi.GetX());
bbox.SetX(pi.GetX());
}
if (bbox.GetY() > pi.GetY()) {
bbox.SetHeight(bbox.GetHeight() + bbox.GetY() - pi.GetY());
bbox.SetY(pi.GetY());
}
if (bbox.GetX() + bbox.GetWidth() < pi.GetX())
bbox.SetWidth(pi.GetX() - bbox.GetX());
if (bbox.GetY() + bbox.GetHeight() < pi.GetY())
bbox.SetHeight(pi.GetY() - bbox.GetY());
}
return bbox;
}
CameraCalibration::CameraCalibration( const std::string& name, double fx, double fy,
double px, double py, double w, double h )
: cameraName( name )
{
GetFx( origInfo ) = fx;
GetFy( origInfo ) = fy;
GetCx( origInfo ) = px;
GetCy( origInfo ) = py;
origInfo.width = w;
origInfo.height = h;
cameraInfo = origInfo;
ReadDistortionCoeffs( origInfo );
SetScale( cv::Size( w, h ) );
}
double CameraCalibration::GetCx() const
{
return GetCx( cameraInfo );
}
