Calculates position of the kinect. More...
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <image_geometry/pinhole_camera_model.h>
#include <boost/thread/mutex.hpp>
#include <boost/lexical_cast.hpp>
#include <pcl/point_types.h>
#include <pcl_visualization/pcl_visualizer.h>
#include <pcl/common/transformation_from_correspondences.h>
#include <pcl/registration/transforms.h>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/extract_indices.h>
#include <terminal_tools/parse.h>
#include <Eigen/Core>
#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <cv_bridge/CvBridge.h>
#include <ground_truth/field_provider.h>
Go to the source code of this file.
Enumerations | |
enum | State |
Functions | |
void | calculateGroundPlane () |
Calculates the ground plane based on user entered points. | |
void | calculateTransformation () |
Calculates the transformation (i.e. location of the kinect sensor) based on known positions of landmarks. | |
void | cloudCallback (const sensor_msgs::PointCloud2ConstPtr &cloudPtrMsg) |
Callback function for the point cloud message received from the kinect driver. | |
void | collectRayInfo (int x, int y) |
Collects information about the ray in the kinect's frame of reference based on pixel indicated by the user. | |
void | displayCloud (pcl_visualization::PCLVisualizer &visualizer, pcl::PointCloud< pcl::PointXYZRGB > &cloudToDisplay) |
Simple function to display the point cloud in the visualizer. | |
void | displayStatus (const char *format,...) |
Displays the status on the visualizer window. | |
float | distanceLineFromPoint (Eigen::Vector3f ep1, Eigen::Vector3f ep2, Eigen::Vector3f point) |
Helper function to calculate the distance of a point from a ray. | |
bool | getPointFromCloud (Eigen::Vector3f &point) |
Returns a point directly sampled from the pointcloud. | |
Eigen::Vector3f | getPointFromGroundPlane () |
Obtains a point by intersecting the ray entered by the user with the ground plane. | |
std::string | getUniqueName (const std::string &baseName, int uniqueId) |
void | imageCallback (const sensor_msgs::ImageConstPtr &image, const sensor_msgs::CameraInfoConstPtr &camInfo) |
Callback function for the image message being received from the kinect driver. | |
void | imageMouseCallback (int event, int x, int y, int flags, void *param) |
The mouse callback on the camera image being displayed by the Kinect. | |
int | main (int argc, char **argv) |
template<typename T > | |
void | noDelete (T *ptr) |
Helper function to attach static object to boost::shared_ptr. Used for efficiency. |
Calculates position of the kinect.
This ROS node calculates the position and orientation of the kinect sensor in the field coordinate system. This information is stored to file and read by the detection system
License: Modified BSD License
$ Id: 08/04/2011 02:18:43 PM piyushk $
Definition in file calibrate.cc.
enum State |
Definition at line 83 of file calibrate.cc.
void calculateGroundPlane | ( | ) |
Calculates the ground plane based on user entered points.
Definition at line 169 of file calibrate.cc.
void calculateTransformation | ( | ) |
Calculates the transformation (i.e. location of the kinect sensor) based on known positions of landmarks.
Definition at line 145 of file calibrate.cc.
void cloudCallback | ( | const sensor_msgs::PointCloud2ConstPtr & | cloudPtrMsg | ) |
Callback function for the point cloud message received from the kinect driver.
Definition at line 383 of file calibrate.cc.
void collectRayInfo | ( | int | x, |
int | y | ||
) |
Collects information about the ray in the kinect's frame of reference based on pixel indicated by the user.
Definition at line 223 of file calibrate.cc.
void displayCloud | ( | pcl_visualization::PCLVisualizer & | visualizer, |
pcl::PointCloud< pcl::PointXYZRGB > & | cloudToDisplay | ||
) |
Simple function to display the point cloud in the visualizer.
Definition at line 114 of file calibrate.cc.
void displayStatus | ( | const char * | format, |
... | |||
) |
Displays the status on the visualizer window.
Definition at line 208 of file calibrate.cc.
float distanceLineFromPoint | ( | Eigen::Vector3f | ep1, |
Eigen::Vector3f | ep2, | ||
Eigen::Vector3f | point | ||
) |
Helper function to calculate the distance of a point from a ray.
This is usefull in obtaining information about a point entered by the user from the point cloud itself
Definition at line 236 of file calibrate.cc.
bool getPointFromCloud | ( | Eigen::Vector3f & | point | ) |
Returns a point directly sampled from the pointcloud.
point | A reference to the object through which the sampled value is returned |
Definition at line 245 of file calibrate.cc.
Eigen::Vector3f getPointFromGroundPlane | ( | ) |
Obtains a point by intersecting the ray entered by the user with the ground plane.
Definition at line 191 of file calibrate.cc.
std::string getUniqueName | ( | const std::string & | baseName, |
int | uniqueId | ||
) | [inline] |
/brief Helper function for attaching a unique id to a string. /return the string with the unique identifier
Definition at line 100 of file calibrate.cc.
void imageCallback | ( | const sensor_msgs::ImageConstPtr & | image, |
const sensor_msgs::CameraInfoConstPtr & | camInfo | ||
) |
Callback function for the image message being received from the kinect driver.
Definition at line 371 of file calibrate.cc.
void imageMouseCallback | ( | int | event, |
int | x, | ||
int | y, | ||
int | flags, | ||
void * | param | ||
) |
The mouse callback on the camera image being displayed by the Kinect.
Mouse Events are used to collect information about the ground points and landmarks being indicated by the user.
Definition at line 280 of file calibrate.cc.
int main | ( | int | argc, |
char ** | argv | ||
) |
Definition at line 390 of file calibrate.cc.
void noDelete | ( | T * | ptr | ) |
Helper function to attach static object to boost::shared_ptr. Used for efficiency.
Definition at line 108 of file calibrate.cc.