Skip to content
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

Jump to bottom

Get camera intrinsics from camera_info topic #26

Open
wants to merge 2 commits into
base: master
Choose a base branch
from
Open

Get camera intrinsics from camera_info topic #26

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
1f3be67
Load depth img intr params from camera_info topic
schornakj Mar 10, 2020
89c9943
Reduce waitForMessage timeout to 1s
schornakj Mar 10, 2020
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
6 changes: 2 additions & 4 deletions yak_ros/launch/demo.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,12 +8,10 @@


<node name="tsdf_node" pkg="yak_ros" type="yak_ros_node">
<remap from="~input_depth_image" to="/image"/>
<remap from="~input_depth_image/image" to="/sim_depth_camera/image"/>
<remap from="~input_depth_image/camera_info" to="/sim_depth_camera/camera_info"/>

<param name="tsdf_frame" value="tsdf_origin"/>
<rosparam param="camera_matrix">[550.0, 0.0, 320.0, 0.0, 550.0, 240.0, 0.0, 0.0, 1.0]</rosparam>
<param name="cols" value="640"/>
<param name="rows" value="480"/>

<param name="volume_resolution" value="0.001"/>
<param name="volume_x" value="640"/>
Expand Down
31 changes: 24 additions & 7 deletions yak_ros/src/demo/sim_depth_image_pub_ros1.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@

#include <chrono>

const static std::string DEFAULT_IMAGE_TOPIC = "image";
const static std::string DEFAULT_IMAGE_TOPIC = "sim_depth_camera/image";

static Eigen::Affine3d lookat(const Eigen::Vector3d& origin, const Eigen::Vector3d& eye, const Eigen::Vector3d& up)
{
Expand All @@ -34,7 +34,7 @@ int main(int argc, char** argv)

// Setup ROS interfaces
image_transport::ImageTransport it(nh);
image_transport::Publisher image_pub = it.advertise(DEFAULT_IMAGE_TOPIC, 1);
image_transport::CameraPublisher image_pub = it.advertiseCamera(DEFAULT_IMAGE_TOPIC, 1);

tf2_ros::TransformBroadcaster broadcaster;

Expand Down Expand Up @@ -69,15 +69,31 @@ int main(int argc, char** argv)
}

gl_depth_sim::CameraProperties props;
props.width = width;
props.height = height;
props.fx = focal_length;
props.fy = focal_length;
props.width = static_cast<int>(width);
props.height = static_cast<int>(height);
props.fx = static_cast<float>(focal_length);
props.fy = static_cast<float>(focal_length);
props.cx = props.width / 2;
props.cy = props.height / 2;
props.z_near = 0.25;
props.z_far = 10.0f;

sensor_msgs::CameraInfo::Ptr camera_info(new sensor_msgs::CameraInfo());
camera_info->width = static_cast<unsigned int>(width);
camera_info->height = static_cast<unsigned int>(height);
camera_info->distortion_model = "plumb_bob";
camera_info->D = std::vector<double>(5, 0.0);

camera_info->K[0] = static_cast<double>(props.fx);
camera_info->K[1] = 0.0;
camera_info->K[2] = static_cast<double>(props.cx);
camera_info->K[3] = 0.0;
camera_info->K[4] = static_cast<double>(props.fy);
camera_info->K[5] = static_cast<double>(props.cy);
camera_info->K[6] = 0.0;
camera_info->K[7] = 0.0;
camera_info->K[8] = 1.0;

// Create the simulation
gl_depth_sim::SimDepthCamera sim (props);
sim.add(*mesh_ptr, Eigen::Affine3d::Identity());
Expand Down Expand Up @@ -117,7 +133,8 @@ int main(int argc, char** argv)
image.header.frame_id = camera_frame;
image.encoding = sensor_msgs::image_encodings::TYPE_16UC1;
image.image = cv_img;
image_pub.publish(image.toImageMsg());
camera_info->header = image.header;
image_pub.publish(image.toImageMsg(), camera_info);

geometry_msgs::TransformStamped transform_stamped;
transform_stamped.transform.translation.x = pose.translation().x();
Expand Down
48 changes: 35 additions & 13 deletions yak_ros/src/yak_ros1_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@
#include <pcl/common/transforms.h>

#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/CameraInfo.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_eigen/tf2_eigen.h>
#include <cv_bridge/cv_bridge.h>
Expand All @@ -22,6 +23,7 @@
using namespace yak_ros;

static const std::double_t DEFAULT_MINIMUM_TRANSLATION = 0.00001;
static const std::string DEFAULT_DEPTH_IMAGE_TOPIC = "input_depth_image";

OnlineFusionServer::OnlineFusionServer(ros::NodeHandle& nh,
const kfusion::KinFuParams& params,
Expand All @@ -35,7 +37,7 @@ OnlineFusionServer::OnlineFusionServer(ros::NodeHandle& nh,
{
// Subscribe to depth images published on the topic named by the depth_topic param. Set up callback to integrate
// images when received.
depth_image_raw_sub_ = nh.subscribe("input_depth_image", 1, &OnlineFusionServer::onReceivedDepthImg, this);
depth_image_raw_sub_ = nh.subscribe(DEFAULT_DEPTH_IMAGE_TOPIC + "/image", 1, &OnlineFusionServer::onReceivedDepthImg, this);

// Subscribe to point cloud
point_cloud_sub_ = nh.subscribe("input_point_cloud", 1, &OnlineFusionServer::onReceivedPointCloud, this);
Expand Down Expand Up @@ -281,18 +283,38 @@ int main(int argc, char** argv)

kfusion::KinFuParams kinfu_params = kfusion::KinFuParams::default_params();

// Size of the input image
pnh.param<int>("cols", kinfu_params.cols, 640);
pnh.param<int>("rows", kinfu_params.rows, 480);

// Get camera intrinsics from params
XmlRpc::XmlRpcValue camera_matrix;
pnh.getParam("camera_matrix", camera_matrix);
kinfu_params.intr.fx = static_cast<float>(static_cast<double>(camera_matrix[0]));
kinfu_params.intr.fy = static_cast<float>(static_cast<double>(camera_matrix[4]));
kinfu_params.intr.cx = static_cast<float>(static_cast<double>(camera_matrix[2]));
kinfu_params.intr.cy = static_cast<float>(static_cast<double>(camera_matrix[5]));
ROS_INFO("Camera Intr Params: %f %f %f %f\n",


std::string camera_info_topic = pnh.getNamespace() + "/" + DEFAULT_DEPTH_IMAGE_TOPIC + "/camera_info";
ROS_DEBUG("Looking for CameraInfo msgs on topic %s", camera_info_topic.c_str());
auto camera_info = ros::topic::waitForMessage<sensor_msgs::CameraInfo>(camera_info_topic, ros::Duration(1.0));
if (camera_info == nullptr)
{
ROS_WARN("Failed to find camera info topic. Loading intrinsics from parameters instead.");
// Get camera intrinsics from params
XmlRpc::XmlRpcValue camera_matrix;
pnh.getParam("camera_matrix", camera_matrix);
kinfu_params.intr.fx = static_cast<float>(static_cast<double>(camera_matrix[0]));
kinfu_params.intr.fy = static_cast<float>(static_cast<double>(camera_matrix[4]));
kinfu_params.intr.cx = static_cast<float>(static_cast<double>(camera_matrix[2]));
kinfu_params.intr.cy = static_cast<float>(static_cast<double>(camera_matrix[5]));

pnh.param<int>("cols", kinfu_params.cols, 640);
pnh.param<int>("rows", kinfu_params.rows, 480);
}
else
{
ROS_DEBUG("Found CameraInfo msg");
kinfu_params.intr.fx = static_cast<float>(camera_info->K[0]);
kinfu_params.intr.fy = static_cast<float>(camera_info->K[4]);
kinfu_params.intr.cx = static_cast<float>(camera_info->K[2]);
kinfu_params.intr.cy = static_cast<float>(camera_info->K[5]);

kinfu_params.cols = static_cast<int>(camera_info->width);
kinfu_params.rows = static_cast<int>(camera_info->height);
}

ROS_INFO("Camera Intrinsic Params: %f %f %f %f\n",
static_cast<double>(kinfu_params.intr.fx),
static_cast<double>(kinfu_params.intr.fy),
static_cast<double>(kinfu_params.intr.cx),
Expand Down