double getCameraSpeed(int &counter, Mat &frame,int frameNumber, Mat &flow) {
vector<float> GPSTime = getTimeStamps(GPS_TIMESTAMP_FILE);
vector<float> FrameTime = getTimeStamps(VIDEO_TIMESTAMP_FILE);
float cameraVelocity = 0.0;
while(FrameTime.at(frameNumber) > GPSTime.at(counter))
cameraVelocity += getGPSVelocity(counter++);
Rect frontOfCar = getRoadRect(frame);
Point2d avg = getRoadAvgVector(frontOfCar, flow);
double pixelVelocity = abs(length((float)avg.x, (float)avg.y));
double realToImage = pixelVelocity/cameraVelocity;
return realToImage;
}
bool visual_slam::TUMUtilities::writingResults(std::vector<visual_slam::Pose_6D> poses)
{
std::ofstream result_file ;
result_file.open(result_filename.c_str());
std::vector<std::string> timestamps = getTimeStamps();
for(int ind = 0 ; ind < poses.size();ind++)
{
DTFMatrix tmp = poses[ind].matrix();
FQuarterionRotation rotation = EigenUtilites::instance()->ExtractRotationMatrixAsQuaternion(tmp);
FTranslatonVec translation = EigenUtilites::instance()->ExtractTranslationVector(tmp);
std::string line = timestamps[ind];
line += " ";
line += std::to_string(translation(0));
line += " ";
line += std::to_string(translation(1));
line += " ";
line += std::to_string(translation(2));
line += " ";
line += std::to_string(rotation.x());
line += " ";
line += std::to_string(rotation.y());
line += " ";
line += std::to_string(rotation.z());
line += " ";
line += std::to_string(rotation.w());
line += "\n";
result_file << line ;
}
result_file.close();
}
