void jacTransform(MXD& J, const mtInput& input) const{
J.setZero();
const int& camID = input.CfP(ID_).camID_;
if(camID != outputCamID_){
input.updateMultiCameraExtrinsics(mpMultiCamera_);
const QPD qDC = input.qCM(outputCamID_)*input.qCM(camID).inverted(); // TODO: avoid double computation
const V3D CrCD = input.qCM(camID).rotate(V3D(input.MrMC(outputCamID_)-input.MrMC(camID)));
const V3D CrCP = input.dep(ID_).getDistance()*input.CfP(ID_).get_nor().getVec();
const V3D DrDP = qDC.rotate(V3D(CrCP-CrCD));
const double d_out = DrDP.norm();
const LWF::NormalVectorElement nor_out(DrDP); // TODO: test if Jacobian works, this new setting of vector is dangerous
const Eigen::Matrix<double,1,3> J_d_DrDP = nor_out.getVec().transpose();
const Eigen::Matrix<double,2,3> J_nor_DrDP = nor_out.getM().transpose()/d_out;
const Eigen::Matrix<double,3,3> J_DrDP_qDC = gSM(DrDP);
const Eigen::Matrix<double,3,3> J_DrDP_CrCP = MPD(qDC).matrix();
const Eigen::Matrix<double,3,3> J_DrDP_CrCD = -MPD(qDC).matrix();
const Eigen::Matrix<double,3,3> J_qDC_qDB = Eigen::Matrix3d::Identity();
const Eigen::Matrix<double,3,3> J_qDC_qCB = -MPD(qDC).matrix();
const Eigen::Matrix<double,3,3> J_CrCD_qCB = gSM(CrCD);
const Eigen::Matrix<double,3,3> J_CrCD_BrBC = -MPD(input.qCM(camID)).matrix();
const Eigen::Matrix<double,3,3> J_CrCD_BrBD = MPD(input.qCM(camID)).matrix();
const Eigen::Matrix<double,3,1> J_CrCP_d = input.CfP(ID_).get_nor().getVec()*input.dep(ID_).getDistanceDerivative();
const Eigen::Matrix<double,3,2> J_CrCP_nor = input.dep(ID_).getDistance()*input.CfP(ID_).get_nor().getM();
J.template block<2,2>(mtOutput::template getId<mtOutput::_fea>(),mtInput::template getId<mtInput::_fea>(ID_)) = J_nor_DrDP*J_DrDP_CrCP*J_CrCP_nor;
J.template block<2,1>(mtOutput::template getId<mtOutput::_fea>(),mtInput::template getId<mtInput::_fea>(ID_)+2) = J_nor_DrDP*J_DrDP_CrCP*J_CrCP_d;
if(!ignoreDistanceOutput_){
J.template block<1,2>(mtOutput::template getId<mtOutput::_fea>()+2,mtInput::template getId<mtInput::_fea>(ID_)) = J_d_DrDP*J_DrDP_CrCP*J_CrCP_nor;
J.template block<1,1>(mtOutput::template getId<mtOutput::_fea>()+2,mtInput::template getId<mtInput::_fea>(ID_)+2) = J_d_DrDP*J_DrDP_CrCP*J_CrCP_d;
}
if(input.aux().doVECalibration_ && camID != outputCamID_){
J.template block<2,3>(mtOutput::template getId<mtOutput::_fea>(),mtInput::template getId<mtInput::_vea>(camID)) = J_nor_DrDP*(J_DrDP_qDC*J_qDC_qCB+J_DrDP_CrCD*J_CrCD_qCB);
J.template block<2,3>(mtOutput::template getId<mtOutput::_fea>(),mtInput::template getId<mtInput::_vea>(outputCamID_)) = J_nor_DrDP*J_DrDP_qDC*J_qDC_qDB;
J.template block<2,3>(mtOutput::template getId<mtOutput::_fea>(),mtInput::template getId<mtInput::_vep>(camID)) = J_nor_DrDP*J_DrDP_CrCD*J_CrCD_BrBC;
J.template block<2,3>(mtOutput::template getId<mtOutput::_fea>(),mtInput::template getId<mtInput::_vep>(outputCamID_)) = J_nor_DrDP*J_DrDP_CrCD*J_CrCD_BrBD;
if(!ignoreDistanceOutput_){
J.template block<1,3>(mtOutput::template getId<mtOutput::_fea>()+2,mtInput::template getId<mtInput::_vea>(camID)) = J_d_DrDP*(J_DrDP_qDC*J_qDC_qCB+J_DrDP_CrCD*J_CrCD_qCB);
J.template block<1,3>(mtOutput::template getId<mtOutput::_fea>()+2,mtInput::template getId<mtInput::_vea>(outputCamID_)) = J_d_DrDP*J_DrDP_qDC*J_qDC_qDB;
J.template block<1,3>(mtOutput::template getId<mtOutput::_fea>()+2,mtInput::template getId<mtInput::_vep>(camID)) = J_d_DrDP*J_DrDP_CrCD*J_CrCD_BrBC;
J.template block<1,3>(mtOutput::template getId<mtOutput::_fea>()+2,mtInput::template getId<mtInput::_vep>(outputCamID_)) = J_d_DrDP*J_DrDP_CrCD*J_CrCD_BrBD;
}
}
} else {
J.template block<2,2>(mtOutput::template getId<mtOutput::_fea>(),mtInput::template getId<mtInput::_fea>(ID_)) = Eigen::Matrix2d::Identity();
J.template block<1,1>(mtOutput::template getId<mtOutput::_fea>()+2,mtInput::template getId<mtInput::_fea>(ID_)+2) = Eigen::Matrix<double,1,1>::Identity();
}
}
commandHandler::commandHandler(thread_data * my_data)
{
std::unique_ptr <command> test(new commandTEST("test") );
commandMap.insert( std::make_pair(test->getCommandName(),std::move( test )) );
std::unique_ptr <command> exit(new commandEXIT("exit"));
commandMap.insert( std::make_pair(exit->getCommandName(),std::move( exit )) );
std::unique_ptr <command> MPD(new command_mpd("MPD"));
commandMap.insert(std::make_pair(MPD->getCommandName(), std::move (MPD)));
std::unique_ptr <command> RS232 (new commandRS232("RS232"));
commandMap.insert(std::make_pair(RS232->getCommandName(), std::move(RS232)));
std::unique_ptr <command> uptime (new command_UPTIME("uptime"));
commandMap.insert(std::make_pair(uptime->getCommandName(), std::move(uptime)));
std::unique_ptr <command> big (new command_big("big"));
commandMap.insert(std::make_pair(big->getCommandName(), std::move(big)));
std::unique_ptr <command> clock (new command_clock("clock"));
commandMap.insert(std::make_pair(clock->getCommandName(), std::move(clock)));
std::unique_ptr <command> cmd (new command_cmd("cmd"));
commandMap.insert(std::make_pair(cmd->getCommandName(), std::move(cmd)));
std::unique_ptr <command> hello (new command_hello("hello"));
commandMap.insert(std::make_pair(hello->getCommandName(), std::move(hello)));
std::unique_ptr <command> help (new command_help("help"));
commandMap.insert(std::make_pair(help->getCommandName(), std::move(help)));
std::unique_ptr <command> ip (new command_ip("ip"));
commandMap.insert(std::make_pair(ip->getCommandName(), std::move(ip)));
std::unique_ptr <command> ok (new command_ok("ok"));
commandMap.insert(std::make_pair(ok->getCommandName(), std::move(ok)));
std::unique_ptr <command> show (new command_show("show"));
commandMap.insert(std::make_pair(show->getCommandName(), std::move(show)));
std::unique_ptr <command> sleep (new command_sleep("sleep"));
commandMap.insert(std::make_pair(sleep->getCommandName(), std::move(sleep)));
std::unique_ptr <command> put (new command_put("put"));
commandMap.insert(std::make_pair(put->getCommandName(), std::move(put)));
std::unique_ptr <command> event (new command_event("event"));
commandMap.insert(std::make_pair(event->getCommandName(), std::move(event)));
this->my_data = my_data;
this->my_data->commandMapPtr = &commandMap;
}
void evalTransform(mtOutput& output, const mtInput& input) const{
input.updateMultiCameraExtrinsics(mpMultiCamera_);
mpMultiCamera_->transformFeature(outputCamID_,input.CfP(ID_),input.dep(ID_),output.c(),output.d());
if(input.CfP(ID_).trackWarping_ && input.CfP(ID_).com_warp_nor()){
const int& camID = input.CfP(ID_).camID_;
const QPD qDC = input.qCM(outputCamID_)*input.qCM(camID).inverted(); // TODO: avoid double computation
const V3D CrCD = input.qCM(camID).rotate(V3D(input.MrMC(outputCamID_)-input.MrMC(camID)));
const V3D CrCP = input.dep(ID_).getDistance()*input.CfP(ID_).get_nor().getVec();
const V3D DrDP = qDC.rotate(V3D(CrCP-CrCD));
const double d_out = DrDP.norm();
const LWF::NormalVectorElement nor_out(DrDP);
const Eigen::Matrix<double,2,3> J_nor_DrDP = nor_out.getM().transpose()/d_out;
const Eigen::Matrix<double,3,3> J_DrDP_CrCP = MPD(qDC).matrix();
const Eigen::Matrix<double,3,2> J_CrCP_nor = input.dep(ID_).getDistance()*input.CfP(ID_).get_nor().getM();
output.c().set_warp_nor(J_nor_DrDP*J_DrDP_CrCP*J_CrCP_nor*input.CfP(ID_).get_warp_nor());
}
}
