vector<Observation> Localization::findGreenObjects(vector<observationBlob>& blobs,
string id) {
vector<Observation> ob;
if ( isUsable(blobs) ) {
for(int i = 0; i < blobs.size(); i++ ){
observationBlob blob = blobs[i];
if ( blob.Area() > 30 ) {
double angleLeft, angleRight ;
( blob.Left() > 1 ) ? angleLeft = getAngle(blob.Left()) : angleLeft = Observation::InvalidBearing ;
( blob.Right() < 159 ) ? angleRight = getAngle(blob.Right()) : angleRight = Observation::InvalidBearing ;
Observation observation = Observation(id,
map,
observationVariance,
angleLeft,
angleRight);
ob.push_back(observation);
blob.setInUse();
if ( !isUsable(blobs) ) {
return ob;
}
}
}
}
return ob;
}
void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud& cloud){
Stamped<btVector3> global_origin;
//create a new observation on the list to be populated
observation_list_.push_front(Observation());
//check whether the origin frame has been set explicitly or whether we should get it from the cloud
string origin_frame = sensor_frame_ == "" ? cloud.header.frame_id : sensor_frame_;
try{
//given these observations come from sensors... we'll need to store the origin pt of the sensor
Stamped<btVector3> local_origin(btVector3(0, 0, 0), cloud.header.stamp, origin_frame);
tf_.transformPoint(global_frame_, local_origin, global_origin);
observation_list_.front().origin_.x = global_origin.getX();
observation_list_.front().origin_.y = global_origin.getY();
observation_list_.front().origin_.z = global_origin.getZ();
//make sure to pass on the raytrace/obstacle range of the observation buffer to the observations the costmap will see
observation_list_.front().raytrace_range_ = raytrace_range_;
observation_list_.front().obstacle_range_ = obstacle_range_;
sensor_msgs::PointCloud global_frame_cloud;
//transform the point cloud
tf_.transformPointCloud(global_frame_, cloud, global_frame_cloud);
global_frame_cloud.header.stamp = cloud.header.stamp;
//now we need to remove observations from the cloud that are below or above our height thresholds
sensor_msgs::PointCloud& observation_cloud = observation_list_.front().cloud_;
unsigned int cloud_size = global_frame_cloud.points.size();
observation_cloud.points.resize(cloud_size);
unsigned int point_count = 0;
//copy over the points that are within our height bounds
for(unsigned int i = 0; i < cloud_size; ++i){
if(global_frame_cloud.points[i].z <= max_obstacle_height_ && global_frame_cloud.points[i].z >= min_obstacle_height_){
observation_cloud.points[point_count++] = global_frame_cloud.points[i];
}
}
//resize the cloud for the number of legal points
observation_cloud.points.resize(point_count);
observation_cloud.header.stamp = cloud.header.stamp;
observation_cloud.header.frame_id = global_frame_cloud.header.frame_id;
}
catch(TransformException& ex){
//if an exception occurs, we need to remove the empty observation from the list
observation_list_.pop_front();
ROS_ERROR("TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s", sensor_frame_.c_str(),
cloud.header.frame_id.c_str(), ex.what());
return;
}
//if the update was successful, we want to update the last updated time
last_updated_ = ros::Time::now();
//we'll also remove any stale observations from the list
purgeStaleObservations();
}
vector<Observation> Localization::getCornerMarkers(vector<observationBlob>& topBlobs,
vector<observationBlob>& bottomBlobs,
string id) {
vector<Observation> ob;
if ( isUsable(topBlobs) && isUsable(bottomBlobs) ){
for( int i = 0; i < topBlobs.size(); i++ ){
for ( int j = 0; j < bottomBlobs.size(); j++ ){
observationBlob& bottom = bottomBlobs[j];
observationBlob& top = topBlobs[i];
// process blob: add the marker only if there isn't a significant gap between them
double topL = static_cast<double>(top.Bottom() - top.Top());
double topW = static_cast<double>(top.Right() - top.Left());
double bottomL = static_cast<double>(bottom.Bottom() - bottom.Top());
double bottomW = static_cast<double>(bottom.Right() - bottom.Left());
double eps = 0.5;
if (blobOnTopOf(top, bottom) && (bottom.Top() - top.Bottom() < ( topL + bottomL )/2 )) {
//cout << "Corner blob found" << endl;
int avgLeft = ( top.Left() + bottom.Left() ) / 2 ;
int avgRight = ( top.Right() + bottom.Right() ) / 2 ;
//int avgCenter = ( avgLeft + avgRight ) / 2 ;
//cout << "avgLeft: " << avgLeft << ", avgRight: " << avgRight << /*", avgCenter: " << avgCenter << */endl;
//double angleCenter = getAngle(avgCenter);
double angleLeft, angleRight ;
( avgLeft > 5 ) ? angleLeft = getAngle(avgLeft) : angleLeft = Observation::InvalidBearing ;
( avgRight < 155 ) ? angleRight = getAngle(avgRight) : angleRight = Observation::InvalidBearing ;
//cout << "angleLeft: " << angleLeft << ", angleRight: " << angleRight << /*", avgCenter: " << angleCenter <<*/ endl;
Observation observation = Observation(id,
map,
//angleCenter,
observationVariance,
angleLeft,
angleRight);
ob.push_back(observation);
top.setInUse();
bottom.setInUse();
if ( !isUsable(topBlobs) || !isUsable(bottomBlobs) ){
return ob;
}
}
}
}
}
return ob;
}
vector<Observation> Localization::getRoomMarkers(vector<observationBlob>& topBlobs,
vector<observationBlob>& middleBlobs,
vector<observationBlob>& bottomBlobs,
string id) {
vector<Observation> ob;
if ( isUsable(topBlobs) && isUsable(middleBlobs) && isUsable(bottomBlobs) ){
for( int i = 0; i < topBlobs.size(); i++ ) {
for ( int k = 0; k < middleBlobs.size(); k++ ) {
for ( int j = 0; j < bottomBlobs.size(); j++ ) {
observationBlob& top = topBlobs[i];
observationBlob& middle = middleBlobs[k];
observationBlob& bottom = bottomBlobs[j];
if ( !top.Used() && !middle.Used() && !bottom.Used() ){
if (blobOnTopOf(top, middle) && blobOnTopOf(middle,bottom)) {
int avgLeft = ( top.Left() + middle.Left() + bottom.Left() ) / 3 ;
int avgRight = ( top.Right() + middle.Right() + bottom.Right() ) / 3 ;
//int avgCenter = ( avgLeft + avgRight ) / 2 ;
//double angleCenter = getAngle(avgCenter);
double angleLeft, angleRight ;
( avgLeft > 5 ) ? angleLeft = getAngle(avgLeft) : angleLeft = Observation::InvalidBearing ;
( avgRight < 155 ) ? angleRight = getAngle(avgRight) : angleRight = Observation::InvalidBearing ;
Observation observation = Observation(id,
map,
//angleCenter,
observationVariance,
angleLeft,
angleRight);
ob.push_back(observation);
top.setInUse();
middle.setInUse();
bottom.setInUse();
if ( !isUsable(topBlobs) || !isUsable(middleBlobs) || !isUsable(bottomBlobs) )
return ob;
}
}
}
}
}
}
return ob;
}
Observation TimestampMergeFilter::getMergedObs(){
Observation result;
vector<float> sortedBids;
vector<float> sortedOffers;
if(distance(m_buffer.begin(), m_buffer.end()) > 1){
for(forward_list<Observation>::iterator it = m_buffer.begin(); it != m_buffer.end(); it++){
sortedBids.push_back(*it->bid);
sortedOffers.push_back(*it->offer);
}
//sort buffers
sort(sortedBids.begin(), sortedBids.end());
sort(sortedOffers.begin(), sortedOffers.end());
float* medOffer = new float(StatsUtils::median(sortedOffers));
float* medBid = new float(StatsUtils::median(sortedBids));
result = Observation(
m_buffer.front().symbol,
m_buffer.front().date,
m_buffer.front().time,
medOffer,
medBid,
m_buffer.front().mode
);
//cleanup
delete m_buffer.front().bid;
delete m_buffer.front().offer;
for(forward_list<Observation>::iterator it = ++m_buffer.begin(); it != m_buffer.end(); it++)
it->deleteAll();
} else{
result = m_buffer.front();
}
return result;
}
/// Init & triangulate landmark observations from validated 3-view correspondences
void triangulate(
SfM_Data & sfm_data,
const std::shared_ptr<Regions_Provider> & regions_provider)
{
openMVG::tracks::STLMAPTracks map_tracksCommon;
openMVG::tracks::TracksBuilder tracksBuilder;
tracksBuilder.Build(triplets_matches);
tracksBuilder.Filter(3);
tracksBuilder.ExportToSTL(map_tracksCommon);
matching::PairWiseMatches().swap(triplets_matches);
// Generate new Structure tracks
sfm_data.structure.clear();
// Fill sfm_data with the computed tracks (no 3D yet)
Landmarks & structure = sfm_data.structure;
IndexT idx(0);
for (tracks::STLMAPTracks::const_iterator itTracks = map_tracksCommon.begin();
itTracks != map_tracksCommon.end();
++itTracks, ++idx)
{
const tracks::submapTrack & track = itTracks->second;
structure[idx] = Landmark();
Observations & obs = structure.at(idx).obs;
for (tracks::submapTrack::const_iterator it = track.begin(); it != track.end(); ++it)
{
const size_t imaIndex = it->first;
const size_t featIndex = it->second;
const Vec2 pt = regions_provider->regions_per_view.at(imaIndex)->GetRegionPosition(featIndex);
obs[imaIndex] = Observation(pt, featIndex);
}
}
// Triangulate them using a robust triangulation scheme
SfM_Data_Structure_Computation_Robust structure_estimator(true);
structure_estimator.triangulate(sfm_data);
}
vector<Observation> Localization::getEnteranceMarkers(vector<observationBlob>& blobs,
string id) {
vector<Observation> ob;
if ( isUsable(blobs) ) {
for(int i = 0; i < blobs.size(); i++ ){
observationBlob blob = blobs[i];
// process blobs: check the length/width ratio. should be close to 2:1. just to be sure 1.5:1
double d = static_cast<double>( blob.Bottom() - blob.Top() ) / ( blob.Right() - blob.Left() );
if ( d > 1.75 ) {
//double angleCenter = getAngle((blob.Right() + blob.Left()) / 2);
double angleLeft, angleRight ;
( blob.Left() > 1 ) ? angleLeft = getAngle(blob.Left()) : angleLeft = Observation::InvalidBearing ;
( blob.Right() < 159 ) ? angleRight = getAngle(blob.Right()) : angleRight = Observation::InvalidBearing ;
Observation observation = Observation(id,
map,
//angleCenter,
observationVariance,
angleLeft,
angleRight);
ob.push_back(observation);
blob.setInUse();
if ( !isUsable(blobs) ) {
return ob;
}
}
}
}
return ob;
}