void ImageLandmarksWidget::setTargetLandmark(QString uid, Vector3D p_target)
{
DataPtr image = this->getCurrentData();
if (!image)
return;
image->getLandmarks()->setLandmark(Landmark(uid, p_target));
}
void ImageLandmarksWidget::importPointMetricsToLandmarkButtonClickedSlot()
{
DataPtr image = this->getCurrentData();
if(!image)
return;
std::map<QString, DataPtr> point_metrics = mServices->patient()->getChildren(image->getUid(), "pointMetric");
std::map<QString, DataPtr>::iterator it = point_metrics.begin();
//Make sure we have enough landmarks
int number_of_landmarks = mServices->patient()->getLandmarkProperties().size();
int number_of_metrics = point_metrics.size();
for(int i=number_of_landmarks; i<number_of_metrics; ++i)
{
QString uid = mServices->patient()->addLandmark();
}
for(; it != point_metrics.end(); ++it)
{
PointMetricPtr point_metric = boost::static_pointer_cast<PointMetric>(it->second);
if(!point_metric)
continue;
Vector3D pos_x = point_metric->getCoordinate();
//Transform3D d_M_x = mServices->spaceProvider()->get_toMfrom(CoordinateSystem::fromString(image->getSpace()), point_metric->getSpace());
//Vector3D pos_d = d_M_x.coord(pos_x);
QString point_metric_name = point_metric->getName();
image->getLandmarks()->setLandmark(Landmark(point_metric_name, pos_x));
this->activateLandmark(point_metric_name);
}
}
std::vector<Landmark> LandmarkRegistrationWidget::getAllLandmarks() const
{
std::vector<Landmark> retval;
LandmarkMap targetData = this->getTargetLandmarks();
std::map<QString, LandmarkProperty> dataData = mServices->patient()->getLandmarkProperties();
std::map<QString, LandmarkProperty>::iterator iter;
for (iter = dataData.begin(); iter != dataData.end(); ++iter)
{
if (targetData.count(iter->first))
retval.push_back(targetData[iter->first]);
else
retval.push_back(Landmark(iter->first));
}
std::sort(retval.begin(), retval.end());
return retval;
}
void ImageLandmarksWidget::editLandmarkButtonClickedSlot()
{
PickerRepPtr PickerRep = this->getPickerRep();
if (!PickerRep)
{
reportError("Need a 3D view to edit landmarks.");
return;
}
DataPtr image = this->getCurrentData();
if (!image)
return;
QString uid = mActiveLandmark;
Vector3D pos_r = PickerRep->getPosition();
Vector3D pos_d = image->get_rMd().inv().coord(pos_r);
image->getLandmarks()->setLandmark(Landmark(uid, pos_d));
this->activateLandmark(this->getNextLandmark());
}
void loadLandmarks(std::string landmarksFileName){
FileReader fr(landmarksFileName);
if(!fr.is_open()){
std::cout << "landmarks file not located, putting landmarks in 'default' position" << std::endl;
setDefaultLandmarkPositions();
return;
}
std::vector<std::string> textline;
fr.readLine(&textline);
while(fr.good()){
if(textline.size() > 1){
double x = atof(textline[0].c_str());
double y = atof(textline[1].c_str());
landmarks.push_back(Landmark(x,y));
}
textline.clear();
fr.readLine(&textline);
}
}
/// 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);
}
void PatientLandMarksWidget::toolSampleButtonClickedSlot()
{
ToolPtr tool = mServices->tracking()->getActiveTool();
if (!tool)
{
reportError("mToolToSample is NULL!");
return;
}
//TODO What if the reference frame isnt visible?
Transform3D lastTransform_prMt = tool->get_prMt();
Vector3D p_pr = lastTransform_prMt.coord(Vector3D(0, 0, tool->getTooltipOffset()));
// TODO: do we want to allow sampling points not defined in image??
if (mActiveLandmark.isEmpty() && !mServices->patient()->getLandmarkProperties().empty())
mActiveLandmark = mServices->patient()->getLandmarkProperties().begin()->first;
mServices->patient()->getPatientLandmarks()->setLandmark(Landmark(mActiveLandmark, p_pr));
reporter()->playSampleSound();
this->activateLandmark(this->getNextLandmark());
this->performRegistration(); // automatic when sampling in physical patient space (Mantis #0000674)s
}
void InformationFilterFtr::CreateMap(Mat *u, vector<int> *ptpairsCMin, vector<CvPoint3D64f> *pointsCMin)
{
//re-mapping
Map* Map3D = map3d_ptr_;
typedef struct
{
vector<RoverState> tEST;
} RoverTrajectory_t;
RoverTrajectory_t* RoverTrajectory = new RoverTrajectory_t;
RoverTrajectory->tEST.resize(step_ptr_ + 1);
RoverTrajectory->tEST[step_ptr_] = curr_pose_ptr_;
int* STEP = &step_ptr_;
vector<sscrovers_pmslam_common::SPoint>* SDatabase = &db_ptr_;
int id;
vector<CvPoint3D64f> vn, vf;
vector<double> vrn, vrf;
vector<int> ids;
int idx = slam_filter_ptr_->idx;
PMSLAM_Data_msg.MapOut.ID.clear();
PMSLAM_Data_msg.MapOut.positions.x.clear();
PMSLAM_Data_msg.MapOut.positions.y.clear();
PMSLAM_Data_msg.MapOut.positions.z.clear();
if (*STEP > 0)
{
slam_filter_ptr_->predict(*u);
vn.clear();
vf.clear();
vrn.clear();
vrf.clear();
ids.clear();
CvPoint3D64f temp;
// Split landmarks into new (vn) and re-observed (vf) ones
for (unsigned int j = 0; j < ptpairsCMin->size(); j ++)
{
ros::param::set("filter", true);
temp.x = (*pointsCMin)[j ].x;
temp.y = (*pointsCMin)[j ].y;
temp.z = (*pointsCMin)[j ].z;
if ((*SDatabase)[(*ptpairsCMin)[j]].id == 0) // if new landmark
{
//ROS_INFO("New Landmark");
//lnadmark added
vn.push_back(temp);
vrn.push_back(0.001);
(*SDatabase)[(*ptpairsCMin)[j]].id = Map3D->map.size();
idVec[(*ptpairsCMin)[j]] = Map3D->map.size();
(*pointsCMin)[j].x = (*pointsCMin)[j].x + RoverTrajectory->tEST[*STEP].x;
(*pointsCMin)[j].y = (*pointsCMin)[j].y + RoverTrajectory->tEST[*STEP].y;
(*pointsCMin)[j].z = (*pointsCMin)[j].z + RoverTrajectory->tEST[*STEP].z;
Map3D->addLandmark(Landmark(Map3D->map.size(), (*pointsCMin)[j], (idx - 3) / 3, 0.05));
idx = idx + 3;
}
else
{
// Update EIF
//ROS_INFO("Update Landmark");
ids.push_back(Map3D->map[idVec[(*ptpairsCMin)[j]]].matPos);
vrf.push_back(Map3D->map[idVec[(*ptpairsCMin)[j]]].stddev);
vf.push_back(temp);
}
}
slam_filter_ptr_->recoverMean();
slam_filter_ptr_->augment(vn, vrn);
slam_filter_ptr_->update(vf, vrf, &ids);
slam_filter_ptr_->recoverMean();
RoverTrajectory->tEST[*STEP].x = (*slam_filter_ptr_->mu).at<double>(0, 0);
RoverTrajectory->tEST[*STEP].y = (*slam_filter_ptr_->mu).at<double>(1, 0);
RoverTrajectory->tEST[*STEP].z = (*slam_filter_ptr_->mu).at<double>(2, 0);
// Publishing the rover's estimated position in ROS
PMSLAM_Data_msg.TrajectoryOut.x = RoverTrajectory->tEST[*STEP].x;
PMSLAM_Data_msg.TrajectoryOut.y = RoverTrajectory->tEST[*STEP].y;
PMSLAM_Data_msg.TrajectoryOut.z = RoverTrajectory->tEST[*STEP].z;
//ROS_INFO("(%f, %f, %f)",PMSLAM_Data_msg.TrajectoryOut.x,PMSLAM_Data_msg.TrajectoryOut.y,PMSLAM_Data_msg.TrajectoryOut.z);
// update map
//#seg fault----------
for (unsigned int i = 0; i < SDatabase->size(); ++i)
{
if ((idVec[i] > 0) && (idVec[i]< (int)Map3D->map.size()))
{
//ROS_INFO("IDIDID: %d\nsizesize: %d",(*SURFDatabase)[i].id, Map3D->map.size());
//wrong id in database
id = Map3D->map[idVec[i]].matPos * 3 + 3;
Map3D->map[idVec[i]].position.x = (*slam_filter_ptr_->mu).at<double>(id, 0);
Map3D->map[idVec[i]].position.y = (*slam_filter_ptr_->mu).at<double>(id + 1, 0);
Map3D->map[idVec[i]].position.z = (*slam_filter_ptr_->mu).at<double>(id + 2, 0);
// Assigning data to the feature position and gtruth vectors
PMSLAM_Data_msg.MapOut.ID.push_back(idVec[i]);
PMSLAM_Data_msg.MapOut.positions.y.push_back(Map3D->map[idVec[i]].position.x);
PMSLAM_Data_msg.MapOut.positions.x.push_back(Map3D->map[idVec[i]].position.y);
PMSLAM_Data_msg.MapOut.positions.z.push_back(Map3D->map[idVec[i]].position.z);
}
}
}
//remove re-mappings
RoverTrajectory->tEST.clear();
delete RoverTrajectory;
}
void PatientLandMarksWidget::setTargetLandmark(QString uid, Vector3D p_target)
{
mServices->patient()->getPatientLandmarks()->setLandmark(Landmark(uid, p_target));
reporter()->playSampleSound();
}
void LandmarkPatientRegistrationWidget::setTargetLandmark(QString uid, Vector3D p_target)
{
mServices.patientModelService->getPatientLandmarks()->setLandmark(Landmark(uid, p_target));
reporter()->playSampleSound();
}
std::vector<Landmark> createLandmarks()
{
std::vector<Landmark> lmks;
SDL_Color color_red = {.r = 255, .g = 0, .b = 0, .a = 255 };
SDL_Color color_green = {.r = 0, .g = 255, .b = 0, .a = 255 };
SDL_Color color_blue = {.r = 0, .g = 0, .b = 255, .a = 255 };
SDL_Color color_purple = {.r = 255, .g = 0, .b = 255, .a = 255 };
lmks.push_back( Landmark(300.,300.,color_red));
lmks.push_back( Landmark(124.,478.,color_blue));
// lmks.push_back( Landmark(214.,312.,color_purple));
return lmks;
}
int main() {
if (SDL_Init(SDL_INIT_VIDEO) != 0){
std::cout << "SDL_Init Error: " << SDL_GetError() << std::endl;
return 1;
}
SDL_Window *win = SDL_CreateWindow("Robot Program", XSTART, YSTART, WWIDTH, WHEIGHT, SDL_WINDOW_SHOWN);
if (win == nullptr){
std::cout << "SDL_CreateWindow Error: " << SDL_GetError() << std::endl;
SDL_Quit();
return 1;
}
SDL_Renderer *ren = SDL_CreateRenderer(win, -1, SDL_RENDERER_ACCELERATED | SDL_RENDERER_PRESENTVSYNC);
if (ren == nullptr){
SDL_DestroyWindow(win);
std::cout << "SDL_CreateRenderer Error: " << SDL_GetError() << std::endl;
SDL_Quit();
return 1;
}
std::vector<Landmark> landmarks = createLandmarks();
SDL_Color orange {.r=255, .g=165, .b=0, .a=255};
SDL_Color red {.r=255, .g=0, .b=0, .a=255};
SDL_Color gray {.r=128, .g=128, .b=128, .a=255};
Robot robby(200, 200, 0.0, 20, red);
// Kalman filter stuff
int n = 3; // number of state variables (x,y,phi)
int m = 3; // number of measurements (landmark x, y, index)
// Best guess of initial states
Eigen::VectorXf x0(n); //[x, y, phi]
x0 << 200., 200., 0.0;
Robot robby_estimate(x0(0), x0(1), x0(2), 18, gray);
// control vector:
Eigen::VectorXf control(2); // [v, omega]
Eigen::MatrixXf A(n, n); // System dynamics matrix
Eigen::MatrixXf C(m, n); // Output matrix
Eigen::MatrixXf Q(n, n); // Process noise covariance
Eigen::MatrixXf R(m, m); // Measurement noise covariance
Eigen::MatrixXf covariance(n, n); // Estimate error covariance
// Reasonable covariance matrices
covariance << 5., .0, .0,
.0, 5., .0,
.0, .0, 5.;
R << 1.0, 0., 0.,
0., 1.0, 0.,
0., 0., 0.1;
Q << 0.1, 0.1, 0.1,
0.1, 0.1, 0.1,
0.1, 0.1, 0.1;
KalmanFilter kf(DT, A, C, Q, R, covariance);
float t0 = 0.0;
kf.init(t0, x0);
// rendering loop
int i = 0;
while (i < 10000) {
//First clear the renderer
SDL_RenderClear(ren);
//Draw the texture
SDL_SetRenderDrawColor(ren, 200, 200, 255, 255);
SDL_RenderClear(ren); // fill the scene with white
SDL_SetRenderDrawColor(ren, 0, 0, 0, 255);
// update renderer
SDL_RenderPresent(ren);
// move robot
SDL_PumpEvents();
const Uint8 *key_pressed = SDL_GetKeyboardState(NULL);
robby.move(key_pressed, control);
// measure landmark positions
auto observed_landmarks = robby.measureLandmarks(landmarks);
// get robot state
Eigen::VectorXf state = robby.get_state();
// Localize via Landmarks
kf.localization_landmarks(observed_landmarks, landmarks, control);
// get estimated state (x,y,phi) as estimated by the EKF
auto x_hat = kf.get_state();
printf("True x,y,phi: %f, %f, %f\n", state(0), state(1),state(2));
printf("Estimated x,y,phi: %f, %f, %f\n", x_hat(0), x_hat(1), x_hat(2));
// move robby estimate to new pose as calculated by kalman filter:
robby_estimate.setPose(x_hat(0), x_hat(1), x_hat(2));
robby_estimate.render(ren);
// render robot
robby.render(ren);
// render landmarks
for (auto lm = landmarks.begin(); lm != landmarks.end(); ++lm)
{
SDL_SetRenderDrawColor(ren, lm->id.r, lm->id.g, lm->id.b, lm->id.a);
lm->render(ren);
}
SDL_SetRenderDrawColor(ren, gray.r, gray.g, gray.b, gray.a);
// render sampled probability distribution
kf.renderSamples(ren);
SDL_RenderPresent(ren);
//Take a quick break after all that hard work
SDL_Delay(30);
if (key_pressed[SDL_SCANCODE_RETURN])
{
printf("Exiting Program!\n");
break;
}
i+=1;
}
SDL_DestroyRenderer(ren);
SDL_DestroyWindow(win);
SDL_Quit();
return EXIT_SUCCESS;
}
void mouseLeftPressed(float x, float y){
landmarks.push_back(Landmark(x - (float)window_width/2 , ((float)window_height/2) - y));
}
void setDefaultLandmarkPositions(){
landmarks.push_back(Landmark(-100,200));
landmarks.push_back(Landmark(300,0));
landmarks.push_back(Landmark(100,70));
}
