void GameObject::init(){
std::ostringstream texturePath;
texturePath << "img/" << getTextureName();
std::cout << texturePath.str() << std::endl;
gTexture = lTextureFactory->fetchTexture(texturePath.str(), true);
width = gTexture->getWidth();
height = gTexture->getHeight();
initRectangle(clip, width, height);
initRectangle(dyingClipRect, width, height);
}
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;
}
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);
}
}
void GameObject::rebirth(){
dead = false;
dying = false;
initRectangle(dyingClipRect, width, height);
}
