bool RectangleHandler::initFeature(const std::string& sensor,
const Eigen::VectorXd& z, ROAMestimation::PoseVertexWrapper_Ptr pv,
long int id) {
const Eigen::VectorXd &cur_frame = pv->getEstimate();
Eigen::VectorXd dim0(2), f0(7);
initRectangle(cur_frame, _lambda, z, dim0, f0);
_filter->addSensor(sensor, RectangularObject, false, false);
_filter->shareSensorFrame("Camera", sensor);
_filter->shareParameter("Camera_CM", sensor + "_CM");
_filter->addConstantParameter(Euclidean2D, sensor + "_Dim", 0.0, dim0, false);
_filter->addConstantParameter(SE3, sensor + "_F", 0.0, f0, false);
//add to current track list
RectangleDescriptor &d = _features[id];
//_filter->setRobustKernel(sensor, true, 3.0);
cerr << "[RectangleHandler] New rectangle, id " << id << endl;
return true;
}
/**
* Loads the mapping between index -> set of detector IDs
*
* If "detector_index", "detector_count" and "detector_list" are all present,
* use these to get the mapping, otherwise spectrum number = detector ID
* (one-to-one)
*
* The spectrum spectrum_index[i] maps to detector_count[i] detectors, whose
* detector IDs are in detector_list starting at the index detector_index[i]
*
* @returns :: map of index -> detector IDs
* @throws std::runtime_error if fails to read data from file
*/
std::map<int, std::set<int>>
LoadMuonNexus2::loadDetectorMapping(const Mantid::NeXus::NXInt &spectrumIndex) {
std::map<int, std::set<int>> mapping;
const int nSpectra = spectrumIndex.dim0();
// Find and open the data group
NXRoot root(getPropertyValue("Filename"));
NXEntry entry = root.openEntry(m_entry_name);
const std::string detectorName = [&entry]() {
// Only the first NXdata found
for (auto &group : entry.groups()) {
std::string className = group.nxclass;
if (className == "NXdata") {
return group.nxname;
}
}
throw std::runtime_error("No NXdata found in file");
}();
NXData dataGroup = entry.openNXData(detectorName);
// Usually for muon data, detector id = spectrum number
// If not, the optional groups "detector_index", "detector_list" and
// "detector_count" will be present to map one to the other
const bool hasDetectorMapping = dataGroup.containsDataSet("detector_index") &&
dataGroup.containsDataSet("detector_list") &&
dataGroup.containsDataSet("detector_count");
if (hasDetectorMapping) {
// Read detector IDs
try {
const auto detIndex = dataGroup.openNXInt("detector_index");
const auto detCount = dataGroup.openNXInt("detector_count");
const auto detList = dataGroup.openNXInt("detector_list");
const int nSpectra = detIndex.dim0();
for (int i = 0; i < nSpectra; ++i) {
const int start = detIndex[i];
const int nDetectors = detCount[i];
std::set<int> detIDs;
for (int jDet = 0; jDet < nDetectors; ++jDet) {
detIDs.insert(detList[start + jDet]);
}
mapping[i] = detIDs;
}
} catch (const ::NeXus::Exception &err) {
// Throw a more user-friendly message
std::ostringstream message;
message << "Failed to read detector mapping: " << err.what();
throw std::runtime_error(message.str());
}
} else {
for (int i = 0; i < nSpectra; ++i) {
mapping[i] = std::set<int>{spectrumIndex[i]};
}
}
return mapping;
}
bool AnchoredRectangleHandlerBootstrap::initFeature(const std::string& sensor,
const Eigen::VectorXd& z, ROAMestimation::PoseVertexWrapper_Ptr pv,
long int id) {
if (_bootstrap) {
const Eigen::VectorXd &anchor_frame = pv->getEstimate();
Eigen::VectorXd dim0(2), f0(7), foq0(4), fohp0(3);
initRectangle(anchor_frame, _lambda, z, dim0, fohp0, foq0);
_filter->addSensor(sensor, AnchoredRectangularObject, false, false);
_filter->shareSensorFrame("Camera", sensor);
_filter->shareParameter("Camera_CM", sensor + "_CM");
_filter->addConstantParameter(Euclidean2D, sensor + "_Dim", 0.0, dim0,
false);
_filter->poseVertexAsParameter(pv, sensor + "_F");
_filter->addConstantParameter(Quaternion, sensor + "_FOq",
pv->getTimestamp(), foq0, false);
_filter->addConstantParameter(Euclidean3D, sensor + "_FOhp",
pv->getTimestamp(), fohp0, false);
// add the sensor for the first edge only
std::string sensor_first = sensor + "_first";
_filter->addSensor(sensor_first, AnchoredRectangularObjectFirst, false,
false);
_filter->shareSensorFrame("Camera", sensor_first);
_filter->shareParameter("Camera_CM", sensor_first + "_CM");
_filter->shareParameter(sensor + "_Dim", sensor_first + "_Dim");
_filter->shareParameter(sensor + "_FOq", sensor_first + "_FOq");
_filter->shareParameter(sensor + "_FOhp", sensor_first + "_FOhp");
/* prior on homogeneous point
const double sigma_pixel = 1;
Eigen::MatrixXd prior_cov(3, 3);
prior_cov << sigma_pixel / pow(_fx, 2), 0, 0, 0, sigma_pixel / pow(_fy, 2), 0, 0, 0, pow(
_lambda / 3.0, 2);
_filter->addPriorOnConstantParameter(Euclidean3DPrior, sensor + "_FOhp",
fohp0, prior_cov);
//*/
//add to current track list
ObjectTrackDescriptor &d = _objects[id];
d.anchorFrame = pv;
d.isInitialized = false;
d.initStrategy = new ObjectSufficientZChange(2.0, d.zHistory, _K.data());
//_filter->setRobustKernel(sensor, true, 3.0);
cerr << "[AnchoredRectangleHandler] New rectangle, id " << id << endl;
return true;
return true;
} else {
return AnchoredRectangleHandler::initFeature(sensor, z, pv, id);
}
}
