// ------------------------------------------------------
// TestPrepareDetector
// ------------------------------------------------------
void TestPrepareDetector()
{
CStringList lst;
CConfigFileMemory cfg;
lst.loadFromFile(myInitFile);
cfg.setContent(lst);
int classifierType = cfg.read_int("Example", "classifierType", 0);
if (classifierType == 0) // Haar
cfg.write(
"CascadeClassifier", "cascadeFileName",
OPENCV_SRC_DIR +
"/data/haarcascades/haarcascade_frontalface_alt2.xml");
else if (classifierType == 1) // LBP
cfg.write(
"CascadeClassifier", "cascadeFileName",
OPENCV_SRC_DIR + "/data/lbpcascades/lbpcascade_frontalface.xml");
else
throw std::runtime_error("Incorrect cascade classifier type.");
showEachDetectedFace = cfg.read_bool("Example", "showEachDetectedFace", 0);
batchMode = cfg.read_bool("Example", "batchMode", false);
if (batchMode)
{
string str = cfg.read_string("Example", "rawlogs", "noRawlogs");
mySplit(str);
size_t numRawlogs = rawlogs.size();
falsePositives.resize(numRawlogs);
ignored.resize(numRawlogs);
for (size_t i = 0; i < numRawlogs; i++)
{
cfg.read_vector(
rawlogs[i], "falsePositives", vector_uint(), falsePositives[i]);
cfg.read_vector(rawlogs[i], "ignored", vector_uint(), ignored[i]);
}
rawlogsDir = cfg.read_string("Example", "rawlogsDir", "");
}
faceDetector.init(cfg);
}
/** This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
* The default implementation in this base class relies on \a saveToConfigFile() to generate a plain text representation of all the parameters.
*/
void CLoadableOptions::dumpToTextStream(CStream &out) const
{
CConfigFileMemory cfg;
this->saveToConfigFile(cfg,"");
out.printf("%s", cfg.getContent().c_str() );
}
/*---------------------------------------------------------------
path_from_rtk_gps
---------------------------------------------------------------*/
void mrpt::topography::path_from_rtk_gps(
mrpt::poses::CPose3DInterpolator &robot_path,
const mrpt::slam::CRawlog &rawlog,
size_t first,
size_t last,
bool isGUI,
bool disableGPSInterp,
int PATH_SMOOTH_FILTER ,
TPathFromRTKInfo *outInfo
)
{
MRPT_START
#if MRPT_HAS_WXWIDGETS
// Use a smart pointer so we are safe against exceptions:
stlplus::smart_ptr<wxBusyCursor> waitCursorPtr;
if (isGUI)
waitCursorPtr = stlplus::smart_ptr<wxBusyCursor>( new wxBusyCursor() );
#endif
// Go: generate the map:
size_t i;
ASSERT_(first<=last);
ASSERT_(last<=rawlog.size()-1);
set<string> lstGPSLabels;
size_t count = 0;
robot_path.clear();
robot_path.setMaxTimeInterpolation(3.0); // Max. seconds of GPS blackout not to interpolate.
robot_path.setInterpolationMethod( CPose3DInterpolator::imSSLSLL );
TPathFromRTKInfo outInfoTemp;
if (outInfo) *outInfo = outInfoTemp;
map<string, map<TTimeStamp,TPoint3D> > gps_paths; // label -> (time -> 3D local coords)
bool abort = false;
bool ref_valid = false;
// Load configuration block:
CConfigFileMemory memFil;
rawlog.getCommentTextAsConfigFile(memFil);
TGeodeticCoords ref(
memFil.read_double("GPS_ORIGIN","lat_deg",0),
memFil.read_double("GPS_ORIGIN","lon_deg",0),
memFil.read_double("GPS_ORIGIN","height",0) );
ref_valid = !ref.isClear();
// Do we have info for the consistency test?
const double std_0 = memFil.read_double("CONSISTENCY_TEST","std_0",0);
bool doConsistencyCheck = std_0>0;
// Do we have the "reference uncertainty" matrix W^\star ??
memFil.read_matrix("UNCERTAINTY","W_star",outInfoTemp.W_star);
const bool doUncertaintyCovs = size(outInfoTemp.W_star,1)!=0;
if (doUncertaintyCovs && (size(outInfoTemp.W_star,1)!=6 || size(outInfoTemp.W_star,2)!=6))
THROW_EXCEPTION("ERROR: W_star matrix for uncertainty estimation is provided but it's not a 6x6 matrix.");
// ------------------------------------------
// Look for the 2 observations:
// ------------------------------------------
#if MRPT_HAS_WXWIDGETS
wxProgressDialog *progDia=NULL;
if (isGUI)
{
progDia = new wxProgressDialog(
wxT("Building map"),
wxT("Getting GPS observations..."),
(int)(last-first+1), // range
NULL, // parent
wxPD_CAN_ABORT |
wxPD_APP_MODAL |
wxPD_SMOOTH |
wxPD_AUTO_HIDE |
wxPD_ELAPSED_TIME |
wxPD_ESTIMATED_TIME |
wxPD_REMAINING_TIME);
}
#endif
// The list with all time ordered gps's in valid RTK mode
typedef std::map< mrpt::system::TTimeStamp, std::map<std::string,CObservationGPSPtr> > TListGPSs;
TListGPSs list_gps_obs;
map<string,size_t> GPS_RTK_reads; // label-># of RTK readings
map<string,TPoint3D> GPS_local_coords_on_vehicle; // label -> local pose on the vehicle
for (i=first;!abort && i<=last;i++)
{
switch ( rawlog.getType(i) )
{
default:
break;
case CRawlog::etObservation:
{
CObservationPtr o = rawlog.getAsObservation(i);
if (o->GetRuntimeClass()==CLASS_ID(CObservationGPS))
{
CObservationGPSPtr obs = CObservationGPSPtr(o);
if (obs->has_GGA_datum && obs->GGA_datum.fix_quality==4)
{
// Add to the list:
list_gps_obs[obs->timestamp][obs->sensorLabel] = obs;
lstGPSLabels.insert(obs->sensorLabel);
}
// Save to GPS paths:
if (obs->has_GGA_datum && (obs->GGA_datum.fix_quality==4 || obs->GGA_datum.fix_quality==5))
{
GPS_RTK_reads[obs->sensorLabel]++;
// map<string,TPoint3D> GPS_local_coords_on_vehicle; // label -> local pose on the vehicle
if (GPS_local_coords_on_vehicle.find(obs->sensorLabel)==GPS_local_coords_on_vehicle.end())
GPS_local_coords_on_vehicle[obs->sensorLabel] = TPoint3D( obs->sensorPose );
//map<string, map<TTimeStamp,TPoint3D> > gps_paths; // label -> (time -> 3D local coords)
gps_paths[obs->sensorLabel][obs->timestamp] = TPoint3D(
obs->GGA_datum.longitude_degrees,
obs->GGA_datum.latitude_degrees,
obs->GGA_datum.altitude_meters );
}
}
}
break;
} // end switch type
// Show progress:
if ((count++ % 100)==0)
{
#if MRPT_HAS_WXWIDGETS
if (progDia)
{
if (!progDia->Update((int)(i-first)))
abort = true;
wxTheApp->Yield();
}
#endif
}
} // end for i
#if MRPT_HAS_WXWIDGETS
if (progDia)
{
delete progDia;
progDia=NULL;
}
#endif
// -----------------------------------------------------------
// At this point we already have the sensor positions, thus
// we can estimate the covariance matrix D:
//
// TODO: Generalize equations for # of GPS > 3
// -----------------------------------------------------------
map< set<string>, double > Ad_ij; // InterGPS distances in 2D
map< set<string>, double > phi_ij; // Directions on XY of the lines between i-j
map< string, size_t> D_cov_indexes; // Sensor label-> index in the matrix (first=0, ...)
map< size_t, string> D_cov_rev_indexes; // Reverse of D_cov_indexes
CMatrixDouble D_cov; // square distances cov
CMatrixDouble D_cov_1; // square distances cov (inverse)
vector_double D_mean; // square distances mean
if (doConsistencyCheck && GPS_local_coords_on_vehicle.size()==3)
{
unsigned int cnt = 0;
for(map<string,TPoint3D>::iterator i=GPS_local_coords_on_vehicle.begin();i!=GPS_local_coords_on_vehicle.end();++i)
{
// Index tables:
D_cov_indexes[i->first] = cnt;
D_cov_rev_indexes[cnt] = i->first;
cnt++;
for(map<string,TPoint3D>::iterator j=i;j!=GPS_local_coords_on_vehicle.end();++j)
{
if (i!=j)
{
const TPoint3D &pi = i->second;
const TPoint3D &pj = j->second;
Ad_ij[ make_set(i->first,j->first) ] = pi.distanceTo( pj );
phi_ij[ make_set(i->first,j->first) ] = atan2( pj.y-pi.y, pj.x-pi.x );
}
}
}
ASSERT_( D_cov_indexes.size()==3 && D_cov_rev_indexes.size()==3 );
D_cov.setSize( D_cov_indexes.size(), D_cov_indexes.size() );
D_mean.resize( D_cov_indexes.size() );
// See paper for the formulas!
// TODO: generalize for N>3
D_cov(0,0) = 2*square( Ad_ij[ make_set(D_cov_rev_indexes[0],D_cov_rev_indexes[1])] );
D_cov(1,1) = 2*square( Ad_ij[ make_set(D_cov_rev_indexes[0],D_cov_rev_indexes[2])] );
D_cov(2,2) = 2*square( Ad_ij[ make_set(D_cov_rev_indexes[1],D_cov_rev_indexes[2])] );
D_cov(1,0) = Ad_ij[ make_set(D_cov_rev_indexes[0],D_cov_rev_indexes[1])] * Ad_ij[ make_set(D_cov_rev_indexes[0],D_cov_rev_indexes[2])]
* cos( phi_ij[ make_set(D_cov_rev_indexes[0],D_cov_rev_indexes[1])] - phi_ij[ make_set(D_cov_rev_indexes[0],D_cov_rev_indexes[2])] );
D_cov(0,1) = D_cov(1,0);
D_cov(2,0) =-Ad_ij[ make_set(D_cov_rev_indexes[0],D_cov_rev_indexes[1])] * Ad_ij[ make_set(D_cov_rev_indexes[1],D_cov_rev_indexes[2])]
* cos( phi_ij[ make_set(D_cov_rev_indexes[0],D_cov_rev_indexes[1])] - phi_ij[ make_set(D_cov_rev_indexes[1],D_cov_rev_indexes[2])] );
D_cov(0,2) = D_cov(2,0);
D_cov(2,1) = Ad_ij[ make_set(D_cov_rev_indexes[0],D_cov_rev_indexes[2])] * Ad_ij[ make_set(D_cov_rev_indexes[1],D_cov_rev_indexes[2])]
* cos( phi_ij[ make_set(D_cov_rev_indexes[0],D_cov_rev_indexes[2])] - phi_ij[ make_set(D_cov_rev_indexes[1],D_cov_rev_indexes[2])] );
D_cov(1,2) = D_cov(2,1);
D_cov *= 4*square(std_0);
D_cov_1 = D_cov.inv();
//cout << D_cov.inMatlabFormat() << endl;
D_mean[0] = square( Ad_ij[ make_set(D_cov_rev_indexes[0],D_cov_rev_indexes[1])] );
D_mean[1] = square( Ad_ij[ make_set(D_cov_rev_indexes[0],D_cov_rev_indexes[2])] );
D_mean[2] = square( Ad_ij[ make_set(D_cov_rev_indexes[1],D_cov_rev_indexes[2])] );
}
else doConsistencyCheck =false;
// ------------------------------------------
// Look for the 2 observations:
// ------------------------------------------
int N_GPSs = list_gps_obs.size();
if (N_GPSs)
{
// loop interpolate 1-out-of-5: this solves the issue with JAVAD GPSs
// that skip some readings at some times .0 .2 .4 .6 .8
if (list_gps_obs.size()>4)
{
TListGPSs::iterator F = list_gps_obs.begin();
++F; ++F;
TListGPSs::iterator E = list_gps_obs.end();
--E; --E;
for (TListGPSs::iterator i=F;i!=E;++i)
{
// Now check if we have 3 gps with the same time stamp:
//const size_t N = i->second.size();
std::map<std::string, CObservationGPSPtr> & GPS = i->second;
// Check if any in "lstGPSLabels" is missing here:
for (set<string>::iterator l=lstGPSLabels.begin();l!=lstGPSLabels.end();++l)
{
// For each GPS in the current timestamp:
bool fnd = ( GPS.find(*l)!=GPS.end() );
if (fnd) continue; // this one is present.
// Ok, we have "*l" missing in the set "*i".
// Try to interpolate from neighbors:
TListGPSs::iterator i_b1 = i; --i_b1;
TListGPSs::iterator i_a1 = i; ++i_a1;
CObservationGPSPtr GPS_b1, GPS_a1;
if (i_b1->second.find( *l )!=i_b1->second.end())
GPS_b1 = i_b1->second.find( *l )->second;
if (i_a1->second.find( *l )!=i_a1->second.end())
GPS_a1 = i_a1->second.find( *l )->second;
if (!disableGPSInterp && GPS_a1 && GPS_b1)
{
if ( mrpt::system::timeDifference( GPS_b1->timestamp, GPS_a1->timestamp ) < 0.5 )
{
CObservationGPSPtr new_gps = CObservationGPSPtr( new CObservationGPS(*GPS_a1) );
new_gps->sensorLabel = *l;
//cout << mrpt::system::timeLocalToString(GPS_b1->timestamp) << " " << mrpt::system::timeLocalToString(GPS_a1->timestamp) << " " << *l;
//cout << endl;
new_gps->GGA_datum.longitude_degrees = 0.5 * ( GPS_a1->GGA_datum.longitude_degrees + GPS_b1->GGA_datum.longitude_degrees );
new_gps->GGA_datum.latitude_degrees = 0.5 * ( GPS_a1->GGA_datum.latitude_degrees + GPS_b1->GGA_datum.latitude_degrees );
new_gps->GGA_datum.altitude_meters = 0.5 * ( GPS_a1->GGA_datum.altitude_meters + GPS_b1->GGA_datum.altitude_meters );
new_gps->timestamp = (GPS_a1->timestamp + GPS_b1->timestamp) / 2;
i->second[new_gps->sensorLabel] = new_gps;
}
}
}
} // end loop interpolate 1-out-of-5
}
#if MRPT_HAS_WXWIDGETS
wxProgressDialog *progDia3=NULL;
if (isGUI)
{
progDia3 = new wxProgressDialog(
wxT("Building map"),
wxT("Estimating 6D path..."),
N_GPSs, // range
NULL, // parent
wxPD_CAN_ABORT |
wxPD_APP_MODAL |
wxPD_SMOOTH |
wxPD_AUTO_HIDE |
wxPD_ELAPSED_TIME |
wxPD_ESTIMATED_TIME |
wxPD_REMAINING_TIME);
}
#endif
int idx_in_GPSs = 0;
for (TListGPSs::iterator i=list_gps_obs.begin();i!=list_gps_obs.end();++i, idx_in_GPSs++)
{
// Now check if we have 3 gps with the same time stamp:
if (i->second.size()>=3)
{
const size_t N = i->second.size();
std::map<std::string, CObservationGPSPtr> & GPS = i->second;
vector_double X(N),Y(N),Z(N); // Global XYZ coordinates
std::map<string,size_t> XYZidxs; // Sensor label -> indices in X Y Z
if (!ref_valid) // get the reference lat/lon, if it's not set from rawlog configuration block.
{
ref_valid = true;
ref = GPS.begin()->second->GGA_datum.getAsStruct<TGeodeticCoords>();
}
// Compute the XYZ coordinates of all sensors:
TMatchingPairList corrs;
unsigned int k;
std::map<std::string, CObservationGPSPtr>::iterator g_it;
for (k=0,g_it=GPS.begin();g_it!=GPS.end();g_it++,k++)
{
TPoint3D P;
mrpt::topography::geodeticToENU_WGS84(
g_it->second->GGA_datum.getAsStruct<TGeodeticCoords>(),
P,
ref );
// Correction of offsets:
const string sect = string("OFFSET_")+g_it->second->sensorLabel;
P.x += memFil.read_double( sect, "x", 0 );
P.y += memFil.read_double( sect, "y", 0 );
P.z += memFil.read_double( sect, "z", 0 );
XYZidxs[g_it->second->sensorLabel] = k; // Save index correspondence
// Create the correspondence:
corrs.push_back( TMatchingPair(
k,k, // Indices
P.x,P.y,P.z, // "This"/Global coords
g_it->second->sensorPose.x(),g_it->second->sensorPose.y(),g_it->second->sensorPose.z() // "other"/local coordinates
));
X[k] = P.x;
Y[k] = P.y;
Z[k] = P.z;
}
if (doConsistencyCheck && GPS.size()==3)
{
// XYZ[k] have the k'd final coordinates of each GPS
// GPS[k] are the CObservations:
// Compute the inter-GPS square distances:
vector_double iGPSdist2(3);
// [0]: sq dist between: D_cov_rev_indexes[0],D_cov_rev_indexes[1]
TPoint3D P0( X[XYZidxs[D_cov_rev_indexes[0]]], Y[XYZidxs[D_cov_rev_indexes[0]]], Z[XYZidxs[D_cov_rev_indexes[0]]] );
TPoint3D P1( X[XYZidxs[D_cov_rev_indexes[1]]], Y[XYZidxs[D_cov_rev_indexes[1]]], Z[XYZidxs[D_cov_rev_indexes[1]]] );
TPoint3D P2( X[XYZidxs[D_cov_rev_indexes[2]]], Y[XYZidxs[D_cov_rev_indexes[2]]], Z[XYZidxs[D_cov_rev_indexes[2]]] );
iGPSdist2[0] = P0.sqrDistanceTo(P1);
iGPSdist2[1] = P0.sqrDistanceTo(P2);
iGPSdist2[2] = P1.sqrDistanceTo(P2);
double mahaD = mrpt::math::mahalanobisDistance( iGPSdist2, D_mean, D_cov_1 );
outInfoTemp.mahalabis_quality_measure[i->first] = mahaD;
//cout << "x: " << iGPSdist2 << " MU: " << D_mean << " -> " << mahaD << endl;
} // end consistency
// Use a 6D matching method to estimate the location of the vehicle:
CPose3D optimal_pose;
double optimal_scale;
// "this" (reference map) -> GPS global coordinates
// "other" -> GPS local coordinates on the vehicle
mrpt::scanmatching::leastSquareErrorRigidTransformation6D( corrs,optimal_pose,optimal_scale, true ); // Force scale=1
//cout << "optimal pose: " << optimal_pose << " " << optimal_scale << endl;
// Final vehicle pose:
CPose3D &veh_pose= optimal_pose;
// Add to the interpolator:
MRPT_CHECK_NORMAL_NUMBER( veh_pose.x() );
MRPT_CHECK_NORMAL_NUMBER( veh_pose.y() );
MRPT_CHECK_NORMAL_NUMBER( veh_pose.z() );
MRPT_CHECK_NORMAL_NUMBER( veh_pose.yaw() );
MRPT_CHECK_NORMAL_NUMBER( veh_pose.pitch() );
MRPT_CHECK_NORMAL_NUMBER( veh_pose.roll() );
robot_path.insert( i->first, veh_pose );
// If we have W_star, compute the pose uncertainty:
if (doUncertaintyCovs)
{
CPose3DPDFGaussian final_veh_uncert;
final_veh_uncert.mean.setFromValues(0,0,0,0,0,0);
final_veh_uncert.cov = outInfoTemp.W_star;
// Rotate the covariance according to the real vehicle pose:
final_veh_uncert.changeCoordinatesReference(veh_pose);
outInfoTemp.vehicle_uncertainty[ i->first ] = final_veh_uncert.cov;
}
}
// Show progress:
if ((count++ % 100)==0)
{
#if MRPT_HAS_WXWIDGETS
if (progDia3)
{
if (!progDia3->Update(idx_in_GPSs))
abort = true;
wxTheApp->Yield();
}
#endif
}
} // end for i
#if MRPT_HAS_WXWIDGETS
if (progDia3)
{
delete progDia3;
progDia3=NULL;
}
#endif
if (PATH_SMOOTH_FILTER>0 && robot_path.size()>1)
{
CPose3DInterpolator filtered_robot_path = robot_path;
// Do Angles smoother filter of the path:
// ---------------------------------------------
const double MAX_DIST_TO_FILTER = 4.0;
for (CPose3DInterpolator::iterator i=robot_path.begin();i!=robot_path.end();++i)
{
CPose3D p = i->second;
vector_double pitchs, rolls; // The elements to average
pitchs.push_back(p.pitch());
rolls.push_back(p.roll());
CPose3DInterpolator::iterator q=i;
for (int k=0;k<PATH_SMOOTH_FILTER && q!=robot_path.begin();k++)
{
--q;
if (abs( mrpt::system::timeDifference(q->first,i->first))<MAX_DIST_TO_FILTER )
{
pitchs.push_back( q->second.pitch() );
rolls.push_back( q->second.roll() );
}
}
q=i;
for (int k=0;k<PATH_SMOOTH_FILTER && q!=(--robot_path.end()) ;k++)
{
++q;
if (abs( mrpt::system::timeDifference(q->first,i->first))<MAX_DIST_TO_FILTER )
{
pitchs.push_back( q->second.pitch() );
rolls.push_back( q->second.roll() );
}
}
p.setYawPitchRoll(p.yaw(), mrpt::math::averageWrap2Pi(pitchs), mrpt::math::averageWrap2Pi(rolls) );
// save in filtered path:
filtered_robot_path.insert( i->first, p );
}
// Replace:
robot_path = filtered_robot_path;
} // end PATH_SMOOTH_FILTER
} // end step generate 6D path
// Here we can set best_gps_path (that with the max. number of RTK fixed/foat readings):
if (outInfo)
{
string bestLabel;
size_t bestNum = 0;
for (map<string,size_t>::iterator i=GPS_RTK_reads.begin();i!=GPS_RTK_reads.end();++i)
{
if (i->second>bestNum)
{
bestNum = i->second;
bestLabel = i->first;
}
}
outInfoTemp.best_gps_path = gps_paths[bestLabel];
// and transform to XYZ:
// Correction of offsets:
const string sect = string("OFFSET_")+bestLabel;
const double off_X = memFil.read_double( sect, "x", 0 );
const double off_Y = memFil.read_double( sect, "y", 0 );
const double off_Z = memFil.read_double( sect, "z", 0 );
// map<TTimeStamp,TPoint3D> best_gps_path; // time -> 3D local coords
for (map<TTimeStamp,TPoint3D>::iterator i=outInfoTemp.best_gps_path.begin();i!=outInfoTemp.best_gps_path.end();++i)
{
TPoint3D P;
mrpt::topography::geodeticToENU_WGS84(
TGeodeticCoords(i->second.x,i->second.y,i->second.z), // i->second.x,i->second.y,i->second.z, // lat, lon, heigh
P, // X Y Z
ref );
i->second.x = P.x + off_X;
i->second.y = P.y + off_Y;
i->second.z = P.z + off_Z;
}
} // end best_gps_path
if (outInfo) *outInfo = outInfoTemp;
MRPT_END
}
/* -------------------------------------------------------
checkerBoardCameraCalibration
------------------------------------------------------- */
bool mrpt::vision::checkerBoardCameraCalibration(
TCalibrationImageList &images,
unsigned int check_size_x,
unsigned int check_size_y,
double check_squares_length_X_meters,
double check_squares_length_Y_meters,
mrpt::utils::TCamera &out_camera_params,
bool normalize_image,
double *out_MSE,
bool skipDrawDetectedImgs,
bool useScaramuzzaAlternativeDetector
)
{
#if MRPT_HAS_OPENCV
try
{
ASSERT_(check_size_x>2)
ASSERT_(check_size_y>2)
ASSERT_(check_squares_length_X_meters>0)
ASSERT_(check_squares_length_Y_meters>0)
if (images.size()<1)
{
std::cout << "ERROR: No input images." << std::endl;
return false;
}
const unsigned CORNERS_COUNT = check_size_x * check_size_y;
const CvSize check_size = cvSize(check_size_x, check_size_y);
// First: Assure all images are loaded:
// -------------------------------------------
TCalibrationImageList::iterator it;
for (it=images.begin();it!=images.end();it++)
{
TImageCalibData &dat = it->second;
dat.projectedPoints_distorted.clear(); // Clear reprojected points.
dat.projectedPoints_undistorted.clear();
// Skip if images are marked as "externalStorage":
if (!dat.img_original.isExternallyStored() && !mrpt::system::extractFileExtension(it->first).empty() )
{
if (!dat.img_original.loadFromFile(it->first))
THROW_EXCEPTION_CUSTOM_MSG1("Error reading image: %s",it->first.c_str());
dat.img_checkboard = dat.img_original;
dat.img_rectified = dat.img_original;
}
}
// For each image, find checkerboard corners:
// -----------------------------------------------
//const unsigned int N = images.size();
unsigned int i;
vector<CvPoint2D64f> corners_list; // = new CvPoint2D32f[ N * CORNERS_COUNT];
unsigned int valid_detected_imgs = 0;
CvSize imgSize = cvSize(0,0);
vector<string> pointsIdx2imageFile;
int find_chess_flags = CV_CALIB_CB_ADAPTIVE_THRESH;
if (normalize_image)
find_chess_flags |= CV_CALIB_CB_NORMALIZE_IMAGE;
for (i=0,it=images.begin();it!=images.end();it++,i++)
{
TImageCalibData &dat = it->second;
// Make grayscale version:
const CImage img_gray( dat.img_original, FAST_REF_OR_CONVERT_TO_GRAY );
if (!i)
{
imgSize = cvSize(img_gray.getWidth(),img_gray.getHeight() );
out_camera_params.ncols = imgSize.width;
out_camera_params.nrows = imgSize.height;
}
else
{
if (imgSize.height != (int)img_gray.getHeight() || imgSize.width != (int)img_gray.getWidth())
{
std::cout << "ERROR: All the images must have the same size" << std::endl;
return false;
}
}
// Try with expanded versions of the image if it fails to detect the checkerboard:
unsigned corners_count;
bool corners_found=false;
corners_count = CORNERS_COUNT;
corners_list.resize( (1+valid_detected_imgs)*CORNERS_COUNT );
dat.detected_corners.clear();
// Do detection (this includes the "refine corners" with cvFindCornerSubPix):
vector<TPixelCoordf> detectedCoords;
corners_found = mrpt::vision::findChessboardCorners(
img_gray,
detectedCoords,
check_size_x,check_size_y,
normalize_image, // normalize_image
useScaramuzzaAlternativeDetector
);
corners_count = detectedCoords.size();
// Copy the data into the overall array of coords:
ASSERT_(detectedCoords.size()<=CORNERS_COUNT);
for (size_t p=0;p<detectedCoords.size();p++)
{
corners_list[valid_detected_imgs*CORNERS_COUNT+p].x = detectedCoords[p].x;
corners_list[valid_detected_imgs*CORNERS_COUNT+p].y = detectedCoords[p].y;
}
if (corners_found && corners_count!=CORNERS_COUNT)
corners_found = false;
cout << format("Img %s: %s\n", mrpt::system::extractFileName(it->first).c_str() , corners_found ? "DETECTED" : "NOT DETECTED" );
if( corners_found )
{
// save the corners in the data structure:
int x, y;
unsigned int k;
for( y = 0, k = 0; y < check_size.height; y++ )
for( x = 0; x < check_size.width; x++, k++ )
dat.detected_corners.push_back( mrpt::utils::TPixelCoordf( corners_list[valid_detected_imgs*CORNERS_COUNT + k].x, corners_list[valid_detected_imgs*CORNERS_COUNT + k].y ) );
// Draw the checkerboard in the corresponding image:
// ----------------------------------------------------
if ( !dat.img_original.isExternallyStored() )
{
const int r = 4;
CvPoint prev_pt= cvPoint(0, 0);
const int line_max = 8;
CvScalar line_colors[8];
line_colors[0] = CV_RGB(255,0,0);
line_colors[1] = CV_RGB(255,128,0);
line_colors[2] = CV_RGB(255,128,0);
line_colors[3] = CV_RGB(200,200,0);
line_colors[4] = CV_RGB(0,255,0);
line_colors[5] = CV_RGB(0,200,200);
line_colors[6] = CV_RGB(0,0,255);
line_colors[7] = CV_RGB(255,0,255);
// Checkboad as color image:
dat.img_original.colorImage( dat.img_checkboard );
void *rgb_img = dat.img_checkboard.getAs<IplImage>();
for( y = 0, k = 0; y < check_size.height; y++ )
{
CvScalar color = line_colors[y % line_max];
for( x = 0; x < check_size.width; x++, k++ )
{
CvPoint pt;
pt.x = cvRound(corners_list[valid_detected_imgs*CORNERS_COUNT + k].x);
pt.y = cvRound(corners_list[valid_detected_imgs*CORNERS_COUNT + k].y);
if( k != 0 ) cvLine( rgb_img, prev_pt, pt, color );
cvLine( rgb_img,
cvPoint( pt.x - r, pt.y - r ),
cvPoint( pt.x + r, pt.y + r ), color );
cvLine( rgb_img,
cvPoint( pt.x - r, pt.y + r),
cvPoint( pt.x + r, pt.y - r), color );
cvCircle( rgb_img, pt, r+1, color );
prev_pt = pt;
}
}
}
}
if( corners_found )
{
pointsIdx2imageFile.push_back( it->first );
valid_detected_imgs++;
}
} // end find corners
std::cout << valid_detected_imgs << " valid images." << std::endl;
if (!valid_detected_imgs)
{
std::cout << "ERROR: No valid images. Perhaps the checkerboard size is incorrect?" << std::endl;
return false;
}
// ---------------------------------------------
// Calculate the camera parameters
// ---------------------------------------------
// Was: FillEtalonObjPoints
vector<CvPoint3D64f> obj_points( valid_detected_imgs * CORNERS_COUNT );
{
unsigned int y,k;
for( y = 0, k = 0; y < check_size_y; y++ )
{
for( unsigned int x = 0; x < check_size_x; x++, k++ )
{
obj_points[k].x =-check_squares_length_X_meters * x; // The "-" is for convenience, so the camera poses appear with Z>0
obj_points[k].y = check_squares_length_Y_meters * y;
obj_points[k].z = 0;
}
}
}
// Repeat the pattern N times:
for( i= 1; i< valid_detected_imgs; i++ )
memcpy( &obj_points[CORNERS_COUNT*i], &obj_points[0], CORNERS_COUNT*sizeof(obj_points[0]));
// Number of detected points in each image (constant):
vector<int> numsPoints(valid_detected_imgs, (int)CORNERS_COUNT );
double proj_matrix[9];
double distortion[4];
vector<CvPoint3D64f> transVects( valid_detected_imgs );
vector<double> rotMatrs( valid_detected_imgs * 9 );
// Calibrate camera
cvCalibrateCamera_64d(
valid_detected_imgs,
&numsPoints[0],
imgSize,
&corners_list[0],
&obj_points[0],
distortion,
proj_matrix,
(double*)&transVects[0],
&rotMatrs[0],
0 );
// Load matrix:
out_camera_params.intrinsicParams = CMatrixDouble33( proj_matrix );
out_camera_params.dist.assign(0);
for (int i=0;i<4;i++)
out_camera_params.dist[i] = distortion[i];
// Load camera poses:
for (i=0;i<valid_detected_imgs;i++)
{
const double *R = &rotMatrs[9*i];
CMatrixDouble HM(4,4);
HM.zeros();
HM(3,3)=1;
HM(0,0)=R[0];
HM(1,0)=R[3];
HM(2,0)=R[6];
HM(0,1)=R[1];
HM(1,1)=R[4];
HM(2,1)=R[7];
HM(0,2)=R[2];
HM(1,2)=R[5];
HM(2,2)=R[8];
HM(0,3)=transVects[i].x;
HM(1,3)=transVects[i].y;
HM(2,3)=transVects[i].z;
CPose3D p = CPose3D(0,0,0) - CPose3D(HM);
images[ pointsIdx2imageFile[i] ].reconstructed_camera_pose = p;
std::cout << "Img: " << mrpt::system::extractFileName(pointsIdx2imageFile[i]) << ": " << p << std::endl;
}
{
CConfigFileMemory cfg;
out_camera_params.saveToConfigFile("CAMERA_PARAMS",cfg);
std::cout << cfg.getContent() << std::endl;
}
// ----------------------------------------
// Undistort images:
// ----------------------------------------
for (it=images.begin();it!=images.end();it++)
{
TImageCalibData &dat = it->second;
if (!dat.img_original.isExternallyStored())
dat.img_original.rectifyImage( dat.img_rectified, out_camera_params);
} // end undistort
// -----------------------------------------------
// Reproject points to measure the fit sqr error
// -----------------------------------------------
double sqrErr = 0;
for (i=0;i<valid_detected_imgs;i++)
{
TImageCalibData & dat = images[ pointsIdx2imageFile[i] ];
if (dat.detected_corners.size()!=CORNERS_COUNT) continue;
// Reproject all the points into pixel coordinates:
// -----------------------------------------------------
vector<TPoint3D> lstPatternPoints(CORNERS_COUNT); // Points as seen from the camera:
for (unsigned int p=0;p<CORNERS_COUNT;p++)
lstPatternPoints[p] = TPoint3D(obj_points[p].x,obj_points[p].y,obj_points[p].z);
vector<TPixelCoordf> &projectedPoints = dat.projectedPoints_undistorted;
vector<TPixelCoordf> &projectedPoints_distorted = dat.projectedPoints_distorted;
vision::pinhole::projectPoints_no_distortion(
lstPatternPoints, // Input points
dat.reconstructed_camera_pose,
out_camera_params.intrinsicParams, // calib matrix
projectedPoints // Output points in pixels
);
vision::pinhole::projectPoints_with_distortion(
lstPatternPoints, // Input points
dat.reconstructed_camera_pose,
out_camera_params.intrinsicParams, // calib matrix
out_camera_params.getDistortionParamsAsVector(),
projectedPoints_distorted// Output points in pixels
);
ASSERT_(projectedPoints.size()==CORNERS_COUNT);
ASSERT_(projectedPoints_distorted.size()==CORNERS_COUNT);
for (unsigned int p=0;p<CORNERS_COUNT;p++)
{
const double px = projectedPoints[p].x;
const double py = projectedPoints[p].y;
const double px_d = projectedPoints_distorted[p].x;
const double py_d = projectedPoints_distorted[p].y;
// Only draw if the img is NOT external:
if (!dat.img_original.isExternallyStored())
{
if( px >= 0 && px < imgSize.width && py >= 0 && py < imgSize.height )
cvCircle( dat.img_rectified.getAs<IplImage>(), cvPoint(px,py), 4, CV_RGB(0,0,255) );
}
// Accumulate error:
sqrErr+=square(px_d-dat.detected_corners[p].x)+square(py_d-dat.detected_corners[p].y); // Error relative to the original (distorted) image.
}
}
if (valid_detected_imgs)
{
sqrErr /= CORNERS_COUNT*valid_detected_imgs;
std::cout << "Average err. of reprojection: " << sqrt(sqrErr) << " pixels" << std::endl;
}
if(out_MSE) *out_MSE = sqrt(sqrErr);
return true;
}
catch(std::exception &e)
{
std::cout << e.what() << std::endl;
return false;
}
#else
THROW_EXCEPTION("Function not available: MRPT was compiled without OpenCV")
#endif
}
// ------------------------------------------------------
// MAIN
// ------------------------------------------------------
int main(int argc, char **argv)
{
try
{
printf(" track-video-features - Part of MRPT\n");
printf(" MRPT C++ Library: %s - BUILD DATE %s\n", mrpt::system::MRPT_getVersion().c_str(), mrpt::system::MRPT_getCompilationDate().c_str());
printf("-------------------------------------------------------------------\n");
// The video source:
CCameraSensorPtr cam;
// process cmd line arguments?
if (argc<1 || argc>3)
{
cerr << "Incorrect number of arguments.\n";
showUsage(argv[0]);
return -1;
}
const bool last_arg_is_save_video = !strcmp("--save-video",argv[argc-1]);
if (last_arg_is_save_video)
argc--; // Discard last argument
if (argc==2)
{
if (!strcmp(argv[1],"--help"))
{
showUsage(argv[0]);
return 0;
}
if (!mrpt::system::fileExists(argv[1]))
{
cerr << "File does not exist: " << argv[1] << endl;
return -1;
}
const string fil = string(argv[1]);
const string ext = mrpt::system::lowerCase(mrpt::system::extractFileExtension(fil,true));
if (ext=="rawlog")
{
// It's a rawlog:
cout << "Interpreting '" << fil << "' as a rawlog file...\n";
cam = CCameraSensorPtr(new CCameraSensor);
CConfigFileMemory cfg;
cfg.write("CONFIG","grabber_type","rawlog");
cfg.write("CONFIG","rawlog_file", fil );
// For delayed-load images:
CImage::IMAGES_PATH_BASE = CRawlog::detectImagesDirectory(fil);
cam->loadConfig(cfg,"CONFIG");
cam->initialize(); // This will raise an exception if neccesary
}
else
{
// Assume it's a video:
cout << "Interpreting '" << fil << "' as a video file...\n";
cam = CCameraSensorPtr(new CCameraSensor);
CConfigFileMemory cfg;
cfg.write("CONFIG","grabber_type","ffmpeg");
cfg.write("CONFIG","ffmpeg_url", fil );
cam->loadConfig(cfg,"CONFIG");
cam->initialize(); // This will raise an exception if neccesary
}
}
if (!cam)
{
cout << "You didn't specify any video source in the command line.\n"
"(You can run with --help to see usage).\n"
"Showing a GUI window to select the video source...\n";
// If no camera opened so far, ask the user for one:
cam = mrpt::hwdrivers::prepareVideoSourceFromUserSelection();
if (!cam)
{
cerr << "No images source was correctly initialized! Exiting.\n";
return -1;
}
}
// do it:
const int ret = DoTrackingDemo(cam, last_arg_is_save_video);
win.clear();
mrpt::system::sleep(150); // give time to close GUI threads
return ret;
}
catch (std::exception &e)
{
std::cerr << e.what() << std::endl << "Program finished for an exception!!" << std::endl;
mrpt::system::pause();
return -1;
}
catch (...)
{
std::cerr << "Untyped exception!!" << std::endl;
mrpt::system::pause();
return -1;
}
}
/* -------------------------------------------------------
checkerBoardCameraCalibration
------------------------------------------------------- */
bool mrpt::vision::checkerBoardCameraCalibration(
TCalibrationImageList &images,
unsigned int check_size_x,
unsigned int check_size_y,
double check_squares_length_X_meters,
double check_squares_length_Y_meters,
mrpt::utils::TCamera &out_camera_params,
bool normalize_image,
double *out_MSE,
bool skipDrawDetectedImgs,
bool useScaramuzzaAlternativeDetector
)
{
MRPT_UNUSED_PARAM(skipDrawDetectedImgs);
#if MRPT_HAS_OPENCV
try
{
ASSERT_(check_size_x>2)
ASSERT_(check_size_y>2)
ASSERT_(check_squares_length_X_meters>0)
ASSERT_(check_squares_length_Y_meters>0)
if (images.size()<1)
{
std::cout << "ERROR: No input images." << std::endl;
return false;
}
const unsigned CORNERS_COUNT = check_size_x * check_size_y;
const CvSize check_size = cvSize(check_size_x, check_size_y);
// Fill the pattern of expected pattern points only once out of the loop:
vector<cv::Point3f> pattern_obj_points(CORNERS_COUNT);
{
unsigned int y,k;
for( y = 0, k = 0; y < check_size_y; y++ )
{
for( unsigned int x = 0; x < check_size_x; x++, k++ )
{
pattern_obj_points[k].x =-check_squares_length_X_meters * x; // The "-" is for convenience, so the camera poses appear with Z>0
pattern_obj_points[k].y = check_squares_length_Y_meters * y;
pattern_obj_points[k].z = 0;
}
}
}
// First: Assure all images are loaded:
// -------------------------------------------
TCalibrationImageList::iterator it;
for (it=images.begin();it!=images.end();++it)
{
TImageCalibData &dat = it->second;
dat.projectedPoints_distorted.clear(); // Clear reprojected points.
dat.projectedPoints_undistorted.clear();
// Skip if images are marked as "externalStorage":
if (!dat.img_original.isExternallyStored() && !mrpt::system::extractFileExtension(it->first).empty() )
{
if (!dat.img_original.loadFromFile(it->first))
THROW_EXCEPTION_CUSTOM_MSG1("Error reading image: %s",it->first.c_str());
dat.img_checkboard = dat.img_original;
dat.img_rectified = dat.img_original;
}
}
// For each image, find checkerboard corners:
// -----------------------------------------------
vector<vector<cv::Point3f> > objectPoints; // final container for detected stuff
vector<vector<cv::Point2f> > imagePoints; // final container for detected stuff
unsigned int valid_detected_imgs = 0;
vector<string> pointsIdx2imageFile;
cv::Size imgSize(0,0);
unsigned int i;
for (i=0,it=images.begin();it!=images.end();it++,i++)
{
TImageCalibData &dat = it->second;
// Make grayscale version:
const CImage img_gray( dat.img_original, FAST_REF_OR_CONVERT_TO_GRAY );
if (!i)
{
imgSize = cv::Size(img_gray.getWidth(),img_gray.getHeight() );
out_camera_params.ncols = imgSize.width;
out_camera_params.nrows = imgSize.height;
}
else
{
if (imgSize.height != (int)img_gray.getHeight() || imgSize.width != (int)img_gray.getWidth())
{
std::cout << "ERROR: All the images must have the same size" << std::endl;
return false;
}
}
// Try with expanded versions of the image if it fails to detect the checkerboard:
unsigned corners_count;
bool corners_found=false;
corners_count = CORNERS_COUNT;
vector<cv::Point2f> this_img_pts(CORNERS_COUNT); // Temporary buffer for points, to be added if the points pass the checks.
dat.detected_corners.clear();
// Do detection (this includes the "refine corners" with cvFindCornerSubPix):
vector<TPixelCoordf> detectedCoords;
corners_found = mrpt::vision::findChessboardCorners(
img_gray,
detectedCoords,
check_size_x,check_size_y,
normalize_image, // normalize_image
useScaramuzzaAlternativeDetector
);
corners_count = detectedCoords.size();
// Copy the data into the overall array of coords:
ASSERT_(detectedCoords.size()<=CORNERS_COUNT);
for (size_t p=0;p<detectedCoords.size();p++)
{
this_img_pts[p].x = detectedCoords[p].x;
this_img_pts[p].y = detectedCoords[p].y;
}
if (corners_found && corners_count!=CORNERS_COUNT)
corners_found = false;
cout << format("Img %s: %s\n", mrpt::system::extractFileName(it->first).c_str() , corners_found ? "DETECTED" : "NOT DETECTED" );
if( corners_found )
{
// save the corners in the data structure:
int x, y;
unsigned int k;
for( y = 0, k = 0; y < check_size.height; y++ )
for( x = 0; x < check_size.width; x++, k++ )
dat.detected_corners.push_back( mrpt::utils::TPixelCoordf( this_img_pts[k].x, this_img_pts[k].y ) );
// Draw the checkerboard in the corresponding image:
// ----------------------------------------------------
if ( !dat.img_original.isExternallyStored() )
{
const int r = 4;
CvPoint prev_pt= cvPoint(0, 0);
const int line_max = 8;
CvScalar line_colors[8];
line_colors[0] = CV_RGB(255,0,0);
line_colors[1] = CV_RGB(255,128,0);
line_colors[2] = CV_RGB(255,128,0);
line_colors[3] = CV_RGB(200,200,0);
line_colors[4] = CV_RGB(0,255,0);
line_colors[5] = CV_RGB(0,200,200);
line_colors[6] = CV_RGB(0,0,255);
line_colors[7] = CV_RGB(255,0,255);
// Checkboad as color image:
dat.img_original.colorImage( dat.img_checkboard );
void *rgb_img = dat.img_checkboard.getAs<IplImage>();
for( y = 0, k = 0; y < check_size.height; y++ )
{
CvScalar color = line_colors[y % line_max];
for( x = 0; x < check_size.width; x++, k++ )
{
CvPoint pt;
pt.x = cvRound(this_img_pts[k].x);
pt.y = cvRound(this_img_pts[k].y);
if( k != 0 ) cvLine( rgb_img, prev_pt, pt, color );
cvLine( rgb_img,
cvPoint( pt.x - r, pt.y - r ),
cvPoint( pt.x + r, pt.y + r ), color );
cvLine( rgb_img,
cvPoint( pt.x - r, pt.y + r),
cvPoint( pt.x + r, pt.y - r), color );
cvCircle( rgb_img, pt, r+1, color );
prev_pt = pt;
}
}
}
// Accept this image as good:
pointsIdx2imageFile.push_back( it->first );
imagePoints.push_back( this_img_pts );
objectPoints.push_back( pattern_obj_points );
valid_detected_imgs++;
}
} // end find corners
std::cout << valid_detected_imgs << " valid images." << std::endl;
if (!valid_detected_imgs)
{
std::cout << "ERROR: No valid images. Perhaps the checkerboard size is incorrect?" << std::endl;
return false;
}
// ---------------------------------------------
// Calculate the camera parameters
// ---------------------------------------------
// Calibrate camera
cv::Mat cameraMatrix, distCoeffs(1,5,CV_64F,cv::Scalar::all(0));
vector<cv::Mat> rvecs, tvecs;
const double cv_calib_err =
cv::calibrateCamera(
objectPoints,imagePoints,imgSize,
cameraMatrix, distCoeffs, rvecs, tvecs,
0 /*flags*/ );
// Load matrix:
out_camera_params.intrinsicParams = CMatrixDouble33( cameraMatrix.ptr<double>() );
out_camera_params.dist.assign(0);
for (int i=0;i<5;i++)
out_camera_params.dist[i] = distCoeffs.ptr<double>()[i];
// Load camera poses:
for (i=0;i<valid_detected_imgs;i++)
{
CMatrixDouble44 HM;
HM.zeros();
HM(3,3)=1;
{
// Convert rotation vectors -> rot matrices:
cv::Mat cv_rot;
cv::Rodrigues(rvecs[i],cv_rot);
Eigen::Matrix3d rot;
cv::my_cv2eigen(cv_rot, rot );
HM.block<3,3>(0,0) = rot;
}
{
Eigen::Matrix<double,3,1> trans;
cv::my_cv2eigen(tvecs[i], trans );
HM.block<3,1>(0,3) = trans;
}
CPose3D p = CPose3D(0,0,0) - CPose3D(HM);
images[ pointsIdx2imageFile[i] ].reconstructed_camera_pose = p;
std::cout << "Img: " << mrpt::system::extractFileName(pointsIdx2imageFile[i]) << ": " << p << std::endl;
}
{
CConfigFileMemory cfg;
out_camera_params.saveToConfigFile("CAMERA_PARAMS",cfg);
std::cout << cfg.getContent() << std::endl;
}
// ----------------------------------------
// Undistort images:
// ----------------------------------------
for (it=images.begin();it!=images.end();++it)
{
TImageCalibData &dat = it->second;
if (!dat.img_original.isExternallyStored())
dat.img_original.rectifyImage( dat.img_rectified, out_camera_params);
} // end undistort
// -----------------------------------------------
// Reproject points to measure the fit sqr error
// -----------------------------------------------
double sqrErr = 0;
for (i=0;i<valid_detected_imgs;i++)
{
TImageCalibData & dat = images[ pointsIdx2imageFile[i] ];
if (dat.detected_corners.size()!=CORNERS_COUNT) continue;
// Reproject all the points into pixel coordinates:
// -----------------------------------------------------
vector<TPoint3D> lstPatternPoints(CORNERS_COUNT); // Points as seen from the camera:
for (unsigned int p=0;p<CORNERS_COUNT;p++)
lstPatternPoints[p] = TPoint3D(pattern_obj_points[p].x,pattern_obj_points[p].y,pattern_obj_points[p].z);
vector<TPixelCoordf> &projectedPoints = dat.projectedPoints_undistorted;
vector<TPixelCoordf> &projectedPoints_distorted = dat.projectedPoints_distorted;
vision::pinhole::projectPoints_no_distortion(
lstPatternPoints, // Input points
dat.reconstructed_camera_pose,
out_camera_params.intrinsicParams, // calib matrix
projectedPoints // Output points in pixels
);
vision::pinhole::projectPoints_with_distortion(
lstPatternPoints, // Input points
dat.reconstructed_camera_pose,
out_camera_params.intrinsicParams, // calib matrix
out_camera_params.getDistortionParamsAsVector(),
projectedPoints_distorted// Output points in pixels
);
ASSERT_(projectedPoints.size()==CORNERS_COUNT);
ASSERT_(projectedPoints_distorted.size()==CORNERS_COUNT);
for (unsigned int p=0;p<CORNERS_COUNT;p++)
{
const double px = projectedPoints[p].x;
const double py = projectedPoints[p].y;
const double px_d = projectedPoints_distorted[p].x;
const double py_d = projectedPoints_distorted[p].y;
// Only draw if the img is NOT external:
if (!dat.img_original.isExternallyStored())
{
if( px >= 0 && px < imgSize.width && py >= 0 && py < imgSize.height )
cvCircle( dat.img_rectified.getAs<IplImage>(), cvPoint(px,py), 4, CV_RGB(0,0,255) );
}
// Accumulate error:
sqrErr+=square(px_d-dat.detected_corners[p].x)+square(py_d-dat.detected_corners[p].y); // Error relative to the original (distorted) image.
}
}
if (valid_detected_imgs)
{
sqrErr /= CORNERS_COUNT*valid_detected_imgs;
std::cout << "Average err. of reprojection: " << sqrt(sqrErr) << " pixels (OpenCV error=" << cv_calib_err << ")\n";
}
if(out_MSE) *out_MSE = sqrt(sqrErr);
return true;
}
catch(std::exception &e)
{
std::cout << e.what() << std::endl;
return false;
}
#else
THROW_EXCEPTION("Function not available: MRPT was compiled without OpenCV")
#endif
}
void xRawLogViewerFrame::OnGenGPSTxt(wxCommandEvent& event)
{
WX_START_TRY
wxString caption = wxT("Save as...");
wxString wildcard = wxT("Text files (*.txt)|*.txt|All files (*.*)|*.*");
wxString defaultDir( _U( iniFile->read_string(iniFileSect,"LastDir",".").c_str() ) );
wxString defaultFilename = _U( ( loadedFileName+string("_GPS.txt")).c_str() );
wxFileDialog dialog(this, caption, defaultDir, defaultFilename,wildcard, wxFD_SAVE | wxFD_OVERWRITE_PROMPT );
if (dialog.ShowModal() == wxID_OK)
{
wxString fileName = dialog.GetPath();
string fil( fileName.mbc_str() );
size_t i, M = 0, n = rawlog.size();
map<string, FILE*> lstFiles;
TGeodeticCoords refCoords(0,0,0);
bool ref_valid = false;
// Load configuration block:
CConfigFileMemory memFil;
rawlog.getCommentTextAsConfigFile(memFil);
refCoords.lat = memFil.read_double("GPS_ORIGIN","lat_deg",0);
refCoords.lon = memFil.read_double("GPS_ORIGIN","lon_deg",0);
refCoords.height = memFil.read_double("GPS_ORIGIN","height",0);
ref_valid = !refCoords.isClear();
CPose3D local_ENU;
if (ref_valid)
{
wxMessageBox(_("GPS origin coordinates taken from rawlog configuration block"),_("Export GPS data"));
}
// Ask the user for the reference?
if (!ref_valid && wxYES!=wxMessageBox(_("Do you want to take the GPS reference automatically from the first found entry?"),_("Export GPS data"),wxYES_NO ))
{
wxString s = wxGetTextFromUser(
_("Reference Latitude (degrees):"),
_("GPS reference"),
_("0.0"), this );
if (s.IsEmpty()) return;
if (!s.ToDouble(&refCoords.lat.decimal_value)) { wxMessageBox(_("Invalid number")); return; }
s = wxGetTextFromUser(
_("Reference Longitude (degrees):"),
_("GPS reference"),
_("0.0"), this );
if (s.IsEmpty()) return;
if (!s.ToDouble(&refCoords.lon.decimal_value)) { wxMessageBox(_("Invalid number")); return; }
s = wxGetTextFromUser(
_("Reference Height (meters):"),
_("GPS reference"),
_("0.0"), this );
if (s.IsEmpty()) return;
if (!s.ToDouble(&refCoords.height)) { wxMessageBox(_("Invalid number")); return; }
ref_valid=true;
// Local coordinates reference:
TPose3D _local_ENU;
mrpt::topography::ENU_axes_from_WGS84(
refCoords.lon, refCoords.lat, refCoords.height,
_local_ENU,
true);
local_ENU = _local_ENU;
}
// All gps data:
map< TTimeStamp, map<string,CPoint3D> > lstXYZallGPS;
set< string > lstAllGPSlabels;
for (i=0;i<n;i++)
{
switch ( rawlog.getType(i) )
{
case CRawlog::etSensoryFrame:
{
CSensoryFramePtr sf = rawlog.getAsObservations(i);
size_t ith_obs = 0;
CObservationGPSPtr obs;
do
{
obs = sf->getObservationByClass<CObservationGPS>(ith_obs++);
if (obs)
{
map<string, FILE*>::const_iterator it = lstFiles.find( obs->sensorLabel );
FILE *f_this;
if ( it==lstFiles.end() ) // A new fiile for this sensorlabel??
{
f_this = lstFiles[ obs->sensorLabel ] = os::fopen(
format("%s_%s.txt",
fil.c_str(),
fileNameStripInvalidChars( obs->sensorLabel ).c_str()
).c_str(),"wt");
if (!f_this)
THROW_EXCEPTION("Cannot open output file for write.");
}
else
f_this = it->second;
if (obs->has_GGA_datum) // && obs->has_RMC_datum )
{
TPoint3D p; // Transformed coordinates
// The first gps datum?
if (!ref_valid)
{
ref_valid=true;
refCoords = obs->GGA_datum.getAsStruct<TGeodeticCoords>();
// Local coordinates reference:
TPose3D _local_ENU;
mrpt::topography::ENU_axes_from_WGS84(
refCoords,
_local_ENU,
true);
local_ENU = _local_ENU;
}
// Local XYZ coordinates transform:
mrpt::topography::geodeticToENU_WGS84(
obs->GGA_datum.getAsStruct<TGeodeticCoords>(),
p,
refCoords );
// Geocentric XYZ:
TPoint3D geo;
mrpt::topography::geodeticToGeocentric_WGS84(
obs->GGA_datum.getAsStruct<TGeodeticCoords>(),
geo);
// Save file:
double tim = mrpt::system::timestampTotime_t(obs->timestamp);
/* obs->GGA_datum.UTCTime.hour * 3600 +
obs->GGA_datum.UTCTime.minute * 60 +
obs->GGA_datum.UTCTime.sec;*/
::fprintf(f_this,"%.4f %.16f %.16f %f %u %u %f %f %.16f %.16f %f %i %.4f %.4f %.4f\n",
tim,
DEG2RAD(obs->GGA_datum.latitude_degrees),
DEG2RAD(obs->GGA_datum.longitude_degrees),
obs->GGA_datum.altitude_meters,
obs->GGA_datum.fix_quality,
obs->GGA_datum.satellitesUsed,
obs->RMC_datum.speed_knots,
DEG2RAD(obs->RMC_datum.direction_degrees),
p.x,p.y,p.z,
(int)i, // rawlog index
geo.x, geo.y, geo.z
);
M++;
if (obs->GGA_datum.fix_quality==4)
{
lstXYZallGPS[obs->timestamp][obs->sensorLabel] = CPoint3D(p);
lstAllGPSlabels.insert( obs->sensorLabel );
}
}
}
} while (obs);
}
break;
case CRawlog::etObservation:
{
CObservationPtr o = rawlog.getAsObservation(i);
if (IS_CLASS(o,CObservationGPS))
{
CObservationGPSPtr obs = CObservationGPSPtr(o);
if (obs)
{
map<string, FILE*>::const_iterator it = lstFiles.find( obs->sensorLabel );
FILE *f_this;
if ( it==lstFiles.end() ) // A new fiile for this sensorlabel??
{
std::string temp = format("%s_%s.txt",
fil.c_str(),
fileNameStripInvalidChars( obs->sensorLabel ).c_str()
);
f_this = lstFiles[ obs->sensorLabel ] = os::fopen( temp.c_str(), "wt");
if (!f_this)
THROW_EXCEPTION("Cannot open output file for write.");
// The first line is a description of the columns:
::fprintf(f_this,
"%% "
"%14s " // Time
"%23s %23s %23s " // lat lon alt
"%4s %4s %11s %11s " // fix #sats speed dir
"%23s %23s %23s " // X Y Z local
"%6s " // rawlog index
"%21s %21s %21s " // X Y Z geocentric
"%21s %21s %21s " // X Y Z Cartessian (GPS)
"%21s %21s %21s " // VX VY VZ Cartessian (GPS)
"%21s %21s %21s " // VX VY VZ Cartessian (Local)
"\n"
,
"Time",
"Lat","Lon","Alt",
"fix","#sats", "speed","dir",
"Local X","Local Y","Local Z",
"rawlog ID",
"Geocen X","Geocen Y","Geocen Z",
"GPS X","GPS Y","GPS Z",
"GPS VX","GPS VY","GPS VZ",
"Local VX","Local VY","Local VZ"
);
}
else
f_this = it->second;
if (obs->has_GGA_datum) // && obs->has_RMC_datum )
{
TPoint3D p; // Transformed coordinates
// The first gps datum?
if (!ref_valid)
{
ref_valid=true;
refCoords.lon = obs->GGA_datum.longitude_degrees;
refCoords.lat = obs->GGA_datum.latitude_degrees;
refCoords.height = obs->GGA_datum.altitude_meters;
// Local coordinates reference:
TPose3D _local_ENU;
mrpt::topography::ENU_axes_from_WGS84(
refCoords.lon, refCoords.lat, refCoords.height,
_local_ENU,
true);
local_ENU = _local_ENU;
}
// Local XYZ coordinates transform:
mrpt::topography::geodeticToENU_WGS84(
obs->GGA_datum.getAsStruct<TGeodeticCoords>(),
p,
refCoords);
// Geocentric XYZ:
TPoint3D geo;
mrpt::topography::geodeticToGeocentric_WGS84(
obs->GGA_datum.getAsStruct<TGeodeticCoords>(),
geo );
// Save file:
double tim = mrpt::system::timestampTotime_t(obs->timestamp);
// If available, Cartessian X Y Z, VX VY VZ, as supplied by the GPS itself:
TPoint3D cart_pos(0,0,0), cart_vel(0,0,0);
TPoint3D cart_vel_local(0,0,0);
if (obs->has_PZS_datum && obs->PZS_datum.hasCartesianPosVel)
{
cart_pos.x = obs->PZS_datum.cartesian_x;
cart_pos.y = obs->PZS_datum.cartesian_y;
cart_pos.z = obs->PZS_datum.cartesian_z;
cart_vel.x = obs->PZS_datum.cartesian_vx;
cart_vel.y = obs->PZS_datum.cartesian_vy;
cart_vel.z = obs->PZS_datum.cartesian_vz;
cart_vel_local = TPoint3D( CPoint3D(cart_vel) - local_ENU );
}
::fprintf(f_this,
"%14.4f " // Time
"%23.16f %23.16f %23.6f " // lat lon alt
"%4u %4u %11.6f %11.6f " // fix #sats speed dir
"%23.16f %23.16f %23.16f " // X Y Z local
"%6i " // rawlog index
"%21.16f %21.16f %21.16f " // X Y Z geocentric
"%21.16f %21.16f %21.16f " // X Y Z Cartessian (GPS)
"%21.16f %21.16f %21.16f " // VX VY VZ Cartessian (GPS)
"%21.16f %21.16f %21.16f " // VX VY VZ Cartessian (Local)
"\n",
tim,
DEG2RAD(obs->GGA_datum.latitude_degrees),
DEG2RAD(obs->GGA_datum.longitude_degrees),
obs->GGA_datum.altitude_meters,
obs->GGA_datum.fix_quality,
obs->GGA_datum.satellitesUsed,
obs->RMC_datum.speed_knots,
DEG2RAD(obs->RMC_datum.direction_degrees),
p.x,p.y,p.z,
(int)i, // rawlog index
geo.x, geo.y, geo.z,
cart_pos.x,cart_pos.y,cart_pos.z,
cart_vel.x,cart_vel.y,cart_vel.z,
cart_vel_local.x,cart_vel_local.y,cart_vel_local.z
);
M++;
if (obs->GGA_datum.fix_quality==4)
{
lstXYZallGPS[obs->timestamp][obs->sensorLabel] = CPoint3D(p);
lstAllGPSlabels.insert( obs->sensorLabel );
}
}
}
}
}
break;
default:
break;
}
}
for (map<string, FILE*>::const_iterator it=lstFiles.begin();it!=lstFiles.end();++it)
{
os::fclose(it->second);
}
lstFiles.clear();
// Save the joint file:
// -------------------------
// Remove those entries with not all the GPSs:
for (map< TTimeStamp, map<string,CPoint3D> >::iterator a = lstXYZallGPS.begin();a!=lstXYZallGPS.end(); )
{
if ( a->second.size()!=lstAllGPSlabels.size() )
{
map< TTimeStamp, map<string,CPoint3D> >::iterator b = a;
b++;
lstXYZallGPS.erase(a);
a = b;
}
else ++a;
}
cout << "# of gps entries with all the GPSs:" << lstXYZallGPS.size() << endl;
CMatrixDouble MAT( lstXYZallGPS.size(), 1+3*lstAllGPSlabels.size() );
int nLabels = 0;
for (map< TTimeStamp, map<string,CPoint3D> >::iterator a = lstXYZallGPS.begin();a!=lstXYZallGPS.end();++a, nLabels++ )
{
MAT(nLabels,0) = timestampTotime_t(a->first);
map<string,CPoint3D> &m = a->second;
int k = 0;
for (set< string >::iterator it=lstAllGPSlabels.begin();it!=lstAllGPSlabels.end();++it, k++)
{
MAT(nLabels,1 + 3*k + 0 ) = m[*it].x();
MAT(nLabels,1 + 3*k + 1 ) = m[*it].y();
MAT(nLabels,1 + 3*k + 2 ) = m[*it].z();
}
}
// The name of the file:
string joint_name;
for (set< string >::iterator it=lstAllGPSlabels.begin();it!=lstAllGPSlabels.end();++it)
{
joint_name += *it;
}
MAT.saveToTextFile( format("%s_JOINT_%s.txt",fil.c_str(), joint_name.c_str() ) );
CMatrixDouble MAT_REF(1,3);
MAT_REF(0,0) = refCoords.lon;
MAT_REF(0,1) = refCoords.lat;
MAT_REF(0,2) = refCoords.height;
MAT_REF.saveToTextFile( format("%s_JOINTREF_%s.txt",fil.c_str(), joint_name.c_str() ), MATRIX_FORMAT_FIXED );
wxMessageBox(_U( format("%u entries saved!",(unsigned)M).c_str() ),_("Done"),wxOK,this);
}
WX_END_TRY
}
void xRawLogViewerFrame::OnMenuDistanceBtwGPSs(wxCommandEvent& event)
{
WX_START_TRY
wxMessageBox(_("It will be measured the distance between two GPSs, assuming they are fixed on the vehicle,\n and using only RTK fixed observations."));
if (listOfSensorLabels.empty())
{
wxMessageBox(_("No sensors were found with proper sensor labels. Labels are required for this operation."));
return;
}
// List of labels:
wxArrayString lstLabels;
for (std::map<std::string,TInfoPerSensorLabel>::iterator i=listOfSensorLabels.begin();i!=listOfSensorLabels.end();++i)
lstLabels.Add( _U( i->first.c_str() ) );
wxString ret = wxGetSingleChoice(
_("Choose the first GPS:"),
_("Sensor Labels"),
lstLabels,
this );
if (ret.IsEmpty()) return;
string gps1 = string(ret.mb_str());
ret = wxGetSingleChoice(
_("Choose the second GPS:"),
_("Sensor Labels"),
lstLabels,
this );
if (ret.IsEmpty()) return;
string gps2 = string(ret.mb_str());
size_t i, n = rawlog.size();
// Look for the 2 observations:
CObservationGPSPtr last_GPS1, last_GPS2;
vector_double dists;
TGeodeticCoords refCoords(0,0,0);
// Load configuration block:
CConfigFileMemory memFil;
rawlog.getCommentTextAsConfigFile(memFil);
refCoords.lat = memFil.read_double("GPS_ORIGIN","lat_deg",0);
refCoords.lon = memFil.read_double("GPS_ORIGIN","lon_deg",0);
refCoords.height = memFil.read_double("GPS_ORIGIN","height",0);
bool ref_valid = !refCoords.isClear();
for (i=0;i<n;i++)
{
switch ( rawlog.getType(i) )
{
case CRawlog::etSensoryFrame:
{
CSensoryFramePtr sf = rawlog.getAsObservations(i);
if (!ref_valid)
{
CObservationGPSPtr o = sf->getObservationByClass<CObservationGPS>();
if (o && o->has_GGA_datum)
{
refCoords = o->GGA_datum.getAsStruct<TGeodeticCoords>();
ref_valid = true;
}
}
CObservationPtr o1 = sf->getObservationBySensorLabel(gps1);
CObservationPtr o2 = sf->getObservationBySensorLabel(gps2);
if (o1)
{
ASSERT_(o1->GetRuntimeClass()==CLASS_ID(CObservationGPS));
CObservationGPSPtr obs = CObservationGPSPtr(o1);
if (obs->has_GGA_datum && obs->GGA_datum.fix_quality==4)
last_GPS1 = obs;
}
if (o2)
{
ASSERT_(o2->GetRuntimeClass()==CLASS_ID(CObservationGPS));
CObservationGPSPtr obs = CObservationGPSPtr(o2);
if (obs->has_GGA_datum && obs->GGA_datum.fix_quality==4)
last_GPS2 = obs;
}
}
break;
case CRawlog::etObservation:
{
CObservationPtr o = rawlog.getAsObservation(i);
if (!ref_valid && IS_CLASS(o,CObservationGPS))
{
CObservationGPSPtr ob = CObservationGPSPtr(o);
if (ob && ob->has_GGA_datum)
{
refCoords = ob->GGA_datum.getAsStruct<TGeodeticCoords>();
ref_valid = true;
}
}
if (o->sensorLabel == gps1)
{
ASSERT_(IS_CLASS(o,CObservationGPS));
CObservationGPSPtr obs = CObservationGPSPtr(o);
if (obs->has_GGA_datum && obs->GGA_datum.fix_quality==4)
last_GPS1 = obs;
}
if (o->sensorLabel == gps2)
{
ASSERT_(IS_CLASS(o,CObservationGPS));
CObservationGPSPtr obs = CObservationGPSPtr(o);
if (obs->has_GGA_datum && obs->GGA_datum.fix_quality==4)
last_GPS2 = obs;
}
}
break;
default:
break;
} // end switch type
// Now check if we have 2 gps with the same time stamp:
if (last_GPS1 && last_GPS2)
{
if (last_GPS1->GGA_datum.UTCTime == last_GPS2->GGA_datum.UTCTime)
{
// Compute distance:
TPoint3D p1;
mrpt::topography::geodeticToENU_WGS84(
last_GPS1->GGA_datum.getAsStruct<TGeodeticCoords>(),
p1,
refCoords);
TPoint3D p2;
mrpt::topography::geodeticToENU_WGS84(
last_GPS2->GGA_datum.getAsStruct<TGeodeticCoords>(),
p2,
refCoords);
// Fix offset:
p1.x += memFil.read_double( string("OFFSET_")+last_GPS1->sensorLabel, "x", 0 );
p1.y += memFil.read_double( string("OFFSET_")+last_GPS1->sensorLabel, "y", 0 );
p1.z += memFil.read_double( string("OFFSET_")+last_GPS1->sensorLabel, "z", 0 );
p2.x += memFil.read_double( string("OFFSET_")+last_GPS2->sensorLabel, "x", 0 );
p2.y += memFil.read_double( string("OFFSET_")+last_GPS2->sensorLabel, "y", 0 );
p2.z += memFil.read_double( string("OFFSET_")+last_GPS2->sensorLabel, "z", 0 );
double d = mrpt::math::distance(p1,p2);
dists.push_back(d);
last_GPS1.clear_unique();
last_GPS2.clear_unique();
}
}
} // end for
if (dists.empty())
{
wxMessageBox(_("No valid GPS observations were found."),_("Done"),wxOK,this);
}
else
{
double d_mean,d_std;
mrpt::math::meanAndStd(dists,d_mean,d_std);
wxMessageBox(_U(
format("The distance between GPS sensors is %.04fm, with\n a sigma=%.04fm, average from %u entries.",
d_mean,d_std, (unsigned)dists.size()).c_str() ),_("Done"),wxOK,this);
}
WX_END_TRY
}
