Skip to content

stereo_undistort: accept output_camera_info_source parameter #53

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
protobits wants to merge 1 commit into
base: master
Choose a base branch
from
Open

stereo_undistort: accept output_camera_info_source parameter #53

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 3 additions & 2 deletions include/image_undistort/camera_parameters.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -172,7 +172,7 @@ class CameraParametersPair {
// rectified images
class StereoCameraParameters {
public:
StereoCameraParameters(const double scale = 1.0);
StereoCameraParameters(const double scale = 1.0, const bool match_input = false);

bool setInputCameraParameters(const ros::NodeHandle& nh,
const std::string& camera_namespace,
Expand All @@ -198,9 +198,10 @@ class StereoCameraParameters {
bool valid(const CameraSide& side, const CameraIO& io) const;

private:
bool generateRectificationParameters();
bool generateRectificationParameters(void);
Copy link
Contributor

@ZacharyTaylor ZacharyTaylor May 28, 2019

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

why add void? From memory this style of syntax, while valid only makes a difference in old C code

Copy link
Author

@protobits protobits May 28, 2019

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

no reason, just a habbit. you can discard this change if you want


double scale_;
double match_input_;
CameraParametersPair first_;
CameraParametersPair second_;
};
Expand Down
14 changes: 14 additions & 0 deletions include/image_undistort/stereo_undistort.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,14 @@ constexpr int kQueueSize = 10;
// true to load input cam_info from ros parameters, false to get it from a
// cam_info topic
constexpr bool kDefaultInputCameraInfoFromROSParams = true;
// source of output camera information, the options are as follows-
// "auto_generated": (default), automatically generated based on the input, the
// focal length is the average of fx and fy of the input, the center point is
// in the center of the image, R=I and only x-translation is preserved.
// Resolution is set to the largest area that contains no empty pixels. The
// size of the output can also be modified with the "scale" parameter
// "match_input": intrinsic values are taken from the input camera parameters
Copy link
Contributor

@ZacharyTaylor ZacharyTaylor May 28, 2019

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Is taking the exact input intrinsics desirable for a stereo pair? At the very least I think you will have to ensure that both cameras are rectified using the same focal length or you will probably get pretty poor stereo depth. One possible solution could be to arbitrarily pick one camera and use its intrinsics to set the output for both cameras.

Copy link
Author

@protobits protobits May 28, 2019

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

this is a copy-paste of the documentation of the parameter from the other header. the step which obtains an equal fov from both cameras and so on is still performed. my change mostly sets the resolution the same as the input. otherwise, the current code produces an image which can become much larger (particulary, wider) than the input, due to how the undistortion works. this leads to wasted computation if this bigger image is processed. this is specially important for embedded image processing. moreover, this produces an output similar to what you obtain with opencv and is what most users expect (see #50).

in any case, I've only added an option and the default is the same, so I doesn't hurt.

Copy link
Contributor

@ZacharyTaylor ZacharyTaylor May 29, 2019

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I just went back and checked, your using the function setOutputFromInput, which sets all intrinsic and extrinsic camera parameters to match the input, not just the image size.

The best approach is probably the creation of a new function that just fixes the focal length, updates image size and moves the principle to the center.

const std::string kDefaultOutputCameraInfoSource = "auto_generated";
// namespace to use when loading first camera parameters from ros params
const std::string kDefaultFirstCameraNamespace = "first_camera";
// namespace to use when loading second camera parameters from ros params
Expand Down Expand Up @@ -124,6 +132,11 @@ class StereoUndistort {
// camera parameters
std::shared_ptr<StereoCameraParameters> stereo_camera_parameters_ptr_;

enum class OutputInfoSource {
AUTO_GENERATED,
MATCH_INPUT,
};

// undistorters
std::shared_ptr<Undistorter> first_undistorter_ptr_;
std::shared_ptr<Undistorter> second_undistorter_ptr_;
Expand All @@ -133,6 +146,7 @@ class StereoUndistort {
int queue_size_;
int process_every_nth_frame_;
std::string output_image_type_;
OutputInfoSource output_camera_info_source_;
bool publish_tf_;
std::string first_output_frame_;
std::string second_output_frame_;
Expand Down
28 changes: 19 additions & 9 deletions src/camera_parameters.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -545,8 +545,8 @@ bool CameraParametersPair::operator!=(const CameraParametersPair& B) const {
return !(*this == B);
}

StereoCameraParameters::StereoCameraParameters(const double scale)
: scale_(scale){};
StereoCameraParameters::StereoCameraParameters(const double scale, const bool match_input)
: scale_(scale), match_input_(match_input){};

bool StereoCameraParameters::setInputCameraParameters(
const ros::NodeHandle& nh, const std::string& camera_namespace,
Expand Down Expand Up @@ -628,7 +628,7 @@ bool StereoCameraParameters::valid(const CameraSide& side,
}
}

bool StereoCameraParameters::generateRectificationParameters() {
bool StereoCameraParameters::generateRectificationParameters(void) {
if (first_.getInputPtr()->p().isApprox(second_.getInputPtr()->p())) {
ROS_ERROR(
"Stereo rectification cannot be performed on cameras with a baseline "
Expand Down Expand Up @@ -661,11 +661,21 @@ bool StereoCameraParameters::generateRectificationParameters() {
T.inverse() * second_.getInputPtr()->T(), second_.getInputPtr()->K(),
second_.getInputPtr()->D(), second_.getInputPtr()->distortionModel());

// set individual outputs
if (!first_.setOptimalOutputCameraParameters(scale_) ||
!second_.setOptimalOutputCameraParameters(scale_)) {
ROS_ERROR("Automatic generation of stereo output parameters failed");
return false;

if (match_input_) {
if (!first_.setOutputFromInput(scale_) ||
!second_.setOutputFromInput(scale_)) {
ROS_ERROR("Automatic generation of stereo output parameters failed");
return false;
}
}
else {
// set individual outputs
if (!first_.setOptimalOutputCameraParameters(scale_) ||
!second_.setOptimalOutputCameraParameters(scale_)) {
ROS_ERROR("Automatic generation of stereo output parameters failed");
return false;
}
}

// grab most conservative values
Expand Down Expand Up @@ -701,4 +711,4 @@ const CameraParametersPair& StereoCameraParameters::getFirst() const {
const CameraParametersPair& StereoCameraParameters::getSecond() const {
return second_;
}
} // namespace image_undistort
} // namespace image_undistort
16 changes: 15 additions & 1 deletion src/stereo_undistort.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,20 @@ StereoUndistort::StereoUndistort(const ros::NodeHandle& nh,
nh_private_.param("rename_radtan_plumb_bob", rename_radtan_plumb_bob_,
kDefaultRenameRadtanPlumbBob);

std::string output_camera_info_source_in;
nh_private_.param("output_camera_info_source", output_camera_info_source_in,
kDefaultOutputCameraInfoSource);
if (output_camera_info_source_in == "auto_generated") {
output_camera_info_source_ = OutputInfoSource::AUTO_GENERATED;
} else if (output_camera_info_source_in == "match_input") {
output_camera_info_source_ = OutputInfoSource::MATCH_INPUT;
} else {
ROS_ERROR(
"Invalid camera source given, valid options are auto_generated, "
"match_input. Defaulting to auto_generated");
output_camera_info_source_ = OutputInfoSource::AUTO_GENERATED;
}

bool invert_T;
nh_private_.param("invert_T", invert_T, kDefaultInvertT);

Expand All @@ -46,7 +60,7 @@ StereoUndistort::StereoUndistort(const ros::NodeHandle& nh,
nh_private_.param("scale", scale, kDefaultScale);

stereo_camera_parameters_ptr_ =
std::make_shared<StereoCameraParameters>(scale);
std::make_shared<StereoCameraParameters>(scale, (output_camera_info_source_ == OutputInfoSource::MATCH_INPUT));

nh_private_.param("process_every_nth_frame", process_every_nth_frame_,
kDefaultProcessEveryNthFrame);
Expand Down