std::vector<tLineSegment<typename TrajectoryCollection::value_type::value_type>>
traclus(TrajectoryCollection &A, double eps, size_t minLines, partitioning_functional &part,
distance_functional &_d, progress_visitor &pvis)
{
typedef typename TrajectoryCollection::value_type TrajectoryType;
std::vector<size_t> index_map;
std::vector<tLineSegment<typename TrajectoryType::value_type>> segments;
TrajectoryType L;
size_t i=0;
pvis.init(A.size());
auto it = A.begin();
while (it != A.end())
{
if((*it).size() == 0) // ignore empty
continue;
TrajectoryType CPi;
CPi = part(*it);
for (size_t k=0; k < CPi.size() -1; k++)
{
segments.push_back(tLineSegment<typename TrajectoryType::value_type>(CPi[k],CPi[k+1],i));
}
it++;i++;
pvis(i,A.size(),"Phase 1: Segment Creation");
}
grouping(segments,eps,minLines,_d, pvis);
pvis.finish();
return segments;
};
cvg_bool Controller_StateMachine_v2::setTrajectory( TrajectoryType &trajectory, TrajectoryConfiguration traj_config) {
cvg_bool error_ocurred = false;
// Ensure that the trajectory is well configured
error_ocurred = trajectory.reset();
// if (error_ocurred)
// return error_ocurred;
if ( traj_config.chk_R <= 0 ) {
traj_config.set2defaultValues();
}
error_ocurred = trajectory.planify_trajectory(&traj_config);
if (error_ocurred)
return error_ocurred;
this->trajectory = trajectory;
return error_ocurred;
}
void SetNewImage(Viewer *viewer) {
vector<string> vstrImageLeft;
vector<string> vstrImageRight;
vector<double> vTimeStamp;
VecIMU vimus;
TrajectoryType gtTraj; // ground truth trajectory
LoadImages(leftFolder, rightFolder, timeFolder, vstrImageLeft, vstrImageRight, vTimeStamp);
LoadImus(imuFolder, vimus);
LoadGroundTruthTraj(groundTruthFile, gtTraj);
assert(!vstrImageLeft.empty());
assert(!vstrImageRight.empty());
assert(!vimus.empty());
assert(!gtTraj.empty());
LOG(INFO) << "Trajectory poses: " << gtTraj.size() << endl;
// read camera parameters
cv::FileStorage fsSettings(configFile, cv::FileStorage::READ);
assert(fsSettings.isOpened());
cv::Mat K_l, K_r, P_l, P_r, R_l, R_r, D_l, D_r;
fsSettings["LEFT.K"] >> K_l;
fsSettings["RIGHT.K"] >> K_r;
fsSettings["LEFT.P"] >> P_l;
fsSettings["RIGHT.P"] >> P_r;
fsSettings["LEFT.R"] >> R_l;
fsSettings["RIGHT.R"] >> R_r;
fsSettings["LEFT.D"] >> D_l;
fsSettings["RIGHT.D"] >> D_r;
int rows_l = fsSettings["LEFT.height"];
int cols_l = fsSettings["LEFT.width"];
int rows_r = fsSettings["RIGHT.height"];
int cols_r = fsSettings["RIGHT.width"];
if (K_l.empty() || K_r.empty() || P_l.empty() || P_r.empty() || R_l.empty() || R_r.empty() || D_l.empty() ||
D_r.empty() ||
rows_l == 0 || rows_r == 0 || cols_l == 0 || cols_r == 0) {
LOG(ERROR) << "ERROR: Calibration parameters to rectify stereo are missing!" << endl;
return;
}
cv::Mat M1l, M2l, M1r, M2r;
cv::initUndistortRectifyMap(K_l, D_l, R_l, P_l.rowRange(0, 3).colRange(0, 3), cv::Size(cols_l, rows_l), CV_32F, M1l,
M2l);
cv::initUndistortRectifyMap(K_r, D_r, R_r, P_r.rowRange(0, 3).colRange(0, 3), cv::Size(cols_r, rows_r), CV_32F, M1r,
M2r);
const int nImages = vstrImageLeft.size();
// Create camera object
setting::initSettings();
float fx = fsSettings["Camera.fx"];
float fy = fsSettings["Camera.fy"];
float cx = fsSettings["Camera.cx"];
float cy = fsSettings["Camera.cy"];
float bf = fsSettings["Camera.bf"];
shared_ptr<CameraParam> camera = make_shared<CameraParam>(fx, fy, cx, cy, bf);
cv::Mat imLeft, imRight, imLeftRect, imRightRect;
size_t imuIndex = 0;
auto gtPoseIter = gtTraj.begin();
SE3d Tiw; // T-world-initial
for (int ni = 0; ni < nImages; ni++) {
if (viewer->IsRunning() == false)
break;
// Read left and right images from file
imLeft = cv::imread(vstrImageLeft[ni], CV_LOAD_IMAGE_UNCHANGED);
imRight = cv::imread(vstrImageRight[ni], CV_LOAD_IMAGE_UNCHANGED);
cv::remap(imLeft, imLeftRect, M1l, M2l, cv::INTER_LINEAR);
cv::remap(imRight, imRightRect, M1r, M2r, cv::INTER_LINEAR);
VecIMU vimu;
double tframe = vTimeStamp[ni];
while (1) {
const ygz::IMUData &imudata = vimus[imuIndex];
if (imudata.mfTimeStamp >= tframe)
break;
vimu.push_back(imudata);
imuIndex++;
}
shared_ptr<Frame> frame = make_shared<Frame>(imLeftRect, imRightRect, tframe, camera, vimu);
LOG(INFO) << "Show frame " << frame->mnId << endl;
ORBExtractor extractor(ORBExtractor::ORB_SLAM2);
extractor.Detect(frame, true);
extractor.Detect(frame, false);
ORBMatcher matcher;
matcher.ComputeStereoMatches(frame);
// find its ground truth pose
for (auto iter = gtPoseIter; iter != gtTraj.end(); iter++) {
if (iter->first > tframe) {
gtPoseIter = iter;
break;
}
}
if (ni == 0)
Tiw = gtPoseIter->second.inverse();
LOG(INFO) << std::setprecision(18) << "Current frame time: " << tframe << ", pose time " << gtPoseIter->first
<< endl;
LOG(INFO) << "Frame " << frame->mnId << " pose is set to \n" << gtPoseIter->second.matrix() << endl;
frame->SetPose(Tiw * gtPoseIter->second); // 设置相对于第一个帧的Pose
viewer->SetCurrentFrame( frame );
usleep(3000);
}
//memory::CleanAllMemory();
camera = nullptr;
setting::destroySettings();
}
