void ofxSick::draw(int gridDivisions, float gridSize) const {
ofPushMatrix();
ofPushStyle();
ofPushMatrix();
ofRotate(invert ? -angleOffset : angleOffset);
ofNoFill();
ofSetColor(ofColor::blue);
ofLine(0, 0, gridSize, 0); // forward
ofSetColor(ofColor::magenta);
ofLine(ofVec2f(0, 0), ofVec2f(gridSize, 0).rotate(startAngle)); // left bound
ofLine(ofVec2f(0, 0), ofVec2f(gridSize, 0).rotate(stopAngle)); // right bound
for(int i = 0; i < gridDivisions; i++) {
float radius = ofMap(i, -1, gridDivisions - 1, 0, gridSize);
ofSetColor(64);
ofCircle(0, 0, radius);
ofVec2f textPosition = ofVec2f(radius, 0).rotate(45);
ofSetColor(255);
ofDrawBitmapString(ofToString(radius, 2) + "mm", textPosition);
}
ofPopMatrix();
ofSetColor(255);
pointCloud(pointsFirst).draw();
ofSetColor(ofColor::red);
pointCloud(pointsSecond).draw();
ofPopStyle();
ofPopMatrix();
}
/*!
* \brief OiGraphix::drawPointCloud
* \param p
*/
void OiGraphix::drawPointCloud(PointCloud *p){
OiGraphixPointCloud pointCloud(p->points);
pointCloud.draw(p->xyz.getAt(0), p->xyz.getAt(1), p->xyz.getAt(2));
drawResiduals();
}
void callBack(const sensor_msgs::PointCloud2::ConstPtr msg)
{
boost::posix_time::ptime now = boost::posix_time::microsec_clock::local_time();
std::string filename = "Capture/v2_" + boost::lexical_cast<std::string,boost::posix_time::ptime>( now ) + ".pcd";
boost::algorithm::erase_all(filename,":");
std::cout << filename << std::endl;
boost::shared_ptr<pcl::PointCloud<Point> > pointCloud(new pcl::PointCloud<Point>);
pcl::io::savePCDFileBinary<Point>(filename,*pointCloud);
ROS_INFO("Height: [%i]",msg->height);
ROS_INFO("Width: [%i]",msg->width);
}
void ElevationMapping::pointCloudCallback(
const sensor_msgs::PointCloud2& rawPointCloud)
{
stopMapUpdateTimer();
boost::recursive_mutex::scoped_lock scopedLock(map_.getRawDataMutex());
// Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud.
// TODO Double check with http://wiki.ros.org/hydro/Migration
pcl::PCLPointCloud2 pcl_pc;
pcl_conversions::toPCL(rawPointCloud, pcl_pc);
PointCloud<PointXYZRGB>::Ptr pointCloud(new PointCloud<PointXYZRGB>);
pcl::fromPCLPointCloud2(pcl_pc, *pointCloud);
Time time;
time.fromNSec(1000 * pointCloud->header.stamp);
ROS_DEBUG("ElevationMap received a point cloud (%i points) for elevation mapping.", static_cast<int>(pointCloud->size()));
// Update map location.
updateMapLocation();
// Update map from motion prediction.
if (!updatePrediction(time)) {
ROS_ERROR("Updating process noise failed.");
resetMapUpdateTimer();
return;
}
// Get robot pose covariance matrix at timestamp of point cloud.
boost::shared_ptr<geometry_msgs::PoseWithCovarianceStamped const> poseMessage = robotPoseCache_.getElemBeforeTime(time);
if (!poseMessage)
{
ROS_ERROR("Could not get pose information from robot for time %f. Buffer empty?", time.toSec());
return;
}
Eigen::Matrix<double, 6, 6> robotPoseCovariance = Eigen::Map<const Eigen::MatrixXd>(poseMessage->pose.covariance.data(), 6, 6);
// Process point cloud.
PointCloud<PointXYZRGB>::Ptr pointCloudProcessed(new PointCloud<PointXYZRGB>);
Eigen::VectorXf measurementVariances;
if (!sensorProcessor_->process(pointCloud, robotPoseCovariance, pointCloudProcessed,
measurementVariances)) {
ROS_ERROR("Point cloud could not be processed.");
resetMapUpdateTimer();
return;
}
// Add point cloud to elevation map.
if (!map_.add(pointCloudProcessed, measurementVariances)) {
ROS_ERROR("Adding point cloud to elevation map failed.");
resetMapUpdateTimer();
return;
}
// Publish elevation map.
map_.publishRawElevationMap();
if (isContinouslyFusing_ && map_.hasFusedMapSubscribers()) {
map_.fuseAll(true);
map_.publishFusedElevationMap();
}
resetMapUpdateTimer();
}
void Pipeline::run(const bool save_clouds, const bool show_clouds)
{
Logger _log("Pipeline");
/**
* Stage 0: Load images from file
*/
Images images;
load_images(folder_path, images);
/**
* Stage 1: Detect features in loaded images
*/
CamFrames cam_Frames;
DescriptorsVec descriptors_vec;
extract_features(images, cam_Frames, descriptors_vec);
// Free Image.gray
for (int i = 0; i < images.size(); i++)
const_cast<cv::Mat&>(images[i].gray).release();
/**
* Stage 2: Calculate descriptors and find image pairs through matching
*/
ImagePairs image_pairs;
find_matching_pairs(images, cam_Frames, descriptors_vec, image_pairs);
// Free some memory
DescriptorsVec().swap(descriptors_vec);
/**
* State 3: Compute pairwise R and t
*/
register_camera(image_pairs, cam_Frames);
/**
* Stage 4: Construct associativity matrix and spanning tree
*/
Associativity assocMat(cam_Frames.size());
for (int p = 0; p < image_pairs.size(); p++)
{
ImagePair* pair = &image_pairs[p];
int i = pair->pair_index.first,
j = pair->pair_index.second;
if (pair -> R.empty()) continue;
assocMat(i, j) = pair;
assocMat(j, i) = pair;
assert(assocMat(i, j) == assocMat(j, i));
assert(assocMat(i, j)->pair_index.first == i &&
assocMat(i, j)->pair_index.second == j);
}
Associativity tree;
const int camera_num = build_spanning_tree(image_pairs, assocMat, tree);
/**
* Stage 5: Compute global Rs and ts
*/
CameraPoses gCameraPoses;
glo_cam_poses(images, gCameraPoses, image_pairs, tree);
/**
* Stage 6: Find and cluster depth points from local camera frame to global camera frame
*/
PointClusters pointClusters;
PointMap pointMap;
find_clusters(assocMat, gCameraPoses, cam_Frames, pointClusters, pointMap);
// Free some memory
ImagePairs().swap(image_pairs);
/**
* Stage 7: get center of mass from clusters
*/
PointCloud pointCloud(pointClusters.size());
find_CoM(pointClusters, pointCloud);
// Free pointClusters
for (int i = 0; i < pointClusters.size(); i++)
PointCluster().swap(pointClusters[i]);
PointClusters().swap(pointClusters);
// Save cloud before BA
Viewer viewer("Before BA");
auto cloud = viewer.createPointCloud(images, gCameraPoses, cameraMatrix);
int n = cloud->points.size();
auto time = ptime::second_clock::local_time();
auto tstamp = ptime::to_iso_string(time);
auto folder = fs::path(folder_path).filename().string();
auto fname = (fmt("%s_%s_%d_noBA.pcd") % folder % tstamp % n).str();
if (save_clouds)
viewer.saveCloud(cloud, fname);
if (show_clouds)
viewer.showCloudPoints(cloud, false);
/**
* State 8: Bundle Adjustment
*/
bundle_adjustment(pointMap, cam_Frames, false, gCameraPoses, pointCloud);
_log.tok();
/**
* Show calculated point cloud
*/
Viewer viewer_ba("After BA no Depth");
cloud = viewer_ba.createPointCloud(images, gCameraPoses, cameraMatrix);
n = cloud->points.size();
fname = (fmt("%s_%s_%d_BA_noD.pcd") % folder % tstamp % n).str();
if (save_clouds)
viewer_ba.saveCloud(cloud, fname);
if (show_clouds)
viewer_ba.showCloudPoints(cloud,false);
bundle_adjustment(pointMap, cam_Frames, true, gCameraPoses, pointCloud);
// Free some memory
PointMap().swap(pointMap);
CamFrames().swap(cam_Frames);
PointCloud().swap(pointCloud);
/**
* Show calculated point cloud
*/
Viewer viewer_baD("After BA with Depth");
cloud = viewer_baD.createPointCloud(images, gCameraPoses, cameraMatrix);
n = cloud->points.size();
fname = (fmt("%s_%s_%d_BA_D.pcd") % folder % tstamp % n).str();
// Free some memory
Images().swap(images);
CameraPoses().swap(gCameraPoses);
if (save_clouds)
viewer_baD.saveCloud(cloud, fname);
if (show_clouds)
viewer_baD.showCloudPoints(cloud);
}