void fi::VPDetectionWrapper::leastQuaresVPFitting(const std::vector<Eigen::Vector3f> &vp_inliers, Eigen::VectorXf &optimized_vp)
{
unsigned int num_in_liers = vp_inliers.size();
//change to double
//std::vector<Eigen::Vector3d> vp_double(num_in_liers);
//for ( unsigned int i = 0; i < num_in_liers; i++)
//{
// Eigen::Vector3f a_vp = vp_inliers[i];
// vp_double[i](0) = a_vp(0);
// vp_double[i](1) = a_vp(1);
// vp_double[i](2) = a_vp(2);
//}
Eigen::VectorXd opt_double_val;
opt_double_val.resize(6);
//porpulate the vps in a cloud just to make things a bit h\E4ndy
pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud(new pcl::PointCloud<pcl::PointXYZ>());
point_cloud->height = 1;
point_cloud->width = num_in_liers;
point_cloud->resize(point_cloud->height * point_cloud->width);
for (unsigned int i = 0; i < num_in_liers; i++)
{
Eigen::Vector3f n_tmp = vp_inliers.at(i);
point_cloud->points[i].x = n_tmp(0);
point_cloud->points[i].y = n_tmp(1);
point_cloud->points[i].z = n_tmp(2);
}
optimized_vp.resize (6);
// Compute the 3x3 covariance matrix
Eigen::Vector4d centroid;
//pcl::compute3DCentroid (*point_cloud, centroid);
Eigen::Matrix3d covariance_matrix;
//pcl::computeCovarianceMatrix(*input_, inliers, centroid, covariance_matrix);
//pcl::computeCovarianceMatrix(*point_cloud, centroid, covariance_matrix);
pcl::computeMeanAndCovarianceMatrix(*point_cloud, covariance_matrix, centroid);
optimized_vp[0] = centroid[0];
optimized_vp[1] = centroid[1];
optimized_vp[2] = centroid[2];
opt_double_val(0) = centroid[0];
opt_double_val(1) = centroid[1];
opt_double_val(2) = centroid[2];
// Extract the eigenvalues and eigenvectors
EIGEN_ALIGN16 Eigen::Vector3d eigen_values;
EIGEN_ALIGN16 Eigen::Matrix3d eigen_vectors;
pcl::eigen33(covariance_matrix, eigen_vectors, eigen_values);
//optimized_vp.tail<3> () = eigen_vectors.col (0).normalized ();
opt_double_val.tail<3> () = eigen_vectors.col (0).normalized ();
optimized_vp[3] = static_cast<float> (opt_double_val[3]);
optimized_vp[4] = static_cast<float> (opt_double_val[4]);
optimized_vp[5] = static_cast<float> (opt_double_val[5]);
}
void TerrainClassifierNode::setPointCloud(const sensor_msgs::PointCloud2& point_cloud_msg)
{
std::string world_frame_id = vigir_footstep_planning::strip_const(terrain_classifier.getFrameId(), '/');
std::string cloud_frame_id = vigir_footstep_planning::strip_const(point_cloud_msg.header.frame_id, '/');
if (world_frame_id != cloud_frame_id)
{
ROS_ERROR_THROTTLE(5.0, "[TerrainClassifierNode] setPointCloud: Frame of input point ('%s') cloud mismatch! Should be '%s'.", cloud_frame_id.c_str(), world_frame_id.c_str());
return;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud(new pcl::PointCloud<pcl::PointXYZ>());
pcl::fromROSMsg(point_cloud_msg, *point_cloud);
// filter input
filterVoxelGrid<pcl::PointXYZ>(point_cloud, 0.02, 0.02, 2.0);
// filtering of input point cloud
terrain_classifier.filterPointCloudData(point_cloud);
terrain_classifier.setPointCloud(point_cloud);
generateTerrainModel();
publishDebugData();
publishTerrainModel();
//ROS_INFO("Saved");
//pcl::io::savePCDFile("/home/alex/vigir/catkin_ws/vigir_footstep_planning/vigir_terrain_classifier/pointclouds/new.pcd", cloud_input);
}
int
process(const tendrils& inputs, const tendrils& outputs)
{
CloudType::Ptr point_cloud(new CloudType);
cvToCloud(*points3d, *point_cloud);
*cloud_out = point_cloud;
return ecto::OK;
}
void SuperFrameParser::fillPointCloudMsg ()
{
pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
convertImageToPointCloud (depth_msgs_, point_cloud);
pcl::toROSMsg (*point_cloud, *point_cloud_msgs_);
point_cloud_msgs_->header.frame_id = depth_msgs_->header.frame_id;
point_cloud_msgs_->header.stamp = depth_msgs_->header.stamp;
point_cloud_msgs_->row_step = point_cloud_msgs_->width * 2;
}
void callback(PointCloud points) {
cv::Mat point_cloud(points.height, points.width, CV_8U, points.data);
cv_bridge::CvImage msg;
msg.header.seq = seq_id;
msg.header.frame_id = seq_id;
msg.header.stamp = ros::Time::now();
msg.encoding = sensor_msgs::image_encodings::MONO8;
msg.image = point_cloud;
points_pub.publish(msg.toImageMsg());
ROS_INFO("[stereo_publisher]: Lag - %u", time(0) - points.stamp);
seq_id++;
}
void TerrainClassifierNode::insertPointCloud(const sensor_msgs::PointCloud2& point_cloud_msg)
{
std::string world_frame_id = vigir_footstep_planning::strip_const(terrain_classifier.getFrameId(), '/');
std::string cloud_frame_id = vigir_footstep_planning::strip_const(point_cloud_msg.header.frame_id, '/');
if (world_frame_id != cloud_frame_id)
{
ROS_ERROR_THROTTLE(5.0, "[TerrainClassifierNode] insertPointCloud: Frame of input point ('%s') cloud mismatch! Should be '%s'.", cloud_frame_id.c_str(), world_frame_id.c_str());
return;
}
if (point_cloud_msg.data.empty())
return;
pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud(new pcl::PointCloud<pcl::PointXYZ>());
pcl::fromROSMsg(point_cloud_msg, *point_cloud);
// filtering of input point cloud
terrain_classifier.filterPointCloudData(point_cloud);
terrain_classifier.insertPointCloud(point_cloud);
if (terrain_classifier.getInputCloud()->size() <= min_aggregation_size)
{
if (cloud_input_pub.getNumSubscribers() > 0)
{
sensor_msgs::PointCloud2 cloud_point_msg;
pcl::toROSMsg(*(terrain_classifier.getInputCloud()), cloud_point_msg);
cloud_point_msg.header.stamp = ros::Time::now();
cloud_point_msg.header.frame_id = terrain_classifier.getFrameId();
cloud_input_pub.publish(cloud_point_msg);
}
return;
}
if (compute_update_skips_counter++ >= compute_update_skips)
{
terrain_classifier.computeNormals(point_cloud);
compute_update_skips_counter = 0;
}
terrain_classifier.generateHeightGridmap(point_cloud);
publishDebugData();
//ROS_INFO("Saved");
//pcl::io::savePCDFile("/home/alex/vigir/catkin_ws/vigir_footstep_planning/vigir_terrain_classifier/pointclouds/new.pcd", cloud_input);
}
void keypoint_map::publish_pointclouds(RmOctomapServer::Ptr & server, const std::string & frame_prefix) {
for (size_t i = 0; i < depth_imgs.size(); i++) {
pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud(
new pcl::PointCloud<pcl::PointXYZ>);
for (int v = 0; v < depth_imgs[i].rows; v++) {
for (int u = 0; u < depth_imgs[i].cols; u++) {
if (depth_imgs[i].at<unsigned short>(v, u) != 0) {
pcl::PointXYZ p;
p.z = depth_imgs[i].at<unsigned short>(v, u) / 1000.0f;
p.x = (u - intrinsics[2]) * p.z / intrinsics[0];
p.y = (v - intrinsics[3]) * p.z / intrinsics[1];
//cv::Vec3b brg = rgb_imgs[i].at<cv::Vec3b>(v, u);
//p.r = brg[2];
//p.g = brg[1];
//p.b = brg[0];
//p.a = 255;
Eigen::Vector4f tmp = camera_positions[i]
* p.getVector4fMap();
if (tmp[2] < 0.6 && tmp[2] > 0.2) {
p.getVector4fMap() = tmp;
point_cloud->push_back(p);
}
}
}
}
point_cloud->sensor_orientation_ = camera_positions[i].rotation();
point_cloud->sensor_origin_ = camera_positions[i].translation().homogeneous();
point_cloud->header.frame_id = frame_prefix + "/map";
point_cloud->header.seq = i;
point_cloud->header.stamp = ros::Time::now();
Eigen::Vector3f pos = camera_positions[i].translation();
server->insertScan(tf::Point(pos[0], pos[1], pos[2]), *point_cloud, pcl::PointCloud<pcl::PointXYZ>());
}
server->publishAll(ros::Time::now());
}
void computeAndPublishPointCloud(
const sensor_msgs::CameraInfoConstPtr& l_info_msg,
const sensor_msgs::ImageConstPtr& l_image_msg,
const sensor_msgs::CameraInfoConstPtr& r_info_msg,
const std::vector<Matcher::p_match>& matches,
const std::vector<int32_t>& inlier_indices)
{
try
{
cv_bridge::CvImageConstPtr cv_ptr;
cv_ptr = cv_bridge::toCvShare(l_image_msg, sensor_msgs::image_encodings::RGB8);
// read calibration info from camera info message
image_geometry::StereoCameraModel model;
model.fromCameraInfo(*l_info_msg, *r_info_msg);
PointCloud::Ptr point_cloud(new PointCloud());
point_cloud->header.frame_id = getSensorFrameId();
point_cloud->header.stamp = l_info_msg->header.stamp;
point_cloud->width = 1;
point_cloud->height = inlier_indices.size();
point_cloud->points.resize(inlier_indices.size());
for (size_t i = 0; i < inlier_indices.size(); ++i)
{
const Matcher::p_match& match = matches[inlier_indices[i]];
cv::Point2d left_uv;
left_uv.x = match.u1c;
left_uv.y = match.v1c;
cv::Point3d point;
double disparity = match.u1c - match.u2c;
model.projectDisparityTo3d(left_uv, disparity, point);
point_cloud->points[i].x = point.x;
point_cloud->points[i].y = point.y;
point_cloud->points[i].z = point.z;
cv::Vec3b color = cv_ptr->image.at<cv::Vec3b>(left_uv.y,left_uv.x);
point_cloud->points[i].r = color[0];
point_cloud->points[i].g = color[1];
point_cloud->points[i].b = color[2];
}
ROS_DEBUG("Publishing point cloud with %zu points.", point_cloud->size());
point_cloud_pub_.publish(point_cloud);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
}
}
void PointsFileSystem::showPointCloud(const QPersistentModelIndex& index)
{
checked_indexes_.insert(index);
osg::ref_ptr<PointCloud> point_cloud(getPointCloud(filePath(index).toStdString()));
if (!point_cloud.valid())
return;
PointCloudMap::iterator point_cloud_map_it = point_cloud_map_.find(index);
if (point_cloud_map_it != point_cloud_map_.end())
return;
MainWindow::getInstance()->getSceneWidget()->addSceneChild(point_cloud);
point_cloud_map_[index] = point_cloud;
return;
}
void TerrainClassifierNode::loadTestPointCloud(const std::string& path)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud(new pcl::PointCloud<pcl::PointXYZ>());
ROS_INFO("Loading point cloud from %s...", path.c_str());
if (pcl::io::loadPCDFile(path, *point_cloud))
{
ROS_ERROR("FAILED!");
return;
}
else
ROS_INFO("Done!");;
sensor_msgs::PointCloud2 point_cloud_msg;
pcl::toROSMsg(*point_cloud, point_cloud_msg);
point_cloud_msg.header.frame_id = terrain_classifier.getFrameId();
point_cloud_msg.header.stamp = ros::Time::now();
ROS_INFO("Generating terrain model...");
setPointCloud(point_cloud_msg);
ROS_INFO("Done!");
}
pcl::PointCloud<pcl::PointXYZ>::Ptr mat2IntegralPointCloud(const cv::Mat& depth_mat, const float focal_length, const float max_depth) {
assert(depth_mat.type() == CV_16U);
pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud(new pcl::PointCloud<pcl::PointXYZ>());
const int half_width = depth_mat.cols / 2;
const int half_height = depth_mat.rows / 2;
const float inv_focal_length = 1.0 / focal_length;
point_cloud->points.reserve(depth_mat.rows * depth_mat.cols);
for (int y = 0; y < depth_mat.rows; y++) {
for (int x = 0; x < depth_mat.cols; x++) {
float z = depth_mat.at<ushort>(cv:: Point(x, y)) * 0.001;
if (z < max_depth && z > 0) {
point_cloud->points.emplace_back(static_cast<float>(x - half_width) * z * inv_focal_length,
static_cast<float>(y - half_height) * z * inv_focal_length,
z);
} else {
point_cloud->points.emplace_back(x, y, NAN);
}
}
}
point_cloud->width = depth_mat.cols;
point_cloud->height = depth_mat.rows;
return point_cloud;
}
void LslidarN301Decoder::publishPointCloud() {
pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
new pcl::PointCloud<pcl::PointXYZI>());
point_cloud->header.stamp =
pcl_conversions::toPCL(sweep_data->header).stamp;
point_cloud->header.frame_id = child_frame_id;
point_cloud->height = 1;
for (size_t i = 0; i < 16; ++i) {
const lslidar_n301_msgs::LslidarN301Scan& scan = sweep_data->scans[i];
// The first and last point in each scan is ignored, which
// seems to be corrupted based on the received data.
// TODO: The two end points should be removed directly
// in the scans.
if (scan.points.size() == 0) continue;
size_t j;
for (j = 1; j < scan.points.size()-1; ++j) {
pcl::PointXYZI point;
point.x = scan.points[j].x;
point.y = scan.points[j].y;
point.z = scan.points[j].z;
point.intensity = scan.points[j].intensity;
point_cloud->points.push_back(point);
++point_cloud->width;
}
}
// if(point_cloud->width > 2000)
{
point_cloud_pub.publish(point_cloud);
}
return;
}
void SceneWidget::slotSampleScan(void) {
MainWindow* main_window = MainWindow::getInstance();
int resolution;
if (!ParameterManager::getInstance().getSampleScanParameters(resolution))
return;
osg::ref_ptr < PointCloud > point_cloud(new PointCloud);
for (size_t i = 0, i_end = mesh_models_.size(); i < i_end; ++i) {
if (i == 0) {
mesh_models_[i]->sampleScan(point_cloud->data(), resolution, 0.0);
} else {
PclPointCloud::Ptr samples(new PclPointCloud);
mesh_models_[i]->sampleScan(samples, resolution, 0.0);
point_cloud->data()->insert(point_cloud->data()->end(), samples->begin(), samples->end());
}
}
removeSceneChild(point_cloud_);
point_cloud_ = point_cloud;
addSceneChild(point_cloud_);
return;
}
// ----------------------------------------------------------------------------
void loadFeatures3d(BoWFeatures &features)
{
typedef pcl::PointXYZ PointType;
float angular_resolution = pcl::deg2rad (0.15f);
float support_size = 0.1f;
features.clear();
features.reserve(files_list_3d.size());
float noise_level = 0.0f;
float min_range = 0.0f;
int border_size = 1;
double acc_media = 0,media=0,scarti=0,varianza=0;
//pcl::visualization::RangeImageVisualizer range_image_widget ("Range image");
pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;
pcl::RangeImage::Ptr range_image_ptr (new pcl::RangeImage);
pcl::RangeImage& range_image = *range_image_ptr;
for (int i = 0; i < files_list_3d.size(); ++i) {
clock_t begin = clock();
pcl::PointCloud<PointType>::Ptr point_cloud_wf (new pcl::PointCloud<PointType>);
pcl::PointCloud<PointType>::Ptr point_cloud (new pcl::PointCloud<PointType>);
pcl::io::loadPCDFile (files_list_3d[i], *point_cloud_wf);
//filtraggio valori NaN
std::vector<int> indices;
pcl::removeNaNFromPointCloud (*point_cloud_wf,*point_cloud_wf,indices);
pcl::VoxelGrid<PointType> sor;
sor.setInputCloud (point_cloud_wf);
sor.setLeafSize (0.01f, 0.01f, 0.01f);
sor.filter (*point_cloud);
cout << "Estrazione NARF: " << files_list_3d[i] ;
Eigen::Affine3f scene_sensor_pose (Eigen::Affine3f::Identity());
scene_sensor_pose = Eigen::Affine3f (Eigen::Translation3f ((*point_cloud).sensor_origin_[0],
(*point_cloud).sensor_origin_[1],
(*point_cloud).sensor_origin_[2])) *
Eigen::Affine3f ((*point_cloud).sensor_orientation_);
// pcl::visualization::RangeImageVisualizer range_image_widget ("Range image");
range_image.max_no_of_threads = 2;
range_image.createFromPointCloud ((*point_cloud),angular_resolution,pcl::deg2rad(360.0f),pcl::deg2rad(180.0f),scene_sensor_pose,coordinate_frame,noise_level,min_range,border_size);
range_image.setUnseenToMaxRange();
//saveRangeImagePlanarFilePNG(boost::lexical_cast<string>(i),range_image);
//range_image_widget.showRangeImage (range_image);
//range_image_widget.spin();
pcl::RangeImageBorderExtractor range_image_border_extractor;
pcl::NarfKeypoint narf_keypoint_detector;
narf_keypoint_detector.setRangeImageBorderExtractor (&range_image_border_extractor);
narf_keypoint_detector.setRangeImage (&range_image);
narf_keypoint_detector.getParameters().support_size = support_size;
//euristiche, per avvicinarsi al real time
narf_keypoint_detector.getParameters().max_no_of_threads = 2;
narf_keypoint_detector.getParameters().calculate_sparse_interest_image=false; //false
narf_keypoint_detector.getParameters().use_recursive_scale_reduction=true; //true
//narf_keypoint_detector.getParameters().add_points_on_straight_edges=true;
pcl::PointCloud<int> keypoint_indices;
narf_keypoint_detector.compute (keypoint_indices);
vector<int> keypoint_indices2;
keypoint_indices2.resize (keypoint_indices.points.size ());
for (unsigned int i=0; i<keypoint_indices.size (); ++i) // This step is necessary to get the right vector type
keypoint_indices2[i]=keypoint_indices.points[i];
pcl::NarfDescriptor narf_descriptor (&range_image, &keypoint_indices2);
narf_descriptor.getParameters().support_size = support_size;
narf_descriptor.getParameters().rotation_invariant = true;
pcl::PointCloud<pcl::Narf36> narf_descriptors;
narf_descriptor.compute (narf_descriptors);
clock_t end = clock();
double elapsed_secs = double(end - begin) / CLOCKS_PER_SEC;
media = media + elapsed_secs;
acc_media = media / (i+1);
cout << "media: " << acc_media<<endl;
scarti += pow(elapsed_secs-acc_media,2);
varianza = sqrt(scarti/(i+1));
cout << "varianza: " << varianza<<endl;
cout << ". Estratti "<<narf_descriptors.size ()<<" descrittori. Punti: " <<keypoint_indices.points.size ()<< "."<<endl;
features.push_back(vector<vector<float> >());
for (int p = 0; p < narf_descriptors.size(); p++) {
vector<float> flot;
copy(narf_descriptors[p].descriptor, narf_descriptors[p].descriptor+FNarf::L, back_inserter(flot));
features.back().push_back(flot);
flot.clear();
}
indices.clear();
range_image_border_extractor.clearData();
narf_keypoint_detector.clearData();
(*range_image_ptr).clear();
keypoint_indices.clear();
keypoint_indices2.clear();
(*point_cloud).clear();
(*point_cloud_wf).clear();
range_image.clear();
narf_descriptors.clear();
narf_descriptor = NULL;
}
cout << "Estrazione terminata." << endl;
}
template <typename PointXYZT, typename PointRGBT> void
pcl::LineRGBD<PointXYZT, PointRGBT>::applyProjectiveDepthICPOnDetections ()
{
const size_t nr_detections = detections_.size ();
for (size_t detection_index = 0; detection_index < nr_detections; ++detection_index)
{
typename pcl::LineRGBD<PointXYZT, PointRGBT>::Detection & detection = detections_[detection_index];
const size_t template_id = detection.template_id;
pcl::PointCloud<pcl::PointXYZRGBA> & point_cloud = template_point_clouds_[template_id];
const size_t start_x = detection.region.x;
const size_t start_y = detection.region.y;
const size_t pc_width = point_cloud.width;
const size_t pc_height = point_cloud.height;
std::vector<std::pair<float, float> > depth_matches;
for (size_t row_index = 0; row_index < pc_height; ++row_index)
{
for (size_t col_index = 0; col_index < pc_width; ++col_index)
{
const pcl::PointXYZRGBA & point_template = point_cloud (col_index, row_index);
const PointXYZT & point_input = (*cloud_xyz_) (col_index + start_x, row_index + start_y);
if (!pcl_isfinite (point_template.z) || !pcl_isfinite (point_input.z))
continue;
depth_matches.push_back (std::make_pair (point_template.z, point_input.z));
}
}
// apply ransac on matches
const size_t nr_matches = depth_matches.size ();
const size_t nr_iterations = 100; // todo: should be a parameter...
const float inlier_threshold = 0.01f; // 5cm // todo: should be a parameter...
size_t best_nr_inliers = 0;
float best_z_translation = 0.0f;
for (size_t iteration_index = 0; iteration_index < nr_iterations; ++iteration_index)
{
const size_t rand_index = (rand () * nr_matches) / RAND_MAX;
const float z_translation = depth_matches[rand_index].second - depth_matches[rand_index].first;
size_t nr_inliers = 0;
for (size_t match_index = 0; match_index < nr_matches; ++match_index)
{
const float error = fabsf (depth_matches[match_index].first + z_translation - depth_matches[match_index].second);
if (error <= inlier_threshold)
{
++nr_inliers;
}
}
if (nr_inliers > best_nr_inliers)
{
best_nr_inliers = nr_inliers;
best_z_translation = z_translation;
}
}
float average_depth = 0.0f;
size_t average_counter = 0;
for (size_t match_index = 0; match_index < nr_matches; ++match_index)
{
const float error = fabsf (depth_matches[match_index].first + best_z_translation - depth_matches[match_index].second);
if (error <= inlier_threshold)
{
//average_depth += depth_matches[match_index].second;
average_depth += depth_matches[match_index].second - depth_matches[match_index].first;
++average_counter;
}
}
average_depth /= static_cast<float> (average_counter);
detection.bounding_box.z = bounding_boxes_[template_id].z + average_depth;// - detection.bounding_box.depth/2.0f;
}
}
bool generateDistanceFields(void) {
if(FLAGS_df_list.empty()) {
return false;
}
if (!boost::filesystem::exists(FLAGS_df_list)) {
return false;
}
std::vector<DFItem> df_list;
std::ifstream fin(FLAGS_df_list);
std::string source, target;
int resolution;
std::set<std::string> folder_set;
while (fin >> source >> resolution >> target) {
df_list.push_back(std::make_tuple(source, resolution, target));
std::string folder = boost::filesystem::path(target).parent_path().string();
if(folder_set.find(folder) == folder_set.end()) {
if (!boost::filesystem::exists(folder)) {
boost::filesystem::create_directories(folder);
folder_set.insert(folder);
}
}
}
LOG(INFO) << df_list.size() << " items to be processed!" << std::endl;
if(!FLAGS_skip_converting) {
int step = 100;
for (int i = 0, i_end = df_list.size(); i < i_end; i ++) {
const std::string filename_df = std::get<2>(df_list[i]);
boost::filesystem::path path(filename_df);
std::string filename_pcd = path.parent_path().string()+"/"+path.stem().string()+".pcd";
osg::ref_ptr <PointCloud> point_cloud(new PointCloud);
if(point_cloud->load(filename_pcd)) {
LOG(INFO) << "Skipping generating " << filename_pcd << " as it exists and reads well..." << std::endl;
continue;
}
const std::string filename_mesh = std::get<0>(df_list[i]);
LOG(INFO) << "Converting " << filename_mesh << " into point cloud..." << std::endl;
osg::ref_ptr <MeshModel> mesh_model(new MeshModel);
if(!mesh_model->load(filename_mesh)) {
LOG(ERROR) << "Reading " << filename_mesh << " failed! Skipping it..." << std::endl;
continue;
}
point_cloud->data()->clear();
double grid_size = mesh_model->sampleScan(point_cloud->data(), 100, 0.0);
point_cloud->buildTree();
point_cloud->voxelGridFilter(grid_size/2, true);
point_cloud->save(filename_pcd);
if ((i+1)%step == 0) {
LOG(INFO) << "Converted " << (i+1) << " items! (total item number: " << i_end << ")" << std::endl;
}
}
}
if (!FLAGS_skip_generation) {
unsigned int n = std::thread::hardware_concurrency()-4;
LOG(INFO) << n << " threads will be used!" << std::endl;
std::vector<std::thread> threads;
for (unsigned int i = 0; i < n; ++ i) {
std::thread thread_obj(generateDistanceField, df_list, n, i);
threads.push_back(std::move(thread_obj));
}
for (unsigned int i = 0; i < n; ++ i) {
threads[i].join();
}
LOG(INFO) << "Distance field generation done!" << std::endl;
}
return true;
}
void keypoint_map::align_z_axis() {
pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud(
new pcl::PointCloud<pcl::PointXYZ>);
for (int v = 0; v < depth_imgs[0].rows; v++) {
for (int u = 0; u < depth_imgs[0].cols; u++) {
if (depth_imgs[0].at<unsigned short>(v, u) != 0) {
pcl::PointXYZ p;
p.z = depth_imgs[0].at<unsigned short>(v, u) / 1000.0f;
p.x = (u - intrinsics[2]) * p.z / intrinsics[0];
p.y = (v - intrinsics[3]) * p.z / intrinsics[1];
p.getVector4fMap() = camera_positions[0] * p.getVector4fMap();
point_cloud->push_back(p);
}
}
}
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZ> seg;
// Optional
seg.setOptimizeCoefficients(true);
// Mandatory
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setDistanceThreshold(0.005);
seg.setProbability(0.99);
seg.setInputCloud(point_cloud);
seg.segment(*inliers, *coefficients);
std::cerr << "Model coefficients: " << coefficients->values[0] << " "
<< coefficients->values[1] << " " << coefficients->values[2] << " "
<< coefficients->values[3] << " Num inliers "
<< inliers->indices.size() << std::endl;
Eigen::Affine3f transform = Eigen::Affine3f::Identity();
if (coefficients->values[2] > 0) {
transform.matrix().coeffRef(0, 2) = coefficients->values[0];
transform.matrix().coeffRef(1, 2) = coefficients->values[1];
transform.matrix().coeffRef(2, 2) = coefficients->values[2];
} else {
transform.matrix().coeffRef(0, 2) = -coefficients->values[0];
transform.matrix().coeffRef(1, 2) = -coefficients->values[1];
transform.matrix().coeffRef(2, 2) = -coefficients->values[2];
}
transform.matrix().col(0).head<3>() =
transform.matrix().col(1).head<3>().cross(
transform.matrix().col(2).head<3>());
transform.matrix().col(1).head<3>() =
transform.matrix().col(2).head<3>().cross(
transform.matrix().col(0).head<3>());
transform = transform.inverse();
transform.matrix().coeffRef(2, 3) = coefficients->values[3];
pcl::transformPointCloud(keypoints3d, keypoints3d, transform);
for (size_t i = 0; i < camera_positions.size(); i++) {
camera_positions[i] = transform * camera_positions[i];
}
}
void keypoint_map::extract_surface() {
octomap::ColorOcTree tree(0.05f);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr point_cloud(
new pcl::PointCloud<pcl::PointXYZRGBA>);
for (size_t i = 0; i < depth_imgs.size(); i++) {
cv::imwrite("rgb/" + boost::lexical_cast<std::string>(i) + ".png",
rgb_imgs[i]);
cv::imwrite("depth/" + boost::lexical_cast<std::string>(i) + ".png",
depth_imgs[i]);
octomap::Pointcloud octomap_cloud;
octomap::point3d sensor_origin(0.0, 0.0, 0.0);
Eigen::Vector3f trans(camera_positions[i].translation());
Eigen::Quaternionf rot(camera_positions[i].rotation());
octomap::pose6d frame_origin(
octomath::Vector3(trans.x(), trans.y(), trans.z()),
octomath::Quaternion(rot.w(), rot.x(), rot.y(), rot.z()));
//std::cerr << camera_positions[i].matrix() << std::endl;
for (int v = 0; v < depth_imgs[i].rows; v++) {
for (int u = 0; u < depth_imgs[i].cols; u++) {
if (depth_imgs[i].at<unsigned short>(v, u) != 0) {
pcl::PointXYZRGBA p;
p.z = depth_imgs[i].at<unsigned short>(v, u) / 1000.0f;
p.x = (u - intrinsics[2]) * p.z / intrinsics[0];
p.y = (v - intrinsics[3]) * p.z / intrinsics[1];
cv::Vec3b brg = rgb_imgs[i].at<cv::Vec3b>(v, u);
p.r = brg[2];
p.g = brg[1];
p.b = brg[0];
p.a = 255;
Eigen::Vector4f tmp = camera_positions[i]
* p.getVector4fMap();
if (tmp[2] < 2.0) {
octomap_cloud.push_back(p.x, p.y, p.z);
p.getVector4fMap() = tmp;
octomap::point3d endpoint(p.x, p.y, p.z);
octomap::ColorOcTreeNode* n = tree.search(endpoint);
if (n) {
n->setColor(p.r, p.g, p.b);
}
//ROS_INFO("Point %f %f %f from %f %f ", p.x, p.y, p.z, keypoints[i].pt.x, keypoints[i].pt.y);
point_cloud->push_back(p);
}
}
}
}
tree.insertScan(octomap_cloud, sensor_origin, frame_origin);
tree.updateInnerOccupancy();
}
tree.write("room.ot");
pcl::io::savePCDFileASCII("room.pcd", *point_cloud);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr point_cloud_filtered(
new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::VoxelGrid<pcl::PointXYZRGBA> sor;
sor.setInputCloud(point_cloud);
sor.setLeafSize(0.05f, 0.05f, 0.05f);
sor.filter(*point_cloud_filtered);
pcl::io::savePCDFileASCII("room_sub.pcd", *point_cloud_filtered);
}
int main(int argc, char** argv)
{
/// load calibration data
cv::Mat leftCameraMatrix; //3x3 floating-point left camera matrix
cv::Mat rightCameraMatrix; //3x3 floating-point right camera matrix
cv::Mat leftDistCoeffs; //8x1 vector of left distortion coefficients
cv::Mat rightDistCoeffs; //8x1 vector of right distortion coefficients
cv::Mat R1; // 3x3 rectification transform (rotation matrix) for the first camera
cv::Mat R2; // 3x3 rectification transform (rotation matrix) for the second camera
cv::Mat P1; // 3x4 projection matrix in the new (rectified) coordinate systems for the first camera
cv::Mat P2; // 3x4 projection matrix in the new (rectified) coordinate systems for the second camera
cv::Mat Q; // 4x4 disparity-to-depth mapping matrix
//load all calibration parameters from outputCalibration.xml
cv::FileStorage fsR("config/outputCalibration.xml", cv::FileStorage::READ);
fsR["leftCameraMatrix"] >> leftCameraMatrix;
fsR["rightCameraMatrix"] >> rightCameraMatrix;
fsR["leftDistCoeffs"] >> leftDistCoeffs;
fsR["rightDistCoeffs"] >> rightDistCoeffs;
fsR["R1"] >> R1;
fsR["R2"] >> R2;
fsR["P1"] >> P1;
fsR["P2"] >> P2;
fsR["Q"] >> Q;
//load test images
std::string left_filename, right_filename;
left_filename="../../dataset/test/left.jpg";
right_filename="../../dataset/test/right.jpg";
cv::Mat left_image = cv::imread(left_filename);
cv::Mat right_image = cv::imread(right_filename);
cv::Size image_size = left_image.size(); //size of test images
//left and right undistort and rectification maps
cv::Mat left_map1;
cv::Mat left_map2;
cv::Mat right_map1;
cv::Mat right_map2;
//timers
clock_t init, timeComplete;
init=clock(); //start timer
/// create rectification maps
cv::initUndistortRectifyMap(leftCameraMatrix, leftDistCoeffs, R1, P1, image_size, CV_32FC1, left_map1, left_map2);
cv::initUndistortRectifyMap(rightCameraMatrix, rightDistCoeffs, R2, P2, image_size, CV_32FC1, right_map1, right_map2);
//remap images
cv::Mat left_image_remap;
cv::Mat right_image_remap;
/// use the maps to rectificate images
cv::remap(left_image, left_image_remap, left_map1, left_map2, cv::INTER_LINEAR, cv::BORDER_CONSTANT, cv::Scalar());
cv::remap(right_image, right_image_remap, right_map1, right_map2, cv::INTER_LINEAR, cv::BORDER_CONSTANT, cv::Scalar());
//show undistorted and rectified test images;
/*cv::namedWindow("Original left image");
cv::imshow("Original left image",left_image);
cv::namedWindow("Original right image");
cv::imshow("Original right image",right_image);
cv::namedWindow("Rectified left image");
cv::imshow("Rectified left image",left_image_remap);
cv::namedWindow("Rectified right image");
cv::imshow("Rectified right image",right_image_remap);
cv::waitKey(0);
return 0;*/
/// compute the disparity
cv::StereoSGBM sgbm;
sgbm.preFilterCap = 100;
sgbm.SADWindowSize = 5;
sgbm.P1 = 8 * left_image_remap.channels() * sgbm.SADWindowSize * sgbm.SADWindowSize;
sgbm.P2 = 32 * left_image_remap.channels() * sgbm.SADWindowSize * sgbm.SADWindowSize;
sgbm.minDisparity = 40;
sgbm.numberOfDisparities = 256;
sgbm.uniquenessRatio = 10;
sgbm.speckleWindowSize = 200;
sgbm.speckleRange = 2;
sgbm.disp12MaxDiff = 0;
cv::Mat disparity_image;
sgbm(left_image_remap, right_image_remap, disparity_image);
//show disparity image;
/*
cv::namedWindow("Disparity");
cv::imshow("Disparity",disparity_image);
cv::waitKey(0);
return 0;*/
/// convert to 3D points
cv::Mat cloud_image;
cv::reprojectImageTo3D(disparity_image, cloud_image, Q);
//convert cloud_image into point format of PCL
pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
float px, py, pz;
int pr, pg, pb;
//scan cloud_image and convert its information in a Point Cloud
for (int i = 0; i < cloud_image.rows; i++)
{
for (int j = 0; j < cloud_image.cols; j++)
{
px= cloud_image.at<cv::Vec3f>(i,j)[0];
py= cloud_image.at<cv::Vec3f>(i,j)[1];
pz= cloud_image.at<cv::Vec3f>(i,j)[2];
pb= left_image.at<cv::Vec3b>(i,j)[0];
pg= left_image.at<cv::Vec3b>(i,j)[1];
pr= left_image.at<cv::Vec3b>(i,j)[2];
//Insert info into point cloud structure
pcl::PointXYZRGB point;
point.x = px;
point.y = py;
point.z = pz;
point.r = pr;
point.g = pg;
point.b = pb;
//delete unwanted infinites points and outliers
if(point.x>-30 && point.x<30 && point.y>-30 && point.y<30 && point.z>-3 && point.z <3){
point_cloud->points.push_back (point);
}
}
}
/// visualize 3D points
pcl::visualization::PCLVisualizer visualizer("PCL visualizer");
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(point_cloud);
visualizer.addPointCloud<pcl::PointXYZRGB> (point_cloud, rgb, "point_cloud");
timeComplete=clock()-init; //final time
std::cout <<"Reconstruction complete in: "<<timeComplete <<std::endl;
visualizer.spin();
return 0;
}
void IbeoSourceWrapper::run()
{
// Create ibeo source
icl_sourcesink::SourceSinkManager manager;
nibeo::IbeoSourceNoAPI::Ptr ibeo_source = manager.createSource<nibeo::IbeoMsg>(m_ibeo_uri, std::vector<std::string>(), true);
nncom::NcomSource::Ptr ncom_source = manager.createSource<nncom::Ncom>(m_ncom_uri);
// We create a data element ptr that we will use to work on.
nibeo::IbeoMsgStamped::Ptr ibeo_element;
nncom::NcomStamped::Ptr ncom_element;
icl_gps::AbsolutePose* ref_pose = NULL;
// for each time step
for (; manager.good(); manager.advance())
{
ibeo_element = ibeo_source->get();
ncom_element = ncom_source->get();
// Skip messages other than IbeoScanMsg
nibeo::IbeoScanMsg scan_msg;
if(!scan_msg.fromIbeoMsg(*ibeo_element))
continue;
LOGGING_INFO(Gpu_voxels, "Number of points in ibeo msg: " << scan_msg.number_of_points << endl);
// Copy points to std::vector
std::vector<Vector3f> point_cloud(scan_msg.number_of_points);
for(int i = 0; i < scan_msg.number_of_points; ++i)
{
point_cloud[i].x = (*scan_msg.scan_points)[i].x;
point_cloud[i].y = (*scan_msg.scan_points)[i].y;
point_cloud[i].z = (*scan_msg.scan_points)[i].z;
}
// Handle position
nncom::Ncom ncom_msg = *ncom_element; // Unwrap message
icl_gps::AbsolutePose abs_pose(ncom_msg->mLon, ncom_msg->mLat, ncom_msg->mAlt, ncom_msg->mTrack);
LOGGING_INFO(Gpu_voxels, "Current pose. long: " << abs_pose.longitude() << " lat: " << abs_pose.latitude() << " alt: " << abs_pose.altitude() << " bear: " << abs_pose.bearing() << endl);
if(!ref_pose) // Use first position as reference
{
ref_pose = new icl_gps::AbsolutePose(abs_pose.longitude(), abs_pose.latitude(), abs_pose.altitude(), abs_pose.bearing());
LOGGING_INFO(Gpu_voxels, "Set reference pose. long: " << abs_pose.longitude() << " lat: " << abs_pose.latitude() << " alt: " << abs_pose.altitude() << " bear: " << abs_pose.bearing() << endl);
}
icl_gps::RelativePose rel_pose = abs_pose - *ref_pose;
icl_math::Pose2d rel_pose2d = rel_pose.toPose2d();
LOGGING_INFO(Gpu_voxels, "Relative pose. north: " << rel_pose.north() << " east: " << rel_pose.east() << " alt: " << rel_pose.altitude() << endl);
Matrix4f matrix;
// copy pose2d rotation matrix
matrix.a11 = rel_pose2d(0,0); matrix.a12 = rel_pose2d(0,1);
matrix.a21 = rel_pose2d(1,0); matrix.a22 = rel_pose2d(1,1);
// copy pose2d translation vector
matrix.a14 = rel_pose2d(0,2);
matrix.a24 = rel_pose2d(1,2);
// set generals structure of transformation matrix
matrix.a33 = 1.0f;
matrix.a44 = 1.0f;
// set z-translation (altitude)
matrix.a34 = static_cast<float>(rel_pose.altitude());
// set additional translation
matrix.a14 += m_additional_translation.x;
matrix.a24 += m_additional_translation.y;
matrix.a34 += m_additional_translation.z;
LOGGING_INFO(Gpu_voxels, "Matrix4f: " << endl << matrix.a11 << " " << matrix.a12 << " " << matrix.a13 << " " << matrix.a14 << endl
<< matrix.a21 << " " << matrix.a22 << " " << matrix.a23 << " " << matrix.a24 << endl
<< matrix.a31 << " " << matrix.a32 << " " << matrix.a33 << " " << matrix.a34 << endl
<< matrix.a41 << " " << matrix.a42 << " " << matrix.a43 << " " << matrix.a44 << endl);
LOGGING_INFO(Gpu_voxels, "Transform point cloud..." << endl);
CudaMath::transform(point_cloud, matrix); // todo: keep point cloud in gpu mem
m_callback(point_cloud);
}
}
void fi::MSHRFileIO::get3DTo2DProjectionsUniBW()
{
//collect the points and images
fi::MSHR3D2DTableParser mshr_results(m_mshr_result_dir, m_image_data_dir, m_image_extension);
pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud(new pcl::PointCloud<pcl::PointXYZ>());
std::vector<std::vector<unsigned int> > corresponding_images; //image names used to extract the corresponding poses
std::vector<std::vector<std::string> > mapping_table;
bool parse_data = mshr_results.parseInput(point_cloud, corresponding_images, mapping_table);
//get the full paths to the images
std::vector<std::vector<std::string> > corresponding_images_filenames;
bool image_present = mshr_results.getCorrespondingImageFiles(corresponding_images, corresponding_images_filenames);
//get the calibration data file
fi::CamCalibParser m_cam(m_mshr_result_dir);
Eigen::Matrix3f cam_calib;
m_cam.getCameraCalib(cam_calib);
std::cout<<cam_calib<<std::endl;
//get the pose
std::vector<std::vector<Eigen::Matrix3f> > rotation_matrix;
std::vector<std::vector<Eigen::Vector3f> > translation_vectors;
std::vector<std::vector<float> > radial_distortion_params_k1;
std::vector<std::vector<float> > radial_distortion_params_k2;
std::vector<std::vector<unsigned int> > image_width;
std::vector<std::vector<unsigned int> > image_height;
std::string unibw_params_folder = m_unibw_params_dir;
fi::CamPOSEParser data_pose(m_unibw_params_dir, corresponding_images);
bool extracted = data_pose.getCamPose(rotation_matrix, translation_vectors, radial_distortion_params_k1, radial_distortion_params_k2, image_width, image_height);
//project to image
unsigned int num_pnts = translation_vectors.size();
for ( unsigned int i = 0; i < num_pnts; i++)
{
//collect for the point all the values of the pose
std::vector<Eigen::Matrix3f> pnt_rotation_matrix = rotation_matrix.at(i);
std::vector<Eigen::Vector3f> pnt_translation_vectors = translation_vectors.at(i);
std::vector<float> pnt_radial_distortion_params_k1 = radial_distortion_params_k1.at(i);
std::vector<float> pnt_radial_distortion_params_k2 = radial_distortion_params_k2.at(i);
std::vector<unsigned int> pnt_image_width = image_width.at(i);
std::vector<unsigned int> pnt_image_height = image_height.at(i);
//
unsigned int pnt_num_images = pnt_radial_distortion_params_k1.size();
std::vector<Eigen::Vector2f> pnt_img_points;
for(unsigned int j = 0; j < pnt_num_images; j ++)
{
//for the selected image compute all the corresponding values
const Eigen::Matrix3f &rot_mat = pnt_rotation_matrix.at(j);
const Eigen::Vector3f &trans_vec = pnt_translation_vectors.at(j);
float undist1 = pnt_radial_distortion_params_k1.at(j);
float undist2 = pnt_radial_distortion_params_k2.at(j);
unsigned int img_w = pnt_image_width.at(j);
unsigned int img_h = pnt_image_height.at(j);
Eigen::Vector2f an_image_point;
getProjectionPointOnImage(point_cloud->points[i], cam_calib, rot_mat, trans_vec, undist1, undist2, img_w, img_h, an_image_point);
pnt_img_points.push_back(an_image_point);
}
//collect all corresponding points
m_image_points2D.push_back(pnt_img_points);
}
m_cloud = point_cloud;
m_corresponding_images_filenames = corresponding_images_filenames;
m_mapping_table = mapping_table;
}
